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