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