src/BulletDynamics/Dynamics/Bullet-C-API.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 /*
  17         Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's.
  18         Work in progress, functionality will be added on demand.
  19
  20         If possible, use the richer Bullet C++ API, by including <src/btBulletDynamicsCommon.h>
  21 */
  22
  23 #include "Bullet-C-Api.h"
  24 #include "btBulletDynamicsCommon.h"
  25 #include "LinearMath/btAlignedAllocator.h"
  26
  27
  28
  29 #include "LinearMath/btVector3.h"
  30 #include "LinearMath/btScalar.h"
  31 #include "LinearMath/btMatrix3x3.h"
  32 #include "LinearMath/btTransform.h"
  33 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
  34 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
  35
  36 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
  37 #include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
  38 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
  39 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
  40 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
  41 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
  42 #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
  43 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
  44 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
  45 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
  46
  47
  48 /*
  49         Create and Delete a Physics SDK
  50 */
  51
  52 struct  btPhysicsSdk
  53 {
  54
  55 //      btDispatcher*                           m_dispatcher;
  56 //      btOverlappingPairCache*         m_pairCache;
  57 //      btConstraintSolver*                     m_constraintSolver
  58
  59         btVector3       m_worldAabbMin;
  60         btVector3       m_worldAabbMax;
  61
  62
  63         //todo: version, hardware/optimization settings etc?
  64         btPhysicsSdk()
  65                 :m_worldAabbMin(-1000,-1000,-1000),
  66                 m_worldAabbMax(1000,1000,1000)
  67         {
  68
  69         }
  70
  71
  72 };
  73
  74 plPhysicsSdkHandle      plNewBulletSdk()
  75 {
  76         void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16);
  77         return (plPhysicsSdkHandle)new (mem)btPhysicsSdk;
  78 }
  79
  80 void            plDeletePhysicsSdk(plPhysicsSdkHandle   physicsSdk)
  81 {
  82         btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk);
  83         btAlignedFree(phys);
  84 }
  85
  86
  87 /* Dynamics World */
  88 plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
  89 {
  90         btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
  91         void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16);
  92         btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration();
  93         mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16);
  94         btDispatcher*                           dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration);
  95         mem = btAlignedAlloc(sizeof(btAxisSweep3),16);
  96         btBroadphaseInterface*          pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
  97         mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
  98         btConstraintSolver*                     constraintSolver = new(mem) btSequentialImpulseConstraintSolver();
  99
 100         mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16);
 101         return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration);
 102 }
 103 void           plDeleteDynamicsWorld(plDynamicsWorldHandle world)
 104 {
 105         //todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration
 106         btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
 107         btAlignedFree(dynamicsWorld);
 108 }
 109
 110 void    plStepSimulation(plDynamicsWorldHandle world,   plReal  timeStep)
 111 {
 112         btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
 113         btAssert(dynamicsWorld);
 114         dynamicsWorld->stepSimulation(timeStep);
 115 }
 116
 117 void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
 118 {
 119         btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
 120         btAssert(dynamicsWorld);
 121         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
 122         btAssert(body);
 123
 124         dynamicsWorld->addRigidBody(body);
 125 }
 126
 127 void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
 128 {
 129         btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
 130         btAssert(dynamicsWorld);
 131         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
 132         btAssert(body);
 133
 134         dynamicsWorld->removeRigidBody(body);
 135 }
 136
 137 /* Rigid Body  */
 138
 139 plRigidBodyHandle plCreateRigidBody(    void* user_data,  float mass, plCollisionShapeHandle cshape )
 140 {
 141         btTransform trans;
 142         trans.setIdentity();
 143         btVector3 localInertia(0,0,0);
 144         btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
 145         btAssert(shape);
 146         if (mass)
 147         {
 148                 shape->calculateLocalInertia(mass,localInertia);
 149         }
 150         void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
 151         btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
 152         btRigidBody* body = new (mem)btRigidBody(rbci);
 153         body->setWorldTransform(trans);
 154         body->setUserPointer(user_data);
 155         return (plRigidBodyHandle) body;
 156 }
 157
 158 void plDeleteRigidBody(plRigidBodyHandle cbody)
 159 {
 160         btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
 161         btAssert(body);
 162         btAlignedFree( body);
 163 }
 164
 165
 166 /* Collision Shape definition */
 167
 168 plCollisionShapeHandle plNewSphereShape(plReal radius)
 169 {
 170         void* mem = btAlignedAlloc(sizeof(btSphereShape),16);
 171         return (plCollisionShapeHandle) new (mem)btSphereShape(radius);
 172
 173 }
 174
 175 plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
 176 {
 177         void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
 178         return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
 179 }
 180
 181 plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height)
 182 {
 183         //capsule is convex hull of 2 spheres, so use btMultiSphereShape
 184
 185         const int numSpheres = 2;
 186         btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
 187         btScalar radi[numSpheres] = {radius,radius};
 188         void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
 189         return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres);
 190 }
 191 plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
 192 {
 193         void* mem = btAlignedAlloc(sizeof(btConeShape),16);
 194         return (plCollisionShapeHandle) new (mem)btConeShape(radius,height);
 195 }
 196
 197 plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
 198 {
 199         void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
 200         return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
 201 }
 202
 203 /* Convex Meshes */
 204 plCollisionShapeHandle plNewConvexHullShape()
 205 {
 206         void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
 207         return (plCollisionShapeHandle) new (mem)btConvexHullShape();
 208 }
 209
 210
 211 /* Concave static triangle meshes */
 212 plMeshInterfaceHandle              plNewMeshInterface()
 213 {
 214         return 0;
 215 }
 216
 217 plCollisionShapeHandle plNewCompoundShape()
 218 {
 219         void* mem = btAlignedAlloc(sizeof(btCompoundShape),16);
 220         return (plCollisionShapeHandle) new (mem)btCompoundShape();
 221 }
 222
 223 void    plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn)
 224 {
 225         btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle);
 226         btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE);
 227         btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape);
 228         btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle);
 229         btTransform     localTrans;
 230         localTrans.setIdentity();
 231         localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2]));
 232         localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3]));
 233         compoundShape->addChildShape(localTrans,childShape);
 234 }
 235
 236 void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient)
 237 {
 238         btQuaternion orn;
 239         orn.setEuler(yaw,pitch,roll);
 240         orient[0] = orn.getX();
 241         orient[1] = orn.getY();
 242         orient[2] = orn.getZ();
 243         orient[3] = orn.getW();
 244
 245 }
 246
 247
 248 //      extern  void            plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
 249 //      extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
 250
 251
 252 void            plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z)
 253 {
 254         btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape);
 255         (void)colShape;
 256         btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
 257         btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
 258         convexHullShape->addPoint(btVector3(x,y,z));
 259
 260 }
 261
 262 void plDeleteShape(plCollisionShapeHandle cshape)
 263 {
 264         btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
 265         btAssert(shape);
 266         btAlignedFree(shape);
 267 }
 268 void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
 269 {
 270         btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
 271         btAssert(shape);
 272         btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
 273         shape->setLocalScaling(scaling);
 274 }
 275
 276
 277
 278 void plSetPosition(plRigidBodyHandle object, const plVector3 position)
 279 {
 280         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
 281         btAssert(body);
 282         btVector3 pos(position[0],position[1],position[2]);
 283         btTransform worldTrans = body->getWorldTransform();
 284         worldTrans.setOrigin(pos);
 285         body->setWorldTransform(worldTrans);
 286 }
 287
 288 void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
 289 {
 290         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
 291         btAssert(body);
 292         btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
 293         btTransform worldTrans = body->getWorldTransform();
 294         worldTrans.setRotation(orn);
 295         body->setWorldTransform(worldTrans);
 296 }
 297
 298 void    plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
 299 {
 300         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
 301         btAssert(body);
 302         btTransform& worldTrans = body->getWorldTransform();
 303         worldTrans.setFromOpenGLMatrix(matrix);
 304 }
 305
 306 void    plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
 307 {
 308         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
 309         btAssert(body);
 310         body->getWorldTransform().getOpenGLMatrix(matrix);
 311
 312 }
 313
 314 void    plGetPosition(plRigidBodyHandle object,plVector3 position)
 315 {
 316         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
 317         btAssert(body);
 318         const btVector3& pos = body->getWorldTransform().getOrigin();
 319         position[0] = pos.getX();
 320         position[1] = pos.getY();
 321         position[2] = pos.getZ();
 322 }
 323
 324 void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
 325 {
 326         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
 327         btAssert(body);
 328         const btQuaternion& orn = body->getWorldTransform().getRotation();
 329         orientation[0] = orn.getX();
 330         orientation[1] = orn.getY();
 331         orientation[2] = orn.getZ();
 332         orientation[3] = orn.getW();
 333 }
 334
 335
 336
 337 //plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
 338
 339 //      extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
 340
 341 double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
 342 {
 343         btVector3 vp(p1[0], p1[1], p1[2]);
 344         btTriangleShape trishapeA(vp,
 345                                   btVector3(p2[0], p2[1], p2[2]),
 346                                   btVector3(p3[0], p3[1], p3[2]));
 347         trishapeA.setMargin(0.000001f);
 348         btVector3 vq(q1[0], q1[1], q1[2]);
 349         btTriangleShape trishapeB(vq,
 350                                   btVector3(q2[0], q2[1], q2[2]),
 351                                   btVector3(q3[0], q3[1], q3[2]));
 352         trishapeB.setMargin(0.000001f);
 353
 354         // btVoronoiSimplexSolver sGjkSimplexSolver;
 355         // btGjkEpaPenetrationDepthSolver penSolverPtr;
 356
 357         static btSimplexSolverInterface sGjkSimplexSolver;
 358         sGjkSimplexSolver.reset();
 359
 360         static btGjkEpaPenetrationDepthSolver Solver0;
 361         static btMinkowskiPenetrationDepthSolver Solver1;
 362
 363         btConvexPenetrationDepthSolver* Solver = NULL;
 364
 365         Solver = &Solver1;
 366
 367         btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
 368
 369         convexConvex.m_catchDegeneracies = 1;
 370
 371         // btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
 372
 373         btPointCollector gjkOutput;
 374         btGjkPairDetector::ClosestPointInput input;
 375
 376
 377         btTransform tr;
 378         tr.setIdentity();
 379
 380         input.m_transformA = tr;
 381         input.m_transformB = tr;
 382
 383         convexConvex.getClosestPoints(input, gjkOutput, 0);
 384
 385
 386         if (gjkOutput.m_hasResult)
 387         {
 388
 389                 pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
 390                 pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
 391                 pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
 392
 393                 pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
 394                 pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
 395                 pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
 396
 397                 normal[0] = gjkOutput.m_normalOnBInWorld[0];
 398                 normal[1] = gjkOutput.m_normalOnBInWorld[1];
 399                 normal[2] = gjkOutput.m_normalOnBInWorld[2];
 400
 401                 return gjkOutput.m_distance;
 402         }
 403         return -1.0f;
 404 }
 405