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