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