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 /* ********************************** */