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