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