c0d75a73b6af1d24bcbad88b4c86be2752a6edbd
[blender.git] / source / blender / 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  
61 #include "RBI_api.h"
62
63 #include "BLI_math.h"
64
65 #include "btBulletDynamicsCommon.h"
66
67 #include "LinearMath/btVector3.h"
68 #include "LinearMath/btScalar.h"        
69 #include "LinearMath/btMatrix3x3.h"
70 #include "LinearMath/btTransform.h"
71 #include "LinearMath/btConvexHullComputer.h"
72
73 #include "BulletCollision/Gimpact/btGImpactShape.h"
74 #include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
75 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
76
77 struct rbDynamicsWorld {
78         btDiscreteDynamicsWorld *dynamicsWorld;
79         btDefaultCollisionConfiguration *collisionConfiguration;
80         btDispatcher *dispatcher;
81         btBroadphaseInterface *pairCache;
82         btConstraintSolver *constraintSolver;
83         btOverlapFilterCallback *filterCallback;
84 };
85 struct rbRigidBody {
86         btRigidBody *body;
87         int col_groups;
88 };
89
90 struct rbCollisionShape {
91         btCollisionShape *cshape;
92         btTriangleMesh *mesh;
93 };
94
95 struct rbFilterCallback : public btOverlapFilterCallback
96 {
97         virtual bool    needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
98         {
99                 rbRigidBody *rb0 = (rbRigidBody*)((btRigidBody*)proxy0->m_clientObject)->getUserPointer();
100                 rbRigidBody *rb1 = (rbRigidBody*)((btRigidBody*)proxy1->m_clientObject)->getUserPointer();
101                 
102                 bool collides;
103                 collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
104                 collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
105                 collides = collides && (rb0->col_groups & rb1->col_groups);
106                 
107                 return collides;
108         }
109 };
110
111 static inline void copy_v3_btvec3(float vec[3], const btVector3 &btvec)
112 {
113         vec[0] = (float)btvec[0];
114         vec[1] = (float)btvec[1];
115         vec[2] = (float)btvec[2];
116 }
117 static inline void copy_quat_btquat(float quat[3], const btQuaternion &btquat)
118 {
119         quat[0] = btquat.getW();
120         quat[1] = btquat.getX();
121         quat[2] = btquat.getY();
122         quat[3] = btquat.getZ();
123 }
124
125 /* ********************************** */
126 /* Dynamics World Methods */
127
128 /* Setup ---------------------------- */
129
130 rbDynamicsWorld *RB_dworld_new(const float gravity[3])
131 {
132         rbDynamicsWorld *world = new rbDynamicsWorld;
133         
134         /* collision detection/handling */
135         world->collisionConfiguration = new btDefaultCollisionConfiguration();
136         
137         world->dispatcher = new btCollisionDispatcher(world->collisionConfiguration);
138         btGImpactCollisionAlgorithm::registerAlgorithm((btCollisionDispatcher*)world->dispatcher); // XXX: experimental
139         
140         world->pairCache = new btDbvtBroadphase();
141         
142         world->filterCallback = new rbFilterCallback();
143         world->pairCache->getOverlappingPairCache()->setOverlapFilterCallback(world->filterCallback);
144         
145         /* constraint solving */
146         world->constraintSolver = new btSequentialImpulseConstraintSolver();
147         
148         /* world */
149         world->dynamicsWorld = new btDiscreteDynamicsWorld(world->dispatcher,world->pairCache,world->constraintSolver,world->collisionConfiguration);
150         
151         RB_dworld_set_gravity(world, gravity);
152         
153         return world;
154 }
155
156 void RB_dworld_delete(rbDynamicsWorld *world)
157 {
158         /* bullet doesn't like if we free these in a different order */
159         delete world->dynamicsWorld;
160         delete world->constraintSolver;
161         delete world->pairCache;
162         delete world->dispatcher;
163         delete world->collisionConfiguration;
164         delete world->filterCallback;
165         delete world;
166 }
167
168 /* Settings ------------------------- */
169
170 /* Gravity */
171 void RB_dworld_get_gravity(rbDynamicsWorld *world, float g_out[3])
172 {
173         copy_v3_btvec3(g_out, world->dynamicsWorld->getGravity());
174 }
175
176 void RB_dworld_set_gravity(rbDynamicsWorld *world, const float g_in[3])
177 {
178         world->dynamicsWorld->setGravity(btVector3(g_in[0], g_in[1], g_in[2]));
179 }
180
181 /* Constraint Solver */
182 void RB_dworld_set_solver_iterations(rbDynamicsWorld *world, int num_solver_iterations)
183 {
184         btContactSolverInfo& info = world->dynamicsWorld->getSolverInfo();
185         
186         info.m_numIterations = num_solver_iterations;
187 }
188
189 /* Split Impulse */
190 void RB_dworld_set_split_impulse(rbDynamicsWorld *world, int split_impulse)
191 {
192         btContactSolverInfo& info = world->dynamicsWorld->getSolverInfo();
193         
194         info.m_splitImpulse = split_impulse;
195 }
196
197 /* Simulation ----------------------- */
198
199 void RB_dworld_step_simulation(rbDynamicsWorld *world, float timeStep, int maxSubSteps, float timeSubStep)
200 {
201         world->dynamicsWorld->stepSimulation(timeStep, maxSubSteps, timeSubStep);
202 }
203
204 /* Export -------------------------- */
205
206 /* Exports entire dynamics world to Bullet's "*.bullet" binary format
207  * which is similar to Blender's SDNA system...
208  * < rbDynamicsWorld: dynamics world to write to file
209  * < filename: assumed to be a valid filename, with .bullet extension 
210  */
211 void RB_dworld_export(rbDynamicsWorld *world, const char *filename)
212 {
213         //create a large enough buffer. There is no method to pre-calculate the buffer size yet.
214         int maxSerializeBufferSize = 1024*1024*5;
215         
216         btDefaultSerializer *serializer = new btDefaultSerializer(maxSerializeBufferSize);
217         world->dynamicsWorld->serialize(serializer);
218         
219         FILE *file = fopen(filename, "wb");
220         fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1, file);
221         fclose(file);
222 }
223
224 /* ********************************** */
225 /* Rigid Body Methods */
226
227 /* Setup ---------------------------- */
228
229 void RB_dworld_add_body(rbDynamicsWorld *world, rbRigidBody *object, int col_groups)
230 {
231         btRigidBody *body = object->body;
232         object->col_groups = col_groups;
233         
234         world->dynamicsWorld->addRigidBody(body);
235 }
236
237 void RB_dworld_remove_body(rbDynamicsWorld *world, rbRigidBody *object)
238 {
239         btRigidBody *body = object->body;
240         
241         world->dynamicsWorld->removeRigidBody(body);
242 }
243
244 /* ............ */
245
246 rbRigidBody *RB_body_new(rbCollisionShape *shape, const float loc[3], const float rot[4])
247 {
248         rbRigidBody *object = new rbRigidBody;
249         /* current transform */
250         btTransform trans;
251         trans.setOrigin(btVector3(loc[0], loc[1], loc[2]));
252         trans.setRotation(btQuaternion(rot[1], rot[2], rot[3], rot[0]));
253         
254         /* create motionstate, which is necessary for interpolation (includes reverse playback) */
255         btDefaultMotionState *motionState = new btDefaultMotionState(trans);
256         
257         /* make rigidbody */
258         btRigidBody::btRigidBodyConstructionInfo rbInfo(1.0f, motionState, shape->cshape);
259         
260         object->body = new btRigidBody(rbInfo);
261         
262         object->body->setUserPointer(object);
263         
264         return object;
265 }
266
267 void RB_body_delete(rbRigidBody *object)
268 {
269         btRigidBody *body = object->body;
270         
271         /* motion state */
272         btMotionState *ms = body->getMotionState();
273         if (ms)
274                 delete ms;
275         
276         /* collision shape is done elsewhere... */
277         
278         /* body itself */
279         
280         /* manually remove constraint refs of the rigid body, normally this happens when removing constraints from the world
281          * but since we delete everything when the world is rebult, we need to do it manually here */
282         for (int i = body->getNumConstraintRefs() - 1; i >= 0; i--) {
283                 btTypedConstraint* con = body->getConstraintRef(i);
284                 body->removeConstraintRef(con);
285         }
286         
287         delete body;
288         delete object;
289 }
290
291 /* Settings ------------------------- */
292
293 void RB_body_set_collision_shape(rbRigidBody *object, rbCollisionShape *shape)
294 {
295         btRigidBody *body = object->body;
296         
297         /* set new collision shape */
298         body->setCollisionShape(shape->cshape);
299         
300         /* recalculate inertia, since that depends on the collision shape... */
301         RB_body_set_mass(object, RB_body_get_mass(object));
302 }
303
304 /* ............ */
305
306 float RB_body_get_mass(rbRigidBody *object)
307 {
308         btRigidBody *body = object->body;
309         
310         /* there isn't really a mass setting, but rather 'inverse mass'  
311          * which we convert back to mass by taking the reciprocal again 
312          */
313         float value = (float)body->getInvMass();
314         
315         if (value)
316                 value = 1.0 / value;
317                 
318         return value;
319 }
320
321 void RB_body_set_mass(rbRigidBody *object, float value)
322 {
323         btRigidBody *body = object->body;
324         btVector3 localInertia(0,0,0);
325         
326         /* calculate new inertia if non-zero mass */
327         if (value) {
328                 btCollisionShape *shape = body->getCollisionShape();
329                 shape->calculateLocalInertia(value, localInertia);
330         }
331         
332         body->setMassProps(value, localInertia);
333         body->updateInertiaTensor();
334 }
335
336
337 float RB_body_get_friction(rbRigidBody *object)
338 {
339         btRigidBody *body = object->body;
340         return body->getFriction();
341 }
342
343 void RB_body_set_friction(rbRigidBody *object, float value)
344 {
345         btRigidBody *body = object->body;
346         body->setFriction(value);
347 }
348
349
350 float RB_body_get_restitution(rbRigidBody *object)
351 {
352         btRigidBody *body = object->body;
353         return body->getRestitution();
354 }
355
356 void RB_body_set_restitution(rbRigidBody *object, float value)
357 {
358         btRigidBody *body = object->body;
359         body->setRestitution(value);
360 }
361
362
363 float RB_body_get_linear_damping(rbRigidBody *object)
364 {
365         btRigidBody *body = object->body;
366         return body->getLinearDamping();
367 }
368
369 void RB_body_set_linear_damping(rbRigidBody *object, float value)
370 {
371         RB_body_set_damping(object, value, RB_body_get_linear_damping(object));
372 }
373
374 float RB_body_get_angular_damping(rbRigidBody *object)
375 {
376         btRigidBody *body = object->body;
377         return body->getAngularDamping();
378 }
379
380 void RB_body_set_angular_damping(rbRigidBody *object, float value)
381 {
382         RB_body_set_damping(object, RB_body_get_linear_damping(object), value);
383 }
384
385 void RB_body_set_damping(rbRigidBody *object, float linear, float angular)
386 {
387         btRigidBody *body = object->body;
388         body->setDamping(linear, angular);
389 }
390
391
392 float RB_body_get_linear_sleep_thresh(rbRigidBody *object)
393 {
394         btRigidBody *body = object->body;
395         return body->getLinearSleepingThreshold();
396 }
397
398 void RB_body_set_linear_sleep_thresh(rbRigidBody *object, float value)
399 {
400         RB_body_set_sleep_thresh(object, value, RB_body_get_angular_sleep_thresh(object));
401 }
402
403 float RB_body_get_angular_sleep_thresh(rbRigidBody *object)
404 {
405         btRigidBody *body = object->body;
406         return body->getAngularSleepingThreshold();
407 }
408
409 void RB_body_set_angular_sleep_thresh(rbRigidBody *object, float value)
410 {
411         RB_body_set_sleep_thresh(object, RB_body_get_linear_sleep_thresh(object), value);
412 }
413
414 void RB_body_set_sleep_thresh(rbRigidBody *object, float linear, float angular)
415 {
416         btRigidBody *body = object->body;
417         body->setSleepingThresholds(linear, angular);
418 }
419
420 /* ............ */
421
422 void RB_body_get_linear_velocity(rbRigidBody *object, float v_out[3])
423 {
424         btRigidBody *body = object->body;
425         
426         copy_v3_btvec3(v_out, body->getLinearVelocity());
427 }
428
429 void RB_body_set_linear_velocity(rbRigidBody *object, const float v_in[3])
430 {
431         btRigidBody *body = object->body;
432         
433         body->setLinearVelocity(btVector3(v_in[0], v_in[1], v_in[2]));
434 }
435
436
437 void RB_body_get_angular_velocity(rbRigidBody *object, float v_out[3])
438 {
439         btRigidBody *body = object->body;
440         
441         copy_v3_btvec3(v_out, body->getAngularVelocity());
442 }
443
444 void RB_body_set_angular_velocity(rbRigidBody *object, const float v_in[3])
445 {
446         btRigidBody *body = object->body;
447         
448         body->setAngularVelocity(btVector3(v_in[0], v_in[1], v_in[2]));
449 }
450
451 void RB_body_set_linear_factor(rbRigidBody *object, float x, float y, float z)
452 {
453         btRigidBody *body = object->body;
454         body->setLinearFactor(btVector3(x, y, z));
455 }
456
457 void RB_body_set_angular_factor(rbRigidBody *object, float x, float y, float z)
458 {
459         btRigidBody *body = object->body;
460         body->setAngularFactor(btVector3(x, y, z));
461 }
462
463 /* ............ */
464
465 void RB_body_set_kinematic_state(rbRigidBody *object, int kinematic)
466 {
467         btRigidBody *body = object->body;
468         if (kinematic)
469                 body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
470         else
471                 body->setCollisionFlags(body->getCollisionFlags() & ~btCollisionObject::CF_KINEMATIC_OBJECT);
472 }
473
474 /* ............ */
475
476 void RB_body_set_activation_state(rbRigidBody *object, int use_deactivation)
477 {
478         btRigidBody *body = object->body;
479         if (use_deactivation)
480                 body->forceActivationState(ACTIVE_TAG);
481         else
482                 body->setActivationState(DISABLE_DEACTIVATION);
483 }
484 void RB_body_activate(rbRigidBody *object)
485 {
486         btRigidBody *body = object->body;
487         body->setActivationState(ACTIVE_TAG);
488 }
489 void RB_body_deactivate(rbRigidBody *object)
490 {
491         btRigidBody *body = object->body;
492         body->setActivationState(ISLAND_SLEEPING);
493 }
494
495 /* ............ */
496
497
498
499 /* Simulation ----------------------- */
500
501 /* The transform matrices Blender uses are OpenGL-style matrices, 
502  * while Bullet uses the Right-Handed coordinate system style instead.
503  */
504
505 void RB_body_get_transform_matrix(rbRigidBody *object, float m_out[4][4])
506 {
507         btRigidBody *body = object->body;
508         btMotionState *ms = body->getMotionState();
509         
510         btTransform trans;
511         ms->getWorldTransform(trans);
512         
513         trans.getOpenGLMatrix((btScalar*)m_out);
514 }
515
516 void RB_body_set_loc_rot(rbRigidBody *object, const float loc[3], const float rot[4])
517 {
518         btRigidBody *body = object->body;
519         btMotionState *ms = body->getMotionState();
520         
521         /* set transform matrix */
522         btTransform trans;
523         trans.setOrigin(btVector3(loc[0], loc[1], loc[2]));
524         trans.setRotation(btQuaternion(rot[1], rot[2], rot[3], rot[0]));
525         
526         ms->setWorldTransform(trans);
527 }
528
529 void RB_body_set_scale(rbRigidBody *object, const float scale[3])
530 {
531         btRigidBody *body = object->body;
532         
533         /* apply scaling factor from matrix above to the collision shape */
534         btCollisionShape *cshape = body->getCollisionShape();
535         if (cshape) {
536                 cshape->setLocalScaling(btVector3(scale[0], scale[1], scale[2]));
537                 
538                 /* GIimpact shapes have to be updated to take scaling into account */
539                 if (cshape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
540                         ((btGImpactMeshShape *)cshape)->updateBound();
541         }
542 }
543
544 /* ............ */
545 /* Read-only state info about status of simulation */
546
547 void RB_body_get_position(rbRigidBody *object, float v_out[3])
548 {
549         btRigidBody *body = object->body;
550         
551         copy_v3_btvec3(v_out, body->getWorldTransform().getOrigin());
552 }
553
554 void RB_body_get_orientation(rbRigidBody *object, float v_out[4])
555 {
556         btRigidBody *body = object->body;
557         
558         copy_quat_btquat(v_out, body->getWorldTransform().getRotation());
559 }
560
561 /* ............ */
562 /* Overrides for simulation */
563
564 void RB_body_apply_central_force(rbRigidBody *object, const float v_in[3])
565 {
566         btRigidBody *body = object->body;
567         
568         body->applyCentralForce(btVector3(v_in[0], v_in[1], v_in[2]));
569 }
570
571 /* ********************************** */
572 /* Collision Shape Methods */
573
574 /* Setup (Standard Shapes) ----------- */
575
576 rbCollisionShape *RB_shape_new_box(float x, float y, float z)
577 {
578         rbCollisionShape *shape = new rbCollisionShape;
579         shape->cshape = new btBoxShape(btVector3(x, y, z));
580         shape->mesh = NULL;
581         return shape;
582 }
583
584 rbCollisionShape *RB_shape_new_sphere(float radius)
585 {
586         rbCollisionShape *shape = new rbCollisionShape;
587         shape->cshape = new btSphereShape(radius);
588         shape->mesh = NULL;
589         return shape;
590 }
591
592 rbCollisionShape *RB_shape_new_capsule(float radius, float height)
593 {
594         rbCollisionShape *shape = new rbCollisionShape;
595         shape->cshape = new btCapsuleShapeZ(radius, height);
596         shape->mesh = NULL;
597         return shape;
598 }
599
600 rbCollisionShape *RB_shape_new_cone(float radius, float height)
601 {
602         rbCollisionShape *shape = new rbCollisionShape;
603         shape->cshape = new btConeShapeZ(radius, height);
604         shape->mesh = NULL;
605         return shape;
606 }
607
608 rbCollisionShape *RB_shape_new_cylinder(float radius, float height)
609 {
610         rbCollisionShape *shape = new rbCollisionShape;
611         shape->cshape = new btCylinderShapeZ(btVector3(radius, radius, height));
612         shape->mesh = NULL;
613         return shape;
614 }
615
616 /* Setup (Convex Hull) ------------ */
617
618 rbCollisionShape *RB_shape_new_convex_hull(float *verts, int stride, int count, float margin, bool *can_embed)
619 {
620         btConvexHullComputer hull_computer = btConvexHullComputer();
621         
622         // try to embed the margin, if that fails don't shrink the hull
623         if (hull_computer.compute(verts, stride, count, margin, 0.0f) < 0.0f) {
624                 hull_computer.compute(verts, stride, count, 0.0f, 0.0f);
625                 *can_embed = false;
626         }
627         
628         rbCollisionShape *shape = new rbCollisionShape;
629         btConvexHullShape *hull_shape = new btConvexHullShape(&(hull_computer.vertices[0].getX()), hull_computer.vertices.size());
630         
631         shape->cshape = hull_shape;
632         shape->mesh = NULL;
633         return shape;
634 }
635
636 /* Setup (Triangle Mesh) ---------- */
637
638 /* Need to call rbTriMeshNewData() followed by rbTriMeshAddTriangle() several times 
639  * to set up the mesh buffer BEFORE calling rbShapeNewTriMesh(). Otherwise,
640  * we get nasty crashes...
641  */
642
643 rbMeshData *RB_trimesh_data_new()
644 {
645         // XXX: welding threshold?
646         return (rbMeshData*) new btTriangleMesh(true, false);
647 }
648  
649 void RB_trimesh_add_triangle(rbMeshData *mesh, const float v1[3], const float v2[3], const float v3[3])
650 {
651         btTriangleMesh *meshData = reinterpret_cast<btTriangleMesh*>(mesh);
652         
653         /* cast vertices to usable forms for Bt-API */
654         btVector3 vtx1((btScalar)v1[0], (btScalar)v1[1], (btScalar)v1[2]);
655         btVector3 vtx2((btScalar)v2[0], (btScalar)v2[1], (btScalar)v2[2]);
656         btVector3 vtx3((btScalar)v3[0], (btScalar)v3[1], (btScalar)v3[2]);
657         
658         /* add to the mesh 
659          *      - remove duplicated verts is enabled
660          */
661         meshData->addTriangle(vtx1, vtx2, vtx3, false);
662 }
663  
664 rbCollisionShape *RB_shape_new_trimesh(rbMeshData *mesh)
665 {
666         rbCollisionShape *shape = new rbCollisionShape;
667         btTriangleMesh *tmesh = reinterpret_cast<btTriangleMesh*>(mesh);
668         
669         /* triangle-mesh we create is a BVH wrapper for triangle mesh data (for faster lookups) */
670         // RB_TODO perhaps we need to allow saving out this for performance when rebuilding?
671         btBvhTriangleMeshShape *unscaledShape = new btBvhTriangleMeshShape(tmesh, true, true);
672         
673         shape->cshape = new btScaledBvhTriangleMeshShape(unscaledShape, btVector3(1.0f,1.0f,1.0f));
674         shape->mesh = tmesh;
675         return shape;
676 }
677
678 rbCollisionShape *RB_shape_new_gimpact_mesh(rbMeshData *mesh)
679 {
680         rbCollisionShape *shape = new rbCollisionShape;
681         /* interpret mesh buffer as btTriangleIndexVertexArray (i.e. an impl of btStridingMeshInterface) */
682         btTriangleMesh *tmesh = reinterpret_cast<btTriangleMesh*>(mesh);
683         
684         btGImpactMeshShape *gimpactShape = new btGImpactMeshShape(tmesh);
685         gimpactShape->updateBound(); // TODO: add this to the update collision margin call?
686         
687         shape->cshape = gimpactShape;
688         shape->mesh = tmesh;
689         return shape;
690 }
691
692 /* Cleanup --------------------------- */
693
694 void RB_shape_delete(rbCollisionShape *shape)
695 {
696         if (shape->cshape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) {
697                 btBvhTriangleMeshShape *child_shape = ((btScaledBvhTriangleMeshShape*)shape->cshape)->getChildShape();
698                 if (child_shape)
699                         delete child_shape;
700         }
701         if (shape->mesh)
702                 delete shape->mesh;
703         delete shape->cshape;
704         delete shape;
705 }
706
707 /* Settings --------------------------- */
708
709 float RB_shape_get_margin(rbCollisionShape *shape)
710 {
711         return shape->cshape->getMargin();
712 }
713
714 void RB_shape_set_margin(rbCollisionShape *shape, float value)
715 {
716         shape->cshape->setMargin(value);
717 }
718
719 /* ********************************** */
720 /* Constraints */
721
722 /* Setup ----------------------------- */
723
724 void RB_dworld_add_constraint(rbDynamicsWorld *world, rbConstraint *con, int disable_collisions)
725 {
726         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
727         
728         world->dynamicsWorld->addConstraint(constraint, disable_collisions);
729 }
730
731 void RB_dworld_remove_constraint(rbDynamicsWorld *world, rbConstraint *con)
732 {
733         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
734         
735         world->dynamicsWorld->removeConstraint(constraint);
736 }
737
738 /* ............ */
739
740 static void make_constraint_transforms(btTransform &transform1, btTransform &transform2, btRigidBody *body1, btRigidBody *body2, float pivot[3], float orn[4])
741 {
742         btTransform pivot_transform = btTransform();
743         pivot_transform.setOrigin(btVector3(pivot[0], pivot[1], pivot[2]));
744         pivot_transform.setRotation(btQuaternion(orn[1], orn[2], orn[3], orn[0]));
745         
746         transform1 = body1->getWorldTransform().inverse() * pivot_transform;
747         transform2 = body2->getWorldTransform().inverse() * pivot_transform;
748 }
749
750 rbConstraint *RB_constraint_new_point(float pivot[3], rbRigidBody *rb1, rbRigidBody *rb2)
751 {
752         btRigidBody *body1 = rb1->body;
753         btRigidBody *body2 = rb2->body;
754         
755         btVector3 pivot1 = body1->getWorldTransform().inverse() * btVector3(pivot[0], pivot[1], pivot[2]);
756         btVector3 pivot2 = body2->getWorldTransform().inverse() * btVector3(pivot[0], pivot[1], pivot[2]);
757         
758         btTypedConstraint *con = new btPoint2PointConstraint(*body1, *body2, pivot1, pivot2);
759         
760         return (rbConstraint*)con;
761 }
762
763 rbConstraint *RB_constraint_new_fixed(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
764 {
765         btRigidBody *body1 = rb1->body;
766         btRigidBody *body2 = rb2->body;
767         btTransform transform1;
768         btTransform transform2;
769         
770         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
771         
772         btGeneric6DofConstraint *con = new btGeneric6DofConstraint(*body1, *body2, transform1, transform2, true);
773         
774         /* lock all axes */
775         for (int i = 0; i < 6; i++)
776                 con->setLimit(i, 0, 0);
777         
778         return (rbConstraint*)con;
779 }
780
781 rbConstraint *RB_constraint_new_hinge(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
782 {
783         btRigidBody *body1 = rb1->body;
784         btRigidBody *body2 = rb2->body;
785         btTransform transform1;
786         btTransform transform2;
787         
788         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
789         
790         btHingeConstraint *con = new btHingeConstraint(*body1, *body2, transform1, transform2);
791         
792         return (rbConstraint*)con;
793 }
794
795 rbConstraint *RB_constraint_new_slider(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
796 {
797         btRigidBody *body1 = rb1->body;
798         btRigidBody *body2 = rb2->body;
799         btTransform transform1;
800         btTransform transform2;
801         
802         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
803         
804         btSliderConstraint *con = new btSliderConstraint(*body1, *body2, transform1, transform2, true);
805         
806         return (rbConstraint*)con;
807 }
808
809 rbConstraint *RB_constraint_new_piston(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
810 {
811         btRigidBody *body1 = rb1->body;
812         btRigidBody *body2 = rb2->body;
813         btTransform transform1;
814         btTransform transform2;
815         
816         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
817         
818         btSliderConstraint *con = new btSliderConstraint(*body1, *body2, transform1, transform2, true);
819         con->setUpperAngLimit(-1.0f); // unlock rotation axis
820         
821         return (rbConstraint*)con;
822 }
823
824 rbConstraint *RB_constraint_new_6dof(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
825 {
826         btRigidBody *body1 = rb1->body;
827         btRigidBody *body2 = rb2->body;
828         btTransform transform1;
829         btTransform transform2;
830         
831         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
832         
833         btTypedConstraint *con = new btGeneric6DofConstraint(*body1, *body2, transform1, transform2, true);
834         
835         return (rbConstraint*)con;
836 }
837
838 rbConstraint *RB_constraint_new_6dof_spring(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
839 {
840         btRigidBody *body1 = rb1->body;
841         btRigidBody *body2 = rb2->body;
842         btTransform transform1;
843         btTransform transform2;
844         
845         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
846         
847         btTypedConstraint *con = new btGeneric6DofSpringConstraint(*body1, *body2, transform1, transform2, true);
848         
849         return (rbConstraint*)con;
850 }
851
852 /* Cleanup ----------------------------- */
853
854 void RB_constraint_delete(rbConstraint *con)
855 {
856         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
857         delete constraint;
858 }
859
860 /* Settings ------------------------- */
861
862 void RB_constraint_set_enabled(rbConstraint *con, int enabled)
863 {
864         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
865         
866         constraint->setEnabled(enabled);
867 }
868
869 void RB_constraint_set_limits_hinge(rbConstraint *con, float lower, float upper)
870 {
871         btHingeConstraint *constraint = reinterpret_cast<btHingeConstraint*>(con);
872         
873         // RB_TODO expose these
874         float softness = 0.9f;
875         float bias_factor = 0.3f;
876         float relaxation_factor = 1.0f;
877         
878         constraint->setLimit(lower, upper, softness, bias_factor, relaxation_factor);
879 }
880
881 void RB_constraint_set_limits_slider(rbConstraint *con, float lower, float upper)
882 {
883         btSliderConstraint *constraint = reinterpret_cast<btSliderConstraint*>(con);
884         
885         constraint->setLowerLinLimit(lower);
886         constraint->setUpperLinLimit(upper);
887 }
888
889 void RB_constraint_set_limits_piston(rbConstraint *con, float lin_lower, float lin_upper, float ang_lower, float ang_upper)
890 {
891         btSliderConstraint *constraint = reinterpret_cast<btSliderConstraint*>(con);
892         
893         constraint->setLowerLinLimit(lin_lower);
894         constraint->setUpperLinLimit(lin_upper);
895         constraint->setLowerAngLimit(ang_lower);
896         constraint->setUpperAngLimit(ang_upper);
897 }
898
899 void RB_constraint_set_limits_6dof(rbConstraint *con, float axis, float lower, float upper)
900 {
901         btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint*>(con);
902         
903         constraint->setLimit(axis, lower, upper);
904 }
905
906 void RB_constraint_set_stiffness_6dof_spring(rbConstraint *con, float axis, float stiffness)
907 {
908         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
909         
910         constraint->setStiffness(axis, stiffness);
911 }
912
913 void RB_constraint_set_damping_6dof_spring(rbConstraint *con, float axis, float damping)
914 {
915         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
916         
917         constraint->setDamping(axis, damping);
918 }
919
920 void RB_constraint_set_spring_6dof_spring(rbConstraint *con, float axis, int enable)
921 {
922         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
923         
924         constraint->enableSpring(axis, enable);
925 }
926
927 void RB_constraint_set_equilibrium_6dof_spring(rbConstraint *con)
928 {
929         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
930         
931         constraint->setEquilibriumPoint();
932 }
933
934 void RB_constraint_set_solver_iterations(rbConstraint *con, int num_solver_iterations)
935 {
936         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
937         
938         constraint->setOverrideNumSolverIterations(num_solver_iterations);
939 }
940
941 void RB_constraint_set_breaking_threshold(rbConstraint *con, float threshold)
942 {
943         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
944         
945         constraint->setBreakingImpulseThreshold(threshold);
946 }
947
948 /* ********************************** */