31f7f38666390f5cae563dfaa8095c4370d3e899
[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 rbCollisionShape *RB_shape_new_gimpact_mesh(rbMeshData *mesh)
767 {
768         rbCollisionShape *shape = new rbCollisionShape;
769         
770         btGImpactMeshShape *gimpactShape = new btGImpactMeshShape(mesh->index_array);
771         gimpactShape->updateBound(); // TODO: add this to the update collision margin call?
772         
773         shape->cshape = gimpactShape;
774         shape->mesh = mesh;
775         return shape;
776 }
777
778 /* Cleanup --------------------------- */
779
780 void RB_shape_delete(rbCollisionShape *shape)
781 {
782         if (shape->cshape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) {
783                 btBvhTriangleMeshShape *child_shape = ((btScaledBvhTriangleMeshShape *)shape->cshape)->getChildShape();
784                 if (child_shape)
785                         delete child_shape;
786         }
787         if (shape->mesh)
788                 RB_trimesh_data_delete(shape->mesh);
789         delete shape->cshape;
790         delete shape;
791 }
792
793 /* Settings --------------------------- */
794
795 float RB_shape_get_margin(rbCollisionShape *shape)
796 {
797         return shape->cshape->getMargin();
798 }
799
800 void RB_shape_set_margin(rbCollisionShape *shape, float value)
801 {
802         shape->cshape->setMargin(value);
803 }
804
805 /* ********************************** */
806 /* Constraints */
807
808 /* Setup ----------------------------- */
809
810 void RB_dworld_add_constraint(rbDynamicsWorld *world, rbConstraint *con, int disable_collisions)
811 {
812         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
813         
814         world->dynamicsWorld->addConstraint(constraint, disable_collisions);
815 }
816
817 void RB_dworld_remove_constraint(rbDynamicsWorld *world, rbConstraint *con)
818 {
819         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
820         
821         world->dynamicsWorld->removeConstraint(constraint);
822 }
823
824 /* ............ */
825
826 static void make_constraint_transforms(btTransform &transform1, btTransform &transform2, btRigidBody *body1, btRigidBody *body2, float pivot[3], float orn[4])
827 {
828         btTransform pivot_transform = btTransform();
829         pivot_transform.setOrigin(btVector3(pivot[0], pivot[1], pivot[2]));
830         pivot_transform.setRotation(btQuaternion(orn[1], orn[2], orn[3], orn[0]));
831         
832         transform1 = body1->getWorldTransform().inverse() * pivot_transform;
833         transform2 = body2->getWorldTransform().inverse() * pivot_transform;
834 }
835
836 rbConstraint *RB_constraint_new_point(float pivot[3], rbRigidBody *rb1, rbRigidBody *rb2)
837 {
838         btRigidBody *body1 = rb1->body;
839         btRigidBody *body2 = rb2->body;
840         
841         btVector3 pivot1 = body1->getWorldTransform().inverse() * btVector3(pivot[0], pivot[1], pivot[2]);
842         btVector3 pivot2 = body2->getWorldTransform().inverse() * btVector3(pivot[0], pivot[1], pivot[2]);
843         
844         btTypedConstraint *con = new btPoint2PointConstraint(*body1, *body2, pivot1, pivot2);
845         
846         return (rbConstraint *)con;
847 }
848
849 rbConstraint *RB_constraint_new_fixed(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
850 {
851         btRigidBody *body1 = rb1->body;
852         btRigidBody *body2 = rb2->body;
853         btTransform transform1;
854         btTransform transform2;
855         
856         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
857         
858         btFixedConstraint *con = new btFixedConstraint(*body1, *body2, transform1, transform2);
859         
860         return (rbConstraint *)con;
861 }
862
863 rbConstraint *RB_constraint_new_hinge(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
864 {
865         btRigidBody *body1 = rb1->body;
866         btRigidBody *body2 = rb2->body;
867         btTransform transform1;
868         btTransform transform2;
869         
870         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
871         
872         btHingeConstraint *con = new btHingeConstraint(*body1, *body2, transform1, transform2);
873         
874         return (rbConstraint *)con;
875 }
876
877 rbConstraint *RB_constraint_new_slider(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
878 {
879         btRigidBody *body1 = rb1->body;
880         btRigidBody *body2 = rb2->body;
881         btTransform transform1;
882         btTransform transform2;
883         
884         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
885         
886         btSliderConstraint *con = new btSliderConstraint(*body1, *body2, transform1, transform2, true);
887         
888         return (rbConstraint *)con;
889 }
890
891 rbConstraint *RB_constraint_new_piston(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
892 {
893         btRigidBody *body1 = rb1->body;
894         btRigidBody *body2 = rb2->body;
895         btTransform transform1;
896         btTransform transform2;
897         
898         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
899         
900         btSliderConstraint *con = new btSliderConstraint(*body1, *body2, transform1, transform2, true);
901         con->setUpperAngLimit(-1.0f); // unlock rotation axis
902         
903         return (rbConstraint *)con;
904 }
905
906 rbConstraint *RB_constraint_new_6dof(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
907 {
908         btRigidBody *body1 = rb1->body;
909         btRigidBody *body2 = rb2->body;
910         btTransform transform1;
911         btTransform transform2;
912         
913         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
914         
915         btTypedConstraint *con = new btGeneric6DofConstraint(*body1, *body2, transform1, transform2, true);
916         
917         return (rbConstraint *)con;
918 }
919
920 rbConstraint *RB_constraint_new_6dof_spring(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
921 {
922         btRigidBody *body1 = rb1->body;
923         btRigidBody *body2 = rb2->body;
924         btTransform transform1;
925         btTransform transform2;
926         
927         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
928         
929         btTypedConstraint *con = new btGeneric6DofSpringConstraint(*body1, *body2, transform1, transform2, true);
930         
931         return (rbConstraint *)con;
932 }
933
934 rbConstraint *RB_constraint_new_motor(float pivot[3], float orn[4], rbRigidBody *rb1, rbRigidBody *rb2)
935 {
936         btRigidBody *body1 = rb1->body;
937         btRigidBody *body2 = rb2->body;
938         btTransform transform1;
939         btTransform transform2;
940         
941         make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);
942         
943         btGeneric6DofConstraint *con = new btGeneric6DofConstraint(*body1, *body2, transform1, transform2, true);
944         
945         /* unlock constraint axes */
946         for (int i = 0; i < 6; i++) {
947                 con->setLimit(i, 0.0f, -1.0f);
948         }
949         /* unlock motor axes */
950         con->getTranslationalLimitMotor()->m_upperLimit.setValue(-1.0f, -1.0f, -1.0f);
951         
952         return (rbConstraint*)con;
953 }
954
955 /* Cleanup ----------------------------- */
956
957 void RB_constraint_delete(rbConstraint *con)
958 {
959         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
960         delete constraint;
961 }
962
963 /* Settings ------------------------- */
964
965 void RB_constraint_set_enabled(rbConstraint *con, int enabled)
966 {
967         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
968         
969         constraint->setEnabled(enabled);
970 }
971
972 void RB_constraint_set_limits_hinge(rbConstraint *con, float lower, float upper)
973 {
974         btHingeConstraint *constraint = reinterpret_cast<btHingeConstraint*>(con);
975         
976         // RB_TODO expose these
977         float softness = 0.9f;
978         float bias_factor = 0.3f;
979         float relaxation_factor = 1.0f;
980         
981         constraint->setLimit(lower, upper, softness, bias_factor, relaxation_factor);
982 }
983
984 void RB_constraint_set_limits_slider(rbConstraint *con, float lower, float upper)
985 {
986         btSliderConstraint *constraint = reinterpret_cast<btSliderConstraint*>(con);
987         
988         constraint->setLowerLinLimit(lower);
989         constraint->setUpperLinLimit(upper);
990 }
991
992 void RB_constraint_set_limits_piston(rbConstraint *con, float lin_lower, float lin_upper, float ang_lower, float ang_upper)
993 {
994         btSliderConstraint *constraint = reinterpret_cast<btSliderConstraint*>(con);
995         
996         constraint->setLowerLinLimit(lin_lower);
997         constraint->setUpperLinLimit(lin_upper);
998         constraint->setLowerAngLimit(ang_lower);
999         constraint->setUpperAngLimit(ang_upper);
1000 }
1001
1002 void RB_constraint_set_limits_6dof(rbConstraint *con, int axis, float lower, float upper)
1003 {
1004         btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint*>(con);
1005         
1006         constraint->setLimit(axis, lower, upper);
1007 }
1008
1009 void RB_constraint_set_stiffness_6dof_spring(rbConstraint *con, int axis, float stiffness)
1010 {
1011         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
1012         
1013         constraint->setStiffness(axis, stiffness);
1014 }
1015
1016 void RB_constraint_set_damping_6dof_spring(rbConstraint *con, int axis, float damping)
1017 {
1018         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
1019         
1020         // invert damping range so that 0 = no damping
1021         constraint->setDamping(axis, 1.0f - damping);
1022 }
1023
1024 void RB_constraint_set_spring_6dof_spring(rbConstraint *con, int axis, int enable)
1025 {
1026         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
1027         
1028         constraint->enableSpring(axis, enable);
1029 }
1030
1031 void RB_constraint_set_equilibrium_6dof_spring(rbConstraint *con)
1032 {
1033         btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint*>(con);
1034         
1035         constraint->setEquilibriumPoint();
1036 }
1037
1038 void RB_constraint_set_solver_iterations(rbConstraint *con, int num_solver_iterations)
1039 {
1040         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
1041         
1042         constraint->setOverrideNumSolverIterations(num_solver_iterations);
1043 }
1044
1045 void RB_constraint_set_breaking_threshold(rbConstraint *con, float threshold)
1046 {
1047         btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint*>(con);
1048         
1049         constraint->setBreakingImpulseThreshold(threshold);
1050 }
1051
1052 void RB_constraint_set_enable_motor(rbConstraint *con, int enable_lin, int enable_ang)
1053 {
1054         btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint*>(con);
1055         
1056         constraint->getTranslationalLimitMotor()->m_enableMotor[0] = enable_lin;
1057         constraint->getRotationalLimitMotor(0)->m_enableMotor = enable_ang;
1058 }
1059
1060 void RB_constraint_set_max_impulse_motor(rbConstraint *con, float max_impulse_lin, float max_impulse_ang)
1061 {
1062         btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint*>(con);
1063         
1064         constraint->getTranslationalLimitMotor()->m_maxMotorForce.setX(max_impulse_lin);
1065         constraint->getRotationalLimitMotor(0)->m_maxMotorForce = max_impulse_ang;
1066 }
1067
1068 void RB_constraint_set_target_velocity_motor(rbConstraint *con, float velocity_lin, float velocity_ang)
1069 {
1070         btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint*>(con);
1071         
1072         constraint->getTranslationalLimitMotor()->m_targetVelocity.setX(velocity_lin);
1073         constraint->getRotationalLimitMotor(0)->m_targetVelocity = velocity_ang;
1074 }
1075
1076 /* ********************************** */