Merge branch 'blender2.7'
[blender.git] / intern / rigidbody / rb_bullet_api.cpp
1 /*
2  * This program is free software; you can redistribute it and/or
3  * modify it under the terms of the GNU General Public License
4  * as published by the Free Software Foundation; either version 2
5  * of the License, or (at your option) any later version.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
10  * GNU General Public License for more details.
11  *
12  * You should have received a copy of the GNU General Public License
13  * along with this program; if not, write to the Free Software Foundation,
14  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
15  *
16  * The Original Code is Copyright (C) 2013 Blender Foundation
17  * All rights reserved.
18  */
19
20 /** \file
21  * \ingroup RigidBody
22  * \brief Rigid Body API implementation for Bullet
23  */
24
25 /*
26 Bullet Continuous Collision Detection and Physics Library
27 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
28
29 This software is provided 'as-is', without any express or implied warranty.
30 In no event will the authors be held liable for any damages arising from the use of this software.
31 Permission is granted to anyone to use this software for any purpose, 
32 including commercial applications, and to alter it and redistribute it freely, 
33 subject to the following restrictions:
34
35 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
36 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
37 3. This notice may not be removed or altered from any source distribution.
38 */
39
40 /* This file defines the "RigidBody interface" for the 
41  * Bullet Physics Engine. This API is designed to be used
42  * from C-code in Blender as part of the Rigid Body simulation
43  * system.
44  *
45  * It is based on the Bullet C-API, but is heavily modified to 
46  * give access to more data types and to offer a nicer interface.
47  *
48  * -- Joshua Leung, June 2010
49  */
50
51 #include <stdio.h>
52 #include <errno.h>
53
54 #include "RBI_api.h"
55
56 #include "btBulletDynamicsCommon.h"
57
58 #include "LinearMath/btVector3.h"
59 #include "LinearMath/btScalar.h"
60 #include "LinearMath/btMatrix3x3.h"
61 #include "LinearMath/btTransform.h"
62 #include "LinearMath/btConvexHullComputer.h"
63
64 #include "BulletCollision/Gimpact/btGImpactShape.h"
65 #include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
66 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
67
68 struct rbDynamicsWorld {
69   btDiscreteDynamicsWorld *dynamicsWorld;
70   btDefaultCollisionConfiguration *collisionConfiguration;
71   btDispatcher *dispatcher;
72   btBroadphaseInterface *pairCache;
73   btConstraintSolver *constraintSolver;
74   btOverlapFilterCallback *filterCallback;
75 };
76 struct rbRigidBody {
77   btRigidBody *body;
78   int col_groups;
79 };
80
81 struct rbVert {
82   float x, y, z;
83 };
84 struct rbTri {
85   int v0, v1, v2;
86 };
87
88 struct rbMeshData {
89   btTriangleIndexVertexArray *index_array;
90   rbVert *vertices;
91   rbTri *triangles;
92   int num_vertices;
93   int num_triangles;
94 };
95
96 struct rbCollisionShape {
97   btCollisionShape *cshape;
98   rbMeshData *mesh;
99 };
100
101 struct rbFilterCallback : public btOverlapFilterCallback {
102   virtual bool needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const
103   {
104     rbRigidBody *rb0 = (rbRigidBody *)((btRigidBody *)proxy0->m_clientObject)->getUserPointer();
105     rbRigidBody *rb1 = (rbRigidBody *)((btRigidBody *)proxy1->m_clientObject)->getUserPointer();
106
107     bool collides;
108     collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
109     collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
110     collides = collides && (rb0->col_groups & rb1->col_groups);
111
112     return collides;
113   }
114 };
115
116 static inline void copy_v3_btvec3(float vec[3], const btVector3 &btvec)
117 {
118   vec[0] = (float)btvec[0];
119   vec[1] = (float)btvec[1];
120   vec[2] = (float)btvec[2];
121 }
122 static inline void copy_quat_btquat(float quat[4], const btQuaternion &btquat)
123 {
124   quat[0] = btquat.getW();
125   quat[1] = btquat.getX();
126   quat[2] = btquat.getY();
127   quat[3] = btquat.getZ();
128 }
129
130 /* ********************************** */
131 /* Dynamics World Methods */
132
133 /* Setup ---------------------------- */
134
135 rbDynamicsWorld *RB_dworld_new(const float gravity[3])
136 {
137   rbDynamicsWorld *world = new rbDynamicsWorld;
138
139   /* collision detection/handling */
140   world->collisionConfiguration = new btDefaultCollisionConfiguration();
141
142   world->dispatcher = new btCollisionDispatcher(world->collisionConfiguration);
143   btGImpactCollisionAlgorithm::registerAlgorithm((btCollisionDispatcher *)world->dispatcher);
144
145   world->pairCache = new btDbvtBroadphase();
146
147   world->filterCallback = new rbFilterCallback();
148   world->pairCache->getOverlappingPairCache()->setOverlapFilterCallback(world->filterCallback);
149
150   /* constraint solving */
151   world->constraintSolver = new btSequentialImpulseConstraintSolver();
152
153   /* world */
154   world->dynamicsWorld = new btDiscreteDynamicsWorld(
155       world->dispatcher, world->pairCache, world->constraintSolver, world->collisionConfiguration);
156
157   RB_dworld_set_gravity(world, gravity);
158
159   return world;
160 }
161
162 void RB_dworld_delete(rbDynamicsWorld *world)
163 {
164   /* bullet doesn't like if we free these in a different order */
165   delete world->dynamicsWorld;
166   delete world->constraintSolver;
167   delete world->pairCache;
168   delete world->dispatcher;
169   delete world->collisionConfiguration;
170   delete world->filterCallback;
171   delete world;
172 }
173
174 /* Settings ------------------------- */
175
176 /* Gravity */
177 void RB_dworld_get_gravity(rbDynamicsWorld *world, float g_out[3])
178 {
179   copy_v3_btvec3(g_out, world->dynamicsWorld->getGravity());
180 }
181
182 void RB_dworld_set_gravity(rbDynamicsWorld *world, const float g_in[3])
183 {
184   world->dynamicsWorld->setGravity(btVector3(g_in[0], g_in[1], g_in[2]));
185 }
186
187 /* Constraint Solver */
188 void RB_dworld_set_solver_iterations(rbDynamicsWorld *world, int num_solver_iterations)
189 {
190   btContactSolverInfo &info = world->dynamicsWorld->getSolverInfo();
191
192   info.m_numIterations = num_solver_iterations;
193 }
194
195 /* Split Impulse */
196 void RB_dworld_set_split_impulse(rbDynamicsWorld *world, int split_impulse)
197 {
198   btContactSolverInfo &info = world->dynamicsWorld->getSolverInfo();
199
200   info.m_splitImpulse = split_impulse;
201 }
202
203 /* Simulation ----------------------- */
204
205 void RB_dworld_step_simulation(rbDynamicsWorld *world,
206                                float timeStep,
207                                int maxSubSteps,
208                                float timeSubStep)
209 {
210   world->dynamicsWorld->stepSimulation(timeStep, maxSubSteps, timeSubStep);
211 }
212
213 /* Export -------------------------- */
214
215 /**
216  * Exports entire dynamics world to Bullet's "*.bullet" binary format
217  * which is similar to Blender's SDNA system.
218  *
219  * \param world Dynamics world to write to file
220  * \param filename Assumed to be a valid filename, with .bullet extension
221  */
222 void RB_dworld_export(rbDynamicsWorld *world, const char *filename)
223 {
224   //create a large enough buffer. There is no method to pre-calculate the buffer size yet.
225   int maxSerializeBufferSize = 1024 * 1024 * 5;
226
227   btDefaultSerializer *serializer = new btDefaultSerializer(maxSerializeBufferSize);
228   world->dynamicsWorld->serialize(serializer);
229
230   FILE *file = fopen(filename, "wb");
231   if (file) {
232     fwrite(serializer->getBufferPointer(), serializer->getCurrentBufferSize(), 1, file);
233     fclose(file);
234   }
235   else {
236     fprintf(stderr, "RB_dworld_export: %s\n", strerror(errno));
237   }
238 }
239
240 /* ********************************** */
241 /* Rigid Body Methods */
242
243 /* Setup ---------------------------- */
244
245 void RB_dworld_add_body(rbDynamicsWorld *world, rbRigidBody *object, int col_groups)
246 {
247   btRigidBody *body = object->body;
248   object->col_groups = col_groups;
249
250   world->dynamicsWorld->addRigidBody(body);
251 }
252
253 void RB_dworld_remove_body(rbDynamicsWorld *world, rbRigidBody *object)
254 {
255   btRigidBody *body = object->body;
256
257   world->dynamicsWorld->removeRigidBody(body);
258 }
259
260 /* Collision detection */
261
262 void RB_world_convex_sweep_test(rbDynamicsWorld *world,
263                                 rbRigidBody *object,
264                                 const float loc_start[3],
265                                 const float loc_end[3],
266                                 float v_location[3],
267                                 float v_hitpoint[3],
268                                 float v_normal[3],
269                                 int *r_hit)
270 {
271   btRigidBody *body = object->body;
272   btCollisionShape *collisionShape = body->getCollisionShape();
273   /* only convex shapes are supported, but user can specify a non convex shape */
274   if (collisionShape->isConvex()) {
275     btCollisionWorld::ClosestConvexResultCallback result(
276         btVector3(loc_start[0], loc_start[1], loc_start[2]),
277         btVector3(loc_end[0], loc_end[1], loc_end[2]));
278
279     btQuaternion obRot = body->getWorldTransform().getRotation();
280
281     btTransform rayFromTrans;
282     rayFromTrans.setIdentity();
283     rayFromTrans.setRotation(obRot);
284     rayFromTrans.setOrigin(btVector3(loc_start[0], loc_start[1], loc_start[2]));
285
286     btTransform rayToTrans;
287     rayToTrans.setIdentity();
288     rayToTrans.setRotation(obRot);
289     rayToTrans.setOrigin(btVector3(loc_end[0], loc_end[1], loc_end[2]));
290
291     world->dynamicsWorld->convexSweepTest(
292         (btConvexShape *)collisionShape, rayFromTrans, rayToTrans, result, 0);
293
294     if (result.hasHit()) {
295       *r_hit = 1;
296
297       v_location[0] = result.m_convexFromWorld[0] +
298                       (result.m_convexToWorld[0] - result.m_convexFromWorld[0]) *
299                           result.m_closestHitFraction;
300       v_location[1] = result.m_convexFromWorld[1] +
301                       (result.m_convexToWorld[1] - result.m_convexFromWorld[1]) *
302                           result.m_closestHitFraction;
303       v_location[2] = result.m_convexFromWorld[2] +
304                       (result.m_convexToWorld[2] - result.m_convexFromWorld[2]) *
305                           result.m_closestHitFraction;
306
307       v_hitpoint[0] = result.m_hitPointWorld[0];
308       v_hitpoint[1] = result.m_hitPointWorld[1];
309       v_hitpoint[2] = result.m_hitPointWorld[2];
310
311       v_normal[0] = result.m_hitNormalWorld[0];
312       v_normal[1] = result.m_hitNormalWorld[1];
313       v_normal[2] = result.m_hitNormalWorld[2];
314     }
315     else {
316       *r_hit = 0;
317     }
318   }
319   else {
320     /* we need to return a value if user passes non convex body, to report */
321     *r_hit = -2;
322   }
323 }
324
325 /* ............ */
326
327 rbRigidBody *RB_body_new(rbCollisionShape *shape, const float loc[3], const float rot[4])
328 {
329   rbRigidBody *object = new rbRigidBody;
330   /* current transform */
331   btTransform trans;
332   trans.setOrigin(btVector3(loc[0], loc[1], loc[2]));
333   trans.setRotation(btQuaternion(rot[1], rot[2], rot[3], rot[0]));
334
335   /* create motionstate, which is necessary for interpolation (includes reverse playback) */
336   btDefaultMotionState *motionState = new btDefaultMotionState(trans);
337
338   /* make rigidbody */
339   btRigidBody::btRigidBodyConstructionInfo rbInfo(1.0f, motionState, shape->cshape);
340
341   object->body = new btRigidBody(rbInfo);
342
343   object->body->setUserPointer(object);
344
345   return object;
346 }
347
348 void RB_body_delete(rbRigidBody *object)
349 {
350   btRigidBody *body = object->body;
351
352   /* motion state */
353   btMotionState *ms = body->getMotionState();
354   if (ms)
355     delete ms;
356
357   /* collision shape is done elsewhere... */
358
359   /* body itself */
360
361   /* manually remove constraint refs of the rigid body, normally this happens when removing constraints from the world
362    * but since we delete everything when the world is rebult, we need to do it manually here */
363   for (int i = body->getNumConstraintRefs() - 1; i >= 0; i--) {
364     btTypedConstraint *con = body->getConstraintRef(i);
365     body->removeConstraintRef(con);
366   }
367
368   delete body;
369   delete object;
370 }
371
372 /* Settings ------------------------- */
373
374 void RB_body_set_collision_shape(rbRigidBody *object, rbCollisionShape *shape)
375 {
376   btRigidBody *body = object->body;
377
378   /* set new collision shape */
379   body->setCollisionShape(shape->cshape);
380
381   /* recalculate inertia, since that depends on the collision shape... */
382   RB_body_set_mass(object, RB_body_get_mass(object));
383 }
384
385 /* ............ */
386
387 float RB_body_get_mass(rbRigidBody *object)
388 {
389   btRigidBody *body = object->body;
390
391   /* there isn't really a mass setting, but rather 'inverse mass'  
392    * which we convert back to mass by taking the reciprocal again 
393    */
394   float value = (float)body->getInvMass();
395
396   if (value)
397     value = 1.0f / value;
398
399   return value;
400 }
401
402 void RB_body_set_mass(rbRigidBody *object, float value)
403 {
404   btRigidBody *body = object->body;
405   btVector3 localInertia(0, 0, 0);
406
407   /* calculate new inertia if non-zero mass */
408   if (value) {
409     btCollisionShape *shape = body->getCollisionShape();
410     shape->calculateLocalInertia(value, localInertia);
411   }
412
413   body->setMassProps(value, localInertia);
414   body->updateInertiaTensor();
415 }
416
417 float RB_body_get_friction(rbRigidBody *object)
418 {
419   btRigidBody *body = object->body;
420   return body->getFriction();
421 }
422
423 void RB_body_set_friction(rbRigidBody *object, float value)
424 {
425   btRigidBody *body = object->body;
426   body->setFriction(value);
427 }
428
429 float RB_body_get_restitution(rbRigidBody *object)
430 {
431   btRigidBody *body = object->body;
432   return body->getRestitution();
433 }
434
435 void RB_body_set_restitution(rbRigidBody *object, float value)
436 {
437   btRigidBody *body = object->body;
438   body->setRestitution(value);
439 }
440
441 float RB_body_get_linear_damping(rbRigidBody *object)
442 {
443   btRigidBody *body = object->body;
444   return body->getLinearDamping();
445 }
446
447 void RB_body_set_linear_damping(rbRigidBody *object, float value)
448 {
449   RB_body_set_damping(object, value, RB_body_get_linear_damping(object));
450 }
451
452 float RB_body_get_angular_damping(rbRigidBody *object)
453 {
454   btRigidBody *body = object->body;
455   return body->getAngularDamping();
456 }
457
458 void RB_body_set_angular_damping(rbRigidBody *object, float value)
459 {
460   RB_body_set_damping(object, RB_body_get_linear_damping(object), value);
461 }
462
463 void RB_body_set_damping(rbRigidBody *object, float linear, float angular)
464 {
465   btRigidBody *body = object->body;
466   body->setDamping(linear, angular);
467 }
468
469 float RB_body_get_linear_sleep_thresh(rbRigidBody *object)
470 {
471   btRigidBody *body = object->body;
472   return body->getLinearSleepingThreshold();
473 }
474
475 void RB_body_set_linear_sleep_thresh(rbRigidBody *object, float value)
476 {
477   RB_body_set_sleep_thresh(object, value, RB_body_get_angular_sleep_thresh(object));
478 }
479
480 float RB_body_get_angular_sleep_thresh(rbRigidBody *object)
481 {
482   btRigidBody *body = object->body;
483   return body->getAngularSleepingThreshold();
484 }
485
486 void RB_body_set_angular_sleep_thresh(rbRigidBody *object, float value)
487 {
488   RB_body_set_sleep_thresh(object, RB_body_get_linear_sleep_thresh(object), value);
489 }
490
491 void RB_body_set_sleep_thresh(rbRigidBody *object, float linear, float angular)
492 {
493   btRigidBody *body = object->body;
494   body->setSleepingThresholds(linear, angular);
495 }
496
497 /* ............ */
498
499 void RB_body_get_linear_velocity(rbRigidBody *object, float v_out[3])
500 {
501   btRigidBody *body = object->body;
502
503   copy_v3_btvec3(v_out, body->getLinearVelocity());
504 }
505
506 void RB_body_set_linear_velocity(rbRigidBody *object, const float v_in[3])
507 {
508   btRigidBody *body = object->body;
509
510   body->setLinearVelocity(btVector3(v_in[0], v_in[1], v_in[2]));
511 }
512
513 void RB_body_get_angular_velocity(rbRigidBody *object, float v_out[3])
514 {
515   btRigidBody *body = object->body;
516
517   copy_v3_btvec3(v_out, body->getAngularVelocity());
518 }
519
520 void RB_body_set_angular_velocity(rbRigidBody *object, const float v_in[3])
521 {
522   btRigidBody *body = object->body;
523
524   body->setAngularVelocity(btVector3(v_in[0], v_in[1], v_in[2]));
525 }
526
527 void RB_body_set_linear_factor(rbRigidBody *object, float x, float y, float z)
528 {
529   btRigidBody *body = object->body;
530   body->setLinearFactor(btVector3(x, y, z));
531 }
532
533 void RB_body_set_angular_factor(rbRigidBody *object, float x, float y, float z)
534 {
535   btRigidBody *body = object->body;
536   body->setAngularFactor(btVector3(x, y, z));
537 }
538
539 /* ............ */
540
541 void RB_body_set_kinematic_state(rbRigidBody *object, int kinematic)
542 {
543   btRigidBody *body = object->body;
544   if (kinematic)
545     body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
546   else
547     body->setCollisionFlags(body->getCollisionFlags() & ~btCollisionObject::CF_KINEMATIC_OBJECT);
548 }
549
550 /* ............ */
551
552 void RB_body_set_activation_state(rbRigidBody *object, int use_deactivation)
553 {
554   btRigidBody *body = object->body;
555   if (use_deactivation)
556     body->forceActivationState(ACTIVE_TAG);
557   else
558     body->setActivationState(DISABLE_DEACTIVATION);
559 }
560 void RB_body_activate(rbRigidBody *object)
561 {
562   btRigidBody *body = object->body;
563   body->setActivationState(ACTIVE_TAG);
564 }
565 void RB_body_deactivate(rbRigidBody *object)
566 {
567   btRigidBody *body = object->body;
568   body->setActivationState(ISLAND_SLEEPING);
569 }
570
571 /* ............ */
572
573 /* Simulation ----------------------- */
574
575 /* The transform matrices Blender uses are OpenGL-style matrices, 
576  * while Bullet uses the Right-Handed coordinate system style instead.
577  */
578
579 void RB_body_get_transform_matrix(rbRigidBody *object, float m_out[4][4])
580 {
581   btRigidBody *body = object->body;
582   btMotionState *ms = body->getMotionState();
583
584   btTransform trans;
585   ms->getWorldTransform(trans);
586
587   trans.getOpenGLMatrix((btScalar *)m_out);
588 }
589
590 void RB_body_set_loc_rot(rbRigidBody *object, const float loc[3], const float rot[4])
591 {
592   btRigidBody *body = object->body;
593   btMotionState *ms = body->getMotionState();
594
595   /* set transform matrix */
596   btTransform trans;
597   trans.setOrigin(btVector3(loc[0], loc[1], loc[2]));
598   trans.setRotation(btQuaternion(rot[1], rot[2], rot[3], rot[0]));
599
600   ms->setWorldTransform(trans);
601 }
602
603 void RB_body_set_scale(rbRigidBody *object, const float scale[3])
604 {
605   btRigidBody *body = object->body;
606
607   /* apply scaling factor from matrix above to the collision shape */
608   btCollisionShape *cshape = body->getCollisionShape();
609   if (cshape) {
610     cshape->setLocalScaling(btVector3(scale[0], scale[1], scale[2]));
611
612     /* GIimpact shapes have to be updated to take scaling into account */
613     if (cshape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
614       ((btGImpactMeshShape *)cshape)->updateBound();
615   }
616 }
617
618 /* ............ */
619 /* Read-only state info about status of simulation */
620
621 void RB_body_get_position(rbRigidBody *object, float v_out[3])
622 {
623   btRigidBody *body = object->body;
624
625   copy_v3_btvec3(v_out, body->getWorldTransform().getOrigin());
626 }
627
628 void RB_body_get_orientation(rbRigidBody *object, float v_out[4])
629 {
630   btRigidBody *body = object->body;
631
632   copy_quat_btquat(v_out, body->getWorldTransform().getRotation());
633 }
634
635 /* ............ */
636 /* Overrides for simulation */
637
638 void RB_body_apply_central_force(rbRigidBody *object, const float v_in[3])
639 {
640   btRigidBody *body = object->body;
641
642   body->applyCentralForce(btVector3(v_in[0], v_in[1], v_in[2]));
643 }
644
645 /* ********************************** */
646 /* Collision Shape Methods */
647
648 /* Setup (Standard Shapes) ----------- */
649
650 rbCollisionShape *RB_shape_new_box(float x, float y, float z)
651 {
652   rbCollisionShape *shape = new rbCollisionShape;
653   shape->cshape = new btBoxShape(btVector3(x, y, z));
654   shape->mesh = NULL;
655   return shape;
656 }
657
658 rbCollisionShape *RB_shape_new_sphere(float radius)
659 {
660   rbCollisionShape *shape = new rbCollisionShape;
661   shape->cshape = new btSphereShape(radius);
662   shape->mesh = NULL;
663   return shape;
664 }
665
666 rbCollisionShape *RB_shape_new_capsule(float radius, float height)
667 {
668   rbCollisionShape *shape = new rbCollisionShape;
669   shape->cshape = new btCapsuleShapeZ(radius, height);
670   shape->mesh = NULL;
671   return shape;
672 }
673
674 rbCollisionShape *RB_shape_new_cone(float radius, float height)
675 {
676   rbCollisionShape *shape = new rbCollisionShape;
677   shape->cshape = new btConeShapeZ(radius, height);
678   shape->mesh = NULL;
679   return shape;
680 }
681
682 rbCollisionShape *RB_shape_new_cylinder(float radius, float height)
683 {
684   rbCollisionShape *shape = new rbCollisionShape;
685   shape->cshape = new btCylinderShapeZ(btVector3(radius, radius, height));
686   shape->mesh = NULL;
687   return shape;
688 }
689
690 /* Setup (Convex Hull) ------------ */
691
692 rbCollisionShape *RB_shape_new_convex_hull(
693     float *verts, int stride, int count, float margin, bool *can_embed)
694 {
695   btConvexHullComputer hull_computer = btConvexHullComputer();
696
697   // try to embed the margin, if that fails don't shrink the hull
698   if (hull_computer.compute(verts, stride, count, margin, 0.0f) < 0.0f) {
699     hull_computer.compute(verts, stride, count, 0.0f, 0.0f);
700     *can_embed = false;
701   }
702
703   rbCollisionShape *shape = new rbCollisionShape;
704   btConvexHullShape *hull_shape = new btConvexHullShape(&(hull_computer.vertices[0].getX()),
705                                                         hull_computer.vertices.size());
706
707   shape->cshape = hull_shape;
708   shape->mesh = NULL;
709   return shape;
710 }
711
712 /* Setup (Triangle Mesh) ---------- */
713
714 /* Need to call RB_trimesh_finish() after creating triangle mesh and adding vertices and triangles */
715
716 rbMeshData *RB_trimesh_data_new(int num_tris, int num_verts)
717 {
718   rbMeshData *mesh = new rbMeshData;
719   mesh->vertices = new rbVert[num_verts];
720   mesh->triangles = new rbTri[num_tris];
721   mesh->num_vertices = num_verts;
722   mesh->num_triangles = num_tris;
723
724   return mesh;
725 }
726
727 static void RB_trimesh_data_delete(rbMeshData *mesh)
728 {
729   delete mesh->index_array;
730   delete[] mesh->vertices;
731   delete[] mesh->triangles;
732   delete mesh;
733 }
734
735 void RB_trimesh_add_vertices(rbMeshData *mesh, float *vertices, int num_verts, int vert_stride)
736 {
737   for (int i = 0; i < num_verts; i++) {
738     float *vert = (float *)(((char *)vertices + i * vert_stride));
739     mesh->vertices[i].x = vert[0];
740     mesh->vertices[i].y = vert[1];
741     mesh->vertices[i].z = vert[2];
742   }
743 }
744 void RB_trimesh_add_triangle_indices(rbMeshData *mesh, int num, int index0, int index1, int index2)
745 {
746   mesh->triangles[num].v0 = index0;
747   mesh->triangles[num].v1 = index1;
748   mesh->triangles[num].v2 = index2;
749 }
750
751 void RB_trimesh_finish(rbMeshData *mesh)
752 {
753   mesh->index_array = new btTriangleIndexVertexArray(mesh->num_triangles,
754                                                      (int *)mesh->triangles,
755                                                      sizeof(rbTri),
756                                                      mesh->num_vertices,
757                                                      (float *)mesh->vertices,
758                                                      sizeof(rbVert));
759 }
760
761 rbCollisionShape *RB_shape_new_trimesh(rbMeshData *mesh)
762 {
763   rbCollisionShape *shape = new rbCollisionShape;
764
765   /* triangle-mesh we create is a BVH wrapper for triangle mesh data (for faster lookups) */
766   // RB_TODO perhaps we need to allow saving out this for performance when rebuilding?
767   btBvhTriangleMeshShape *unscaledShape = new btBvhTriangleMeshShape(
768       mesh->index_array, true, true);
769
770   shape->cshape = new btScaledBvhTriangleMeshShape(unscaledShape, btVector3(1.0f, 1.0f, 1.0f));
771   shape->mesh = mesh;
772   return shape;
773 }
774
775 void RB_shape_trimesh_update(rbCollisionShape *shape,
776                              float *vertices,
777                              int num_verts,
778                              int vert_stride,
779                              float min[3],
780                              float max[3])
781 {
782   if (shape->mesh == NULL || num_verts != shape->mesh->num_vertices)
783     return;
784
785   for (int i = 0; i < num_verts; i++) {
786     float *vert = (float *)(((char *)vertices + i * vert_stride));
787     shape->mesh->vertices[i].x = vert[0];
788     shape->mesh->vertices[i].y = vert[1];
789     shape->mesh->vertices[i].z = vert[2];
790   }
791
792   if (shape->cshape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) {
793     btScaledBvhTriangleMeshShape *scaled_shape = (btScaledBvhTriangleMeshShape *)shape->cshape;
794     btBvhTriangleMeshShape *mesh_shape = scaled_shape->getChildShape();
795     mesh_shape->refitTree(btVector3(min[0], min[1], min[2]), btVector3(max[0], max[1], max[2]));
796   }
797   else if (shape->cshape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) {
798     btGImpactMeshShape *mesh_shape = (btGImpactMeshShape *)shape->cshape;
799     mesh_shape->updateBound();
800   }
801 }
802
803 rbCollisionShape *RB_shape_new_gimpact_mesh(rbMeshData *mesh)
804 {
805   rbCollisionShape *shape = new rbCollisionShape;
806
807   btGImpactMeshShape *gimpactShape = new btGImpactMeshShape(mesh->index_array);
808   gimpactShape->updateBound();  // TODO: add this to the update collision margin call?
809
810   shape->cshape = gimpactShape;
811   shape->mesh = mesh;
812   return shape;
813 }
814
815 /* Cleanup --------------------------- */
816
817 void RB_shape_delete(rbCollisionShape *shape)
818 {
819   if (shape->cshape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) {
820     btBvhTriangleMeshShape *child_shape =
821         ((btScaledBvhTriangleMeshShape *)shape->cshape)->getChildShape();
822     if (child_shape)
823       delete child_shape;
824   }
825   if (shape->mesh)
826     RB_trimesh_data_delete(shape->mesh);
827   delete shape->cshape;
828   delete shape;
829 }
830
831 /* Settings --------------------------- */
832
833 float RB_shape_get_margin(rbCollisionShape *shape)
834 {
835   return shape->cshape->getMargin();
836 }
837
838 void RB_shape_set_margin(rbCollisionShape *shape, float value)
839 {
840   shape->cshape->setMargin(value);
841 }
842
843 /* ********************************** */
844 /* Constraints */
845
846 /* Setup ----------------------------- */
847
848 void RB_dworld_add_constraint(rbDynamicsWorld *world, rbConstraint *con, int disable_collisions)
849 {
850   btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);
851
852   world->dynamicsWorld->addConstraint(constraint, disable_collisions);
853 }
854
855 void RB_dworld_remove_constraint(rbDynamicsWorld *world, rbConstraint *con)
856 {
857   btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);
858
859   world->dynamicsWorld->removeConstraint(constraint);
860 }
861
862 /* ............ */
863
864 static void make_constraint_transforms(btTransform &transform1,
865                                        btTransform &transform2,
866                                        btRigidBody *body1,
867                                        btRigidBody *body2,
868                                        float pivot[3],
869                                        float orn[4])
870 {
871   btTransform pivot_transform = btTransform();
872   pivot_transform.setOrigin(btVector3(pivot[0], pivot[1], pivot[2]));
873   pivot_transform.setRotation(btQuaternion(orn[1], orn[2], orn[3], orn[0]));
874
875   transform1 = body1->getWorldTransform().inverse() * pivot_transform;
876   transform2 = body2->getWorldTransform().inverse() * pivot_transform;
877 }
878
879 rbConstraint *RB_constraint_new_point(float pivot[3], rbRigidBody *rb1, rbRigidBody *rb2)
880 {
881   btRigidBody *body1 = rb1->body;
882   btRigidBody *body2 = rb2->body;
883
884   btVector3 pivot1 = body1->getWorldTransform().inverse() *
885                      btVector3(pivot[0], pivot[1], pivot[2]);
886   btVector3 pivot2 = body2->getWorldTransform().inverse() *
887                      btVector3(pivot[0], pivot[1], pivot[2]);
888
889   btTypedConstraint *con = new btPoint2PointConstraint(*body1, *body2, pivot1, pivot2);
890
891   return (rbConstraint *)con;
892 }
893
894 rbConstraint *RB_constraint_new_fixed(float pivot[3],
895                                       float orn[4],
896                                       rbRigidBody *rb1,
897                                       rbRigidBody *rb2)
898 {
899   btRigidBody *body1 = rb1->body;
900   btRigidBody *body2 = rb2->body;
901   btTransform transform1;
902   btTransform transform2;
903
904   make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
905
906   btFixedConstraint *con = new btFixedConstraint(*body1, *body2, transform1, transform2);
907
908   return (rbConstraint *)con;
909 }
910
911 rbConstraint *RB_constraint_new_hinge(float pivot[3],
912                                       float orn[4],
913                                       rbRigidBody *rb1,
914                                       rbRigidBody *rb2)
915 {
916   btRigidBody *body1 = rb1->body;
917   btRigidBody *body2 = rb2->body;
918   btTransform transform1;
919   btTransform transform2;
920
921   make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
922
923   btHingeConstraint *con = new btHingeConstraint(*body1, *body2, transform1, transform2);
924
925   return (rbConstraint *)con;
926 }
927
928 rbConstraint *RB_constraint_new_slider(float pivot[3],
929                                        float orn[4],
930                                        rbRigidBody *rb1,
931                                        rbRigidBody *rb2)
932 {
933   btRigidBody *body1 = rb1->body;
934   btRigidBody *body2 = rb2->body;
935   btTransform transform1;
936   btTransform transform2;
937
938   make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
939
940   btSliderConstraint *con = new btSliderConstraint(*body1, *body2, transform1, transform2, true);
941
942   return (rbConstraint *)con;
943 }
944
945 rbConstraint *RB_constraint_new_piston(float pivot[3],
946                                        float orn[4],
947                                        rbRigidBody *rb1,
948                                        rbRigidBody *rb2)
949 {
950   btRigidBody *body1 = rb1->body;
951   btRigidBody *body2 = rb2->body;
952   btTransform transform1;
953   btTransform transform2;
954
955   make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
956
957   btSliderConstraint *con = new btSliderConstraint(*body1, *body2, transform1, transform2, true);
958   con->setUpperAngLimit(-1.0f);  // unlock rotation axis
959
960   return (rbConstraint *)con;
961 }
962
963 rbConstraint *RB_constraint_new_6dof(float pivot[3],
964                                      float orn[4],
965                                      rbRigidBody *rb1,
966                                      rbRigidBody *rb2)
967 {
968   btRigidBody *body1 = rb1->body;
969   btRigidBody *body2 = rb2->body;
970   btTransform transform1;
971   btTransform transform2;
972
973   make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
974
975   btTypedConstraint *con = new btGeneric6DofConstraint(
976       *body1, *body2, transform1, transform2, true);
977
978   return (rbConstraint *)con;
979 }
980
981 rbConstraint *RB_constraint_new_6dof_spring(float pivot[3],
982                                             float orn[4],
983                                             rbRigidBody *rb1,
984                                             rbRigidBody *rb2)
985 {
986   btRigidBody *body1 = rb1->body;
987   btRigidBody *body2 = rb2->body;
988   btTransform transform1;
989   btTransform transform2;
990
991   make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
992
993   btTypedConstraint *con = new btGeneric6DofSpringConstraint(
994       *body1, *body2, transform1, transform2, true);
995
996   return (rbConstraint *)con;
997 }
998
999 rbConstraint *RB_constraint_new_6dof_spring2(float pivot[3],
1000                                              float orn[4],
1001                                              rbRigidBody *rb1,
1002                                              rbRigidBody *rb2)
1003 {
1004   btRigidBody *body1 = rb1->body;
1005   btRigidBody *body2 = rb2->body;
1006   btTransform transform1;
1007   btTransform transform2;
1008
1009   make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
1010
1011   btTypedConstraint *con = new btGeneric6DofSpring2Constraint(
1012       *body1, *body2, transform1, transform2);
1013
1014   return (rbConstraint *)con;
1015 }
1016
1017 rbConstraint *RB_constraint_new_motor(float pivot[3],
1018                                       float orn[4],
1019                                       rbRigidBody *rb1,
1020                                       rbRigidBody *rb2)
1021 {
1022   btRigidBody *body1 = rb1->body;
1023   btRigidBody *body2 = rb2->body;
1024   btTransform transform1;
1025   btTransform transform2;
1026
1027   make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
1028
1029   btGeneric6DofConstraint *con = new btGeneric6DofConstraint(
1030       *body1, *body2, transform1, transform2, true);
1031
1032   /* unlock constraint axes */
1033   for (int i = 0; i < 6; i++) {
1034     con->setLimit(i, 0.0f, -1.0f);
1035   }
1036   /* unlock motor axes */
1037   con->getTranslationalLimitMotor()->m_upperLimit.setValue(-1.0f, -1.0f, -1.0f);
1038
1039   return (rbConstraint *)con;
1040 }
1041
1042 /* Cleanup ----------------------------- */
1043
1044 void RB_constraint_delete(rbConstraint *con)
1045 {
1046   btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);
1047   delete constraint;
1048 }
1049
1050 /* Settings ------------------------- */
1051
1052 void RB_constraint_set_enabled(rbConstraint *con, int enabled)
1053 {
1054   btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);
1055
1056   constraint->setEnabled(enabled);
1057 }
1058
1059 void RB_constraint_set_limits_hinge(rbConstraint *con, float lower, float upper)
1060 {
1061   btHingeConstraint *constraint = reinterpret_cast<btHingeConstraint *>(con);
1062
1063   // RB_TODO expose these
1064   float softness = 0.9f;
1065   float bias_factor = 0.3f;
1066   float relaxation_factor = 1.0f;
1067
1068   constraint->setLimit(lower, upper, softness, bias_factor, relaxation_factor);
1069 }
1070
1071 void RB_constraint_set_limits_slider(rbConstraint *con, float lower, float upper)
1072 {
1073   btSliderConstraint *constraint = reinterpret_cast<btSliderConstraint *>(con);
1074
1075   constraint->setLowerLinLimit(lower);
1076   constraint->setUpperLinLimit(upper);
1077 }
1078
1079 void RB_constraint_set_limits_piston(
1080     rbConstraint *con, float lin_lower, float lin_upper, float ang_lower, float ang_upper)
1081 {
1082   btSliderConstraint *constraint = reinterpret_cast<btSliderConstraint *>(con);
1083
1084   constraint->setLowerLinLimit(lin_lower);
1085   constraint->setUpperLinLimit(lin_upper);
1086   constraint->setLowerAngLimit(ang_lower);
1087   constraint->setUpperAngLimit(ang_upper);
1088 }
1089
1090 void RB_constraint_set_limits_6dof(rbConstraint *con, int axis, float lower, float upper)
1091 {
1092   btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint *>(con);
1093
1094   constraint->setLimit(axis, lower, upper);
1095 }
1096
1097 void RB_constraint_set_limits_6dof_spring2(rbConstraint *con, int axis, float lower, float upper)
1098 {
1099   btGeneric6DofSpring2Constraint *constraint = reinterpret_cast<btGeneric6DofSpring2Constraint *>(
1100       con);
1101
1102   constraint->setLimit(axis, lower, upper);
1103 }
1104
1105 void RB_constraint_set_stiffness_6dof_spring(rbConstraint *con, int axis, float stiffness)
1106 {
1107   btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint *>(
1108       con);
1109
1110   constraint->setStiffness(axis, stiffness);
1111 }
1112
1113 void RB_constraint_set_damping_6dof_spring(rbConstraint *con, int axis, float damping)
1114 {
1115   btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint *>(
1116       con);
1117
1118   // invert damping range so that 0 = no damping
1119   damping = (damping > 1.0f) ? 0.0f : 1.0f - damping;
1120
1121   constraint->setDamping(axis, damping);
1122 }
1123
1124 void RB_constraint_set_spring_6dof_spring(rbConstraint *con, int axis, int enable)
1125 {
1126   btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint *>(
1127       con);
1128
1129   constraint->enableSpring(axis, enable);
1130 }
1131
1132 void RB_constraint_set_equilibrium_6dof_spring(rbConstraint *con)
1133 {
1134   btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint *>(
1135       con);
1136
1137   constraint->setEquilibriumPoint();
1138 }
1139
1140 void RB_constraint_set_stiffness_6dof_spring2(rbConstraint *con, int axis, float stiffness)
1141 {
1142   btGeneric6DofSpring2Constraint *constraint = reinterpret_cast<btGeneric6DofSpring2Constraint *>(
1143       con);
1144
1145   constraint->setStiffness(axis, stiffness);
1146 }
1147
1148 void RB_constraint_set_damping_6dof_spring2(rbConstraint *con, int axis, float damping)
1149 {
1150   btGeneric6DofSpring2Constraint *constraint = reinterpret_cast<btGeneric6DofSpring2Constraint *>(
1151       con);
1152
1153   constraint->setDamping(axis, damping);
1154 }
1155
1156 void RB_constraint_set_spring_6dof_spring2(rbConstraint *con, int axis, int enable)
1157 {
1158   btGeneric6DofSpring2Constraint *constraint = reinterpret_cast<btGeneric6DofSpring2Constraint *>(
1159       con);
1160
1161   constraint->enableSpring(axis, enable);
1162 }
1163
1164 void RB_constraint_set_equilibrium_6dof_spring2(rbConstraint *con)
1165 {
1166   btGeneric6DofSpring2Constraint *constraint = reinterpret_cast<btGeneric6DofSpring2Constraint *>(
1167       con);
1168
1169   constraint->setEquilibriumPoint();
1170 }
1171
1172 void RB_constraint_set_solver_iterations(rbConstraint *con, int num_solver_iterations)
1173 {
1174   btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);
1175
1176   constraint->setOverrideNumSolverIterations(num_solver_iterations);
1177 }
1178
1179 void RB_constraint_set_breaking_threshold(rbConstraint *con, float threshold)
1180 {
1181   btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);
1182
1183   constraint->setBreakingImpulseThreshold(threshold);
1184 }
1185
1186 void RB_constraint_set_enable_motor(rbConstraint *con, int enable_lin, int enable_ang)
1187 {
1188   btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint *>(con);
1189
1190   constraint->getTranslationalLimitMotor()->m_enableMotor[0] = enable_lin;
1191   constraint->getRotationalLimitMotor(0)->m_enableMotor = enable_ang;
1192 }
1193
1194 void RB_constraint_set_max_impulse_motor(rbConstraint *con,
1195                                          float max_impulse_lin,
1196                                          float max_impulse_ang)
1197 {
1198   btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint *>(con);
1199
1200   constraint->getTranslationalLimitMotor()->m_maxMotorForce.setX(max_impulse_lin);
1201   constraint->getRotationalLimitMotor(0)->m_maxMotorForce = max_impulse_ang;
1202 }
1203
1204 void RB_constraint_set_target_velocity_motor(rbConstraint *con,
1205                                              float velocity_lin,
1206                                              float velocity_ang)
1207 {
1208   btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint *>(con);
1209
1210   constraint->getTranslationalLimitMotor()->m_targetVelocity.setX(velocity_lin);
1211   constraint->getRotationalLimitMotor(0)->m_targetVelocity = velocity_ang;
1212 }
1213
1214 /* ********************************** */