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