Rigidbody: Allow triangle mesh shapes to deform during simulation
[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(rbDynamicsWorld *world, rbRigidBody *object, const float loc_start[3], const float loc_end[3], float v_location[3],  float v_hitpoint[3],  float v_normal[3], int *r_hit)
269 {
270         btRigidBody *body = object->body;
271         btCollisionShape *collisionShape = body->getCollisionShape();
272         /* only convex shapes are supported, but user can specify a non convex shape */
273         if (collisionShape->isConvex()) {
274                 btCollisionWorld::ClosestConvexResultCallback result(btVector3(loc_start[0], loc_start[1], loc_start[2]), btVector3(loc_end[0], loc_end[1], loc_end[2]));
275
276                 btQuaternion obRot = body->getWorldTransform().getRotation();
277                 
278                 btTransform rayFromTrans;
279                 rayFromTrans.setIdentity();
280                 rayFromTrans.setRotation(obRot);
281                 rayFromTrans.setOrigin(btVector3(loc_start[0], loc_start[1], loc_start[2]));
282
283                 btTransform rayToTrans;
284                 rayToTrans.setIdentity();
285                 rayToTrans.setRotation(obRot);
286                 rayToTrans.setOrigin(btVector3(loc_end[0], loc_end[1], loc_end[2]));
287                 
288                 world->dynamicsWorld->convexSweepTest((btConvexShape*) collisionShape, rayFromTrans, rayToTrans, result, 0);
289                 
290                 if (result.hasHit()) {
291                         *r_hit = 1;
292                         
293                         v_location[0] = result.m_convexFromWorld[0]+(result.m_convexToWorld[0]-result.m_convexFromWorld[0])*result.m_closestHitFraction;
294                         v_location[1] = result.m_convexFromWorld[1]+(result.m_convexToWorld[1]-result.m_convexFromWorld[1])*result.m_closestHitFraction;
295                         v_location[2] = result.m_convexFromWorld[2]+(result.m_convexToWorld[2]-result.m_convexFromWorld[2])*result.m_closestHitFraction;
296                         
297                         v_hitpoint[0] = result.m_hitPointWorld[0];
298                         v_hitpoint[1] = result.m_hitPointWorld[1];
299                         v_hitpoint[2] = result.m_hitPointWorld[2];
300                         
301                         v_normal[0] = result.m_hitNormalWorld[0];
302                         v_normal[1] = result.m_hitNormalWorld[1];
303                         v_normal[2] = result.m_hitNormalWorld[2];
304                         
305                 }
306                 else {
307                         *r_hit = 0;
308                 }
309         }
310         else{
311                 /* we need to return a value if user passes non convex body, to report */
312                 *r_hit = -2;
313         }
314 }
315
316 /* ............ */
317
318 rbRigidBody *RB_body_new(rbCollisionShape *shape, const float loc[3], const float rot[4])
319 {
320         rbRigidBody *object = new rbRigidBody;
321         /* current transform */
322         btTransform trans;
323         trans.setOrigin(btVector3(loc[0], loc[1], loc[2]));
324         trans.setRotation(btQuaternion(rot[1], rot[2], rot[3], rot[0]));
325         
326         /* create motionstate, which is necessary for interpolation (includes reverse playback) */
327         btDefaultMotionState *motionState = new btDefaultMotionState(trans);
328         
329         /* make rigidbody */
330         btRigidBody::btRigidBodyConstructionInfo rbInfo(1.0f, motionState, shape->cshape);
331         
332         object->body = new btRigidBody(rbInfo);
333         
334         object->body->setUserPointer(object);
335         
336         return object;
337 }
338
339 void RB_body_delete(rbRigidBody *object)
340 {
341         btRigidBody *body = object->body;
342         
343         /* motion state */
344         btMotionState *ms = body->getMotionState();
345         if (ms)
346                 delete ms;
347         
348         /* collision shape is done elsewhere... */
349         
350         /* body itself */
351         
352         /* manually remove constraint refs of the rigid body, normally this happens when removing constraints from the world
353          * but since we delete everything when the world is rebult, we need to do it manually here */
354         for (int i = body->getNumConstraintRefs() - 1; i >= 0; i--) {
355                 btTypedConstraint *con = body->getConstraintRef(i);
356                 body->removeConstraintRef(con);
357         }
358         
359         delete body;
360         delete object;
361 }
362
363 /* Settings ------------------------- */
364
365 void RB_body_set_collision_shape(rbRigidBody *object, rbCollisionShape *shape)
366 {
367         btRigidBody *body = object->body;
368         
369         /* set new collision shape */
370         body->setCollisionShape(shape->cshape);
371         
372         /* recalculate inertia, since that depends on the collision shape... */
373         RB_body_set_mass(object, RB_body_get_mass(object));
374 }
375
376 /* ............ */
377
378 float RB_body_get_mass(rbRigidBody *object)
379 {
380         btRigidBody *body = object->body;
381         
382         /* there isn't really a mass setting, but rather 'inverse mass'  
383          * which we convert back to mass by taking the reciprocal again 
384          */
385         float value = (float)body->getInvMass();
386         
387         if (value)
388                 value = 1.0 / value;
389                 
390         return value;
391 }
392
393 void RB_body_set_mass(rbRigidBody *object, float value)
394 {
395         btRigidBody *body = object->body;
396         btVector3 localInertia(0, 0, 0);
397         
398         /* calculate new inertia if non-zero mass */
399         if (value) {
400                 btCollisionShape *shape = body->getCollisionShape();
401                 shape->calculateLocalInertia(value, localInertia);
402         }
403         
404         body->setMassProps(value, localInertia);
405         body->updateInertiaTensor();
406 }
407
408
409 float RB_body_get_friction(rbRigidBody *object)
410 {
411         btRigidBody *body = object->body;
412         return body->getFriction();
413 }
414
415 void RB_body_set_friction(rbRigidBody *object, float value)
416 {
417         btRigidBody *body = object->body;
418         body->setFriction(value);
419 }
420
421
422 float RB_body_get_restitution(rbRigidBody *object)
423 {
424         btRigidBody *body = object->body;
425         return body->getRestitution();
426 }
427
428 void RB_body_set_restitution(rbRigidBody *object, float value)
429 {
430         btRigidBody *body = object->body;
431         body->setRestitution(value);
432 }
433
434
435 float RB_body_get_linear_damping(rbRigidBody *object)
436 {
437         btRigidBody *body = object->body;
438         return body->getLinearDamping();
439 }
440
441 void RB_body_set_linear_damping(rbRigidBody *object, float value)
442 {
443         RB_body_set_damping(object, value, RB_body_get_linear_damping(object));
444 }
445
446 float RB_body_get_angular_damping(rbRigidBody *object)
447 {
448         btRigidBody *body = object->body;
449         return body->getAngularDamping();
450 }
451
452 void RB_body_set_angular_damping(rbRigidBody *object, float value)
453 {
454         RB_body_set_damping(object, RB_body_get_linear_damping(object), value);
455 }
456
457 void RB_body_set_damping(rbRigidBody *object, float linear, float angular)
458 {
459         btRigidBody *body = object->body;
460         body->setDamping(linear, angular);
461 }
462
463
464 float RB_body_get_linear_sleep_thresh(rbRigidBody *object)
465 {
466         btRigidBody *body = object->body;
467         return body->getLinearSleepingThreshold();
468 }
469
470 void RB_body_set_linear_sleep_thresh(rbRigidBody *object, float value)
471 {
472         RB_body_set_sleep_thresh(object, value, RB_body_get_angular_sleep_thresh(object));
473 }
474
475 float RB_body_get_angular_sleep_thresh(rbRigidBody *object)
476 {
477         btRigidBody *body = object->body;
478         return body->getAngularSleepingThreshold();
479 }
480
481 void RB_body_set_angular_sleep_thresh(rbRigidBody *object, float value)
482 {
483         RB_body_set_sleep_thresh(object, RB_body_get_linear_sleep_thresh(object), value);
484 }
485
486 void RB_body_set_sleep_thresh(rbRigidBody *object, float linear, float angular)
487 {
488         btRigidBody *body = object->body;
489         body->setSleepingThresholds(linear, angular);
490 }
491
492 /* ............ */
493
494 void RB_body_get_linear_velocity(rbRigidBody *object, float v_out[3])
495 {
496         btRigidBody *body = object->body;
497         
498         copy_v3_btvec3(v_out, body->getLinearVelocity());
499 }
500
501 void RB_body_set_linear_velocity(rbRigidBody *object, const float v_in[3])
502 {
503         btRigidBody *body = object->body;
504         
505         body->setLinearVelocity(btVector3(v_in[0], v_in[1], v_in[2]));
506 }
507
508
509 void RB_body_get_angular_velocity(rbRigidBody *object, float v_out[3])
510 {
511         btRigidBody *body = object->body;
512         
513         copy_v3_btvec3(v_out, body->getAngularVelocity());
514 }
515
516 void RB_body_set_angular_velocity(rbRigidBody *object, const float v_in[3])
517 {
518         btRigidBody *body = object->body;
519         
520         body->setAngularVelocity(btVector3(v_in[0], v_in[1], v_in[2]));
521 }
522
523 void RB_body_set_linear_factor(rbRigidBody *object, float x, float y, float z)
524 {
525         btRigidBody *body = object->body;
526         body->setLinearFactor(btVector3(x, y, z));
527 }
528
529 void RB_body_set_angular_factor(rbRigidBody *object, float x, float y, float z)
530 {
531         btRigidBody *body = object->body;
532         body->setAngularFactor(btVector3(x, y, z));
533 }
534
535 /* ............ */
536
537 void RB_body_set_kinematic_state(rbRigidBody *object, int kinematic)
538 {
539         btRigidBody *body = object->body;
540         if (kinematic)
541                 body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
542         else
543                 body->setCollisionFlags(body->getCollisionFlags() & ~btCollisionObject::CF_KINEMATIC_OBJECT);
544 }
545
546 /* ............ */
547
548 void RB_body_set_activation_state(rbRigidBody *object, int use_deactivation)
549 {
550         btRigidBody *body = object->body;
551         if (use_deactivation)
552                 body->forceActivationState(ACTIVE_TAG);
553         else
554                 body->setActivationState(DISABLE_DEACTIVATION);
555 }
556 void RB_body_activate(rbRigidBody *object)
557 {
558         btRigidBody *body = object->body;
559         body->setActivationState(ACTIVE_TAG);
560 }
561 void RB_body_deactivate(rbRigidBody *object)
562 {
563         btRigidBody *body = object->body;
564         body->setActivationState(ISLAND_SLEEPING);
565 }
566
567 /* ............ */
568
569
570
571 /* Simulation ----------------------- */
572
573 /* The transform matrices Blender uses are OpenGL-style matrices, 
574  * while Bullet uses the Right-Handed coordinate system style instead.
575  */
576
577 void RB_body_get_transform_matrix(rbRigidBody *object, float m_out[4][4])
578 {
579         btRigidBody *body = object->body;
580         btMotionState *ms = body->getMotionState();
581         
582         btTransform trans;
583         ms->getWorldTransform(trans);
584         
585         trans.getOpenGLMatrix((btScalar *)m_out);
586 }
587
588 void RB_body_set_loc_rot(rbRigidBody *object, const float loc[3], const float rot[4])
589 {
590         btRigidBody *body = object->body;
591         btMotionState *ms = body->getMotionState();
592         
593         /* set transform matrix */
594         btTransform trans;
595         trans.setOrigin(btVector3(loc[0], loc[1], loc[2]));
596         trans.setRotation(btQuaternion(rot[1], rot[2], rot[3], rot[0]));
597         
598         ms->setWorldTransform(trans);
599 }
600
601 void RB_body_set_scale(rbRigidBody *object, const float scale[3])
602 {
603         btRigidBody *body = object->body;
604         
605         /* apply scaling factor from matrix above to the collision shape */
606         btCollisionShape *cshape = body->getCollisionShape();
607         if (cshape) {
608                 cshape->setLocalScaling(btVector3(scale[0], scale[1], scale[2]));
609                 
610                 /* GIimpact shapes have to be updated to take scaling into account */
611                 if (cshape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
612                         ((btGImpactMeshShape *)cshape)->updateBound();
613         }
614 }
615
616 /* ............ */
617 /* Read-only state info about status of simulation */
618
619 void RB_body_get_position(rbRigidBody *object, float v_out[3])
620 {
621         btRigidBody *body = object->body;
622         
623         copy_v3_btvec3(v_out, body->getWorldTransform().getOrigin());
624 }
625
626 void RB_body_get_orientation(rbRigidBody *object, float v_out[4])
627 {
628         btRigidBody *body = object->body;
629         
630         copy_quat_btquat(v_out, body->getWorldTransform().getRotation());
631 }
632
633 /* ............ */
634 /* Overrides for simulation */
635
636 void RB_body_apply_central_force(rbRigidBody *object, const float v_in[3])
637 {
638         btRigidBody *body = object->body;
639         
640         body->applyCentralForce(btVector3(v_in[0], v_in[1], v_in[2]));
641 }
642
643 /* ********************************** */
644 /* Collision Shape Methods */
645
646 /* Setup (Standard Shapes) ----------- */
647
648 rbCollisionShape *RB_shape_new_box(float x, float y, float z)
649 {
650         rbCollisionShape *shape = new rbCollisionShape;
651         shape->cshape = new btBoxShape(btVector3(x, y, z));
652         shape->mesh = NULL;
653         return shape;
654 }
655
656 rbCollisionShape *RB_shape_new_sphere(float radius)
657 {
658         rbCollisionShape *shape = new rbCollisionShape;
659         shape->cshape = new btSphereShape(radius);
660         shape->mesh = NULL;
661         return shape;
662 }
663
664 rbCollisionShape *RB_shape_new_capsule(float radius, float height)
665 {
666         rbCollisionShape *shape = new rbCollisionShape;
667         shape->cshape = new btCapsuleShapeZ(radius, height);
668         shape->mesh = NULL;
669         return shape;
670 }
671
672 rbCollisionShape *RB_shape_new_cone(float radius, float height)
673 {
674         rbCollisionShape *shape = new rbCollisionShape;
675         shape->cshape = new btConeShapeZ(radius, height);
676         shape->mesh = NULL;
677         return shape;
678 }
679
680 rbCollisionShape *RB_shape_new_cylinder(float radius, float height)
681 {
682         rbCollisionShape *shape = new rbCollisionShape;
683         shape->cshape = new btCylinderShapeZ(btVector3(radius, radius, height));
684         shape->mesh = NULL;
685         return shape;
686 }
687
688 /* Setup (Convex Hull) ------------ */
689
690 rbCollisionShape *RB_shape_new_convex_hull(float *verts, int stride, int count, float margin, bool *can_embed)
691 {
692         btConvexHullComputer hull_computer = btConvexHullComputer();
693         
694         // try to embed the margin, if that fails don't shrink the hull
695         if (hull_computer.compute(verts, stride, count, margin, 0.0f) < 0.0f) {
696                 hull_computer.compute(verts, stride, count, 0.0f, 0.0f);
697                 *can_embed = false;
698         }
699         
700         rbCollisionShape *shape = new rbCollisionShape;
701         btConvexHullShape *hull_shape = new btConvexHullShape(&(hull_computer.vertices[0].getX()), hull_computer.vertices.size());
702         
703         shape->cshape = hull_shape;
704         shape->mesh = NULL;
705         return shape;
706 }
707
708 /* Setup (Triangle Mesh) ---------- */
709
710 /* Need to call RB_trimesh_finish() after creating triangle mesh and adding vertices and triangles */
711
712 rbMeshData *RB_trimesh_data_new(int num_tris, int num_verts)
713 {
714         rbMeshData *mesh = new rbMeshData;
715         mesh->vertices = new rbVert[num_verts];
716         mesh->triangles = new rbTri[num_tris];
717         mesh->num_vertices = num_verts;
718         mesh->num_triangles = num_tris;
719         
720         return mesh;
721 }
722
723 static void RB_trimesh_data_delete(rbMeshData *mesh)
724 {
725         delete mesh->index_array;
726         delete mesh->vertices;
727         delete mesh->triangles;
728         delete mesh;
729 }
730  
731 void RB_trimesh_add_vertices(rbMeshData *mesh, float *vertices, int num_verts, int vert_stride)
732 {
733         for (int i = 0; i < num_verts; i++) {
734                 float *vert = (float*)(((char*)vertices + i * vert_stride));
735                 mesh->vertices[i].x = vert[0];
736                 mesh->vertices[i].y = vert[1];
737                 mesh->vertices[i].z = vert[2];
738         }
739 }
740 void RB_trimesh_add_triangle_indices(rbMeshData *mesh, int num, int index0, int index1, int index2)
741 {
742         mesh->triangles[num].v0 = index0;
743         mesh->triangles[num].v1 = index1;
744         mesh->triangles[num].v2 = index2;
745 }
746
747 void RB_trimesh_finish(rbMeshData *mesh)
748 {
749         mesh->index_array = new btTriangleIndexVertexArray(mesh->num_triangles, (int*)mesh->triangles, sizeof(rbTri),
750                                                            mesh->num_vertices, (float*)mesh->vertices, sizeof(rbVert));
751 }
752  
753 rbCollisionShape *RB_shape_new_trimesh(rbMeshData *mesh)
754 {
755         rbCollisionShape *shape = new rbCollisionShape;
756         
757         /* triangle-mesh we create is a BVH wrapper for triangle mesh data (for faster lookups) */
758         // RB_TODO perhaps we need to allow saving out this for performance when rebuilding?
759         btBvhTriangleMeshShape *unscaledShape = new btBvhTriangleMeshShape(mesh->index_array, true, true);
760         
761         shape->cshape = new btScaledBvhTriangleMeshShape(unscaledShape, btVector3(1.0f, 1.0f, 1.0f));
762         shape->mesh = mesh;
763         return shape;
764 }
765
766 void RB_shape_trimesh_update(rbCollisionShape *shape, float *vertices, int num_verts, int vert_stride, float min[3], float max[3])
767 {
768         if (shape->mesh == NULL || num_verts != shape->mesh->num_vertices)
769                 return;
770         
771         for (int i = 0; i < num_verts; i++) {
772                 float *vert = (float*)(((char*)vertices + i * vert_stride));
773                 shape->mesh->vertices[i].x = vert[0];
774                 shape->mesh->vertices[i].y = vert[1];
775                 shape->mesh->vertices[i].z = vert[2];
776         }
777         
778         if (shape->cshape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) {
779                 btScaledBvhTriangleMeshShape *scaled_shape = (btScaledBvhTriangleMeshShape *)shape->cshape;
780                 btBvhTriangleMeshShape *mesh_shape = scaled_shape->getChildShape();
781                 mesh_shape->refitTree(btVector3(min[0], min[1], min[2]), btVector3(max[0], max[1], max[2]));
782         }
783         else if (shape->cshape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) {
784                 btGImpactMeshShape *mesh_shape = (btGImpactMeshShape*)shape->cshape;
785                 mesh_shape->updateBound();
786         }
787 }
788
789 rbCollisionShape *RB_shape_new_gimpact_mesh(rbMeshData *mesh)
790 {
791         rbCollisionShape *shape = new rbCollisionShape;
792         
793         btGImpactMeshShape *gimpactShape = new btGImpactMeshShape(mesh->index_array);
794         gimpactShape->updateBound(); // TODO: add this to the update collision margin call?
795         
796         shape->cshape = gimpactShape;
797         shape->mesh = mesh;
798         return shape;
799 }
800
801 /* Cleanup --------------------------- */
802
803 void RB_shape_delete(rbCollisionShape *shape)
804 {
805         if (shape->cshape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) {
806                 btBvhTriangleMeshShape *child_shape = ((btScaledBvhTriangleMeshShape *)shape->cshape)->getChildShape();
807                 if (child_shape)
808                         delete child_shape;
809         }
810         if (shape->mesh)
811                 RB_trimesh_data_delete(shape->mesh);
812         delete shape->cshape;
813         delete shape;
814 }
815
816 /* Settings --------------------------- */
817
818 float RB_shape_get_margin(rbCollisionShape *shape)
819 {
820         return shape->cshape->getMargin();
821 }
822
823 void RB_shape_set_margin(rbCollisionShape *shape, float value)
824 {
825         shape->cshape->setMargin(value);
826 }
827
828 /* ********************************** */
829 /* Constraints */
830
831 /* Setup ----------------------------- */
832
833 void RB_dworld_add_constraint(rbDynamicsWorld *world, rbConstraint *con, int disable_collisions)
834 {
835         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
836         
837         world->dynamicsWorld->addConstraint(constraint, disable_collisions);
838 }
839
840 void RB_dworld_remove_constraint(rbDynamicsWorld *world, rbConstraint *con)
841 {
842         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
843         
844         world->dynamicsWorld->removeConstraint(constraint);
845 }
846
847 /* ............ */
848
849 static void make_constraint_transforms(btTransform &transform1, btTransform &transform2, btRigidBody *body1, btRigidBody *body2, float pivot[3], float orn[4])
850 {
851         btTransform pivot_transform = btTransform();
852         pivot_transform.setOrigin(btVector3(pivot[0], pivot[1], pivot[2]));
853         pivot_transform.setRotation(btQuaternion(orn[1], orn[2], orn[3], orn[0]));
854         
855         transform1 = body1->getWorldTransform().inverse() * pivot_transform;
856         transform2 = body2->getWorldTransform().inverse() * pivot_transform;
857 }
858
859 rbConstraint *RB_constraint_new_point(float pivot[3], rbRigidBody *rb1, rbRigidBody *rb2)
860 {
861         btRigidBody *body1 = rb1->body;
862         btRigidBody *body2 = rb2->body;
863         
864         btVector3 pivot1 = body1->getWorldTransform().inverse() * btVector3(pivot[0], pivot[1], pivot[2]);
865         btVector3 pivot2 = body2->getWorldTransform().inverse() * btVector3(pivot[0], pivot[1], pivot[2]);
866         
867         btTypedConstraint *con = new btPoint2PointConstraint(*body1, *body2, pivot1, pivot2);
868         
869         return (rbConstraint *)con;
870 }
871
872 rbConstraint *RB_constraint_new_fixed(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
873 {
874         btRigidBody *body1 = rb1->body;
875         btRigidBody *body2 = rb2->body;
876         btTransform transform1;
877         btTransform transform2;
878         
879         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
880         
881         btFixedConstraint *con = new btFixedConstraint(*body1, *body2, transform1, transform2);
882         
883         return (rbConstraint *)con;
884 }
885
886 rbConstraint *RB_constraint_new_hinge(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
887 {
888         btRigidBody *body1 = rb1->body;
889         btRigidBody *body2 = rb2->body;
890         btTransform transform1;
891         btTransform transform2;
892         
893         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
894         
895         btHingeConstraint *con = new btHingeConstraint(*body1, *body2, transform1, transform2);
896         
897         return (rbConstraint *)con;
898 }
899
900 rbConstraint *RB_constraint_new_slider(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
901 {
902         btRigidBody *body1 = rb1->body;
903         btRigidBody *body2 = rb2->body;
904         btTransform transform1;
905         btTransform transform2;
906         
907         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
908         
909         btSliderConstraint *con = new btSliderConstraint(*body1, *body2, transform1, transform2, true);
910         
911         return (rbConstraint *)con;
912 }
913
914 rbConstraint *RB_constraint_new_piston(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
915 {
916         btRigidBody *body1 = rb1->body;
917         btRigidBody *body2 = rb2->body;
918         btTransform transform1;
919         btTransform transform2;
920         
921         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
922         
923         btSliderConstraint *con = new btSliderConstraint(*body1, *body2, transform1, transform2, true);
924         con->setUpperAngLimit(-1.0f); // unlock rotation axis
925         
926         return (rbConstraint *)con;
927 }
928
929 rbConstraint *RB_constraint_new_6dof(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
930 {
931         btRigidBody *body1 = rb1->body;
932         btRigidBody *body2 = rb2->body;
933         btTransform transform1;
934         btTransform transform2;
935         
936         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
937         
938         btTypedConstraint *con = new btGeneric6DofConstraint(*body1, *body2, transform1, transform2, true);
939         
940         return (rbConstraint *)con;
941 }
942
943 rbConstraint *RB_constraint_new_6dof_spring(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
944 {
945         btRigidBody *body1 = rb1->body;
946         btRigidBody *body2 = rb2->body;
947         btTransform transform1;
948         btTransform transform2;
949         
950         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
951         
952         btTypedConstraint *con = new btGeneric6DofSpringConstraint(*body1, *body2, transform1, transform2, true);
953         
954         return (rbConstraint *)con;
955 }
956
957 rbConstraint *RB_constraint_new_motor(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
958 {
959         btRigidBody *body1 = rb1->body;
960         btRigidBody *body2 = rb2->body;
961         btTransform transform1;
962         btTransform transform2;
963         
964         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
965         
966         btGeneric6DofConstraint *con = new btGeneric6DofConstraint(*body1, *body2, transform1, transform2, true);
967         
968         /* unlock constraint axes */
969         for (int i = 0; i < 6; i++) {
970                 con->setLimit(i, 0.0f, -1.0f);
971         }
972         /* unlock motor axes */
973         con->getTranslationalLimitMotor()->m_upperLimit.setValue(-1.0f, -1.0f, -1.0f);
974         
975         return (rbConstraint*)con;
976 }
977
978 /* Cleanup ----------------------------- */
979
980 void RB_constraint_delete(rbConstraint *con)
981 {
982         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
983         delete constraint;
984 }
985
986 /* Settings ------------------------- */
987
988 void RB_constraint_set_enabled(rbConstraint *con, int enabled)
989 {
990         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
991         
992         constraint->setEnabled(enabled);
993 }
994
995 void RB_constraint_set_limits_hinge(rbConstraint *con, float lower, float upper)
996 {
997         btHingeConstraint *constraint = reinterpret_cast<btHingeConstraint*>(con);
998         
999         // RB_TODO expose these
1000         float softness = 0.9f;
1001         float bias_factor = 0.3f;
1002         float relaxation_factor = 1.0f;
1003         
1004         constraint->setLimit(lower, upper, softness, bias_factor, relaxation_factor);
1005 }
1006
1007 void RB_constraint_set_limits_slider(rbConstraint *con, float lower, float upper)
1008 {
1009         btSliderConstraint *constraint = reinterpret_cast<btSliderConstraint*>(con);
1010         
1011         constraint->setLowerLinLimit(lower);
1012         constraint->setUpperLinLimit(upper);
1013 }
1014
1015 void RB_constraint_set_limits_piston(rbConstraint *con, float lin_lower, float lin_upper, float ang_lower, float ang_upper)
1016 {
1017         btSliderConstraint *constraint = reinterpret_cast<btSliderConstraint*>(con);
1018         
1019         constraint->setLowerLinLimit(lin_lower);
1020         constraint->setUpperLinLimit(lin_upper);
1021         constraint->setLowerAngLimit(ang_lower);
1022         constraint->setUpperAngLimit(ang_upper);
1023 }
1024
1025 void RB_constraint_set_limits_6dof(rbConstraint *con, int axis, float lower, float upper)
1026 {
1027         btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint*>(con);
1028         
1029         constraint->setLimit(axis, lower, upper);
1030 }
1031
1032 void RB_constraint_set_stiffness_6dof_spring(rbConstraint *con, int axis, float stiffness)
1033 {
1034         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
1035         
1036         constraint->setStiffness(axis, stiffness);
1037 }
1038
1039 void RB_constraint_set_damping_6dof_spring(rbConstraint *con, int axis, float damping)
1040 {
1041         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
1042         
1043         // invert damping range so that 0 = no damping
1044         constraint->setDamping(axis, 1.0f - damping);
1045 }
1046
1047 void RB_constraint_set_spring_6dof_spring(rbConstraint *con, int axis, int enable)
1048 {
1049         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
1050         
1051         constraint->enableSpring(axis, enable);
1052 }
1053
1054 void RB_constraint_set_equilibrium_6dof_spring(rbConstraint *con)
1055 {
1056         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
1057         
1058         constraint->setEquilibriumPoint();
1059 }
1060
1061 void RB_constraint_set_solver_iterations(rbConstraint *con, int num_solver_iterations)
1062 {
1063         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
1064         
1065         constraint->setOverrideNumSolverIterations(num_solver_iterations);
1066 }
1067
1068 void RB_constraint_set_breaking_threshold(rbConstraint *con, float threshold)
1069 {
1070         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
1071         
1072         constraint->setBreakingImpulseThreshold(threshold);
1073 }
1074
1075 void RB_constraint_set_enable_motor(rbConstraint *con, int enable_lin, int enable_ang)
1076 {
1077         btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint*>(con);
1078         
1079         constraint->getTranslationalLimitMotor()->m_enableMotor[0] = enable_lin;
1080         constraint->getRotationalLimitMotor(0)->m_enableMotor = enable_ang;
1081 }
1082
1083 void RB_constraint_set_max_impulse_motor(rbConstraint *con, float max_impulse_lin, float max_impulse_ang)
1084 {
1085         btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint*>(con);
1086         
1087         constraint->getTranslationalLimitMotor()->m_maxMotorForce.setX(max_impulse_lin);
1088         constraint->getRotationalLimitMotor(0)->m_maxMotorForce = max_impulse_ang;
1089 }
1090
1091 void RB_constraint_set_target_velocity_motor(rbConstraint *con, float velocity_lin, float velocity_ang)
1092 {
1093         btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint*>(con);
1094         
1095         constraint->getTranslationalLimitMotor()->m_targetVelocity.setX(velocity_lin);
1096         constraint->getRotationalLimitMotor(0)->m_targetVelocity = velocity_ang;
1097 }
1098
1099 /* ********************************** */