Bullet-C-API.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 /*
00017         Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's.
00018         Work in progress, functionality will be added on demand.
00019 
00020         If possible, use the richer Bullet C++ API, by including <src/btBulletDynamicsCommon.h>
00021 */
00022 
00023 #include "Bullet-C-Api.h"
00024 #include "btBulletDynamicsCommon.h"
00025 #include "LinearMath/btAlignedAllocator.h"
00026 
00027 
00028 
00029 #include "LinearMath/btVector3.h"
00030 #include "LinearMath/btScalar.h"        
00031 #include "LinearMath/btMatrix3x3.h"
00032 #include "LinearMath/btTransform.h"
00033 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
00034 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00035 
00036 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
00037 #include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
00038 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
00039 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00040 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
00041 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
00042 #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
00043 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
00044 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
00045 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
00046 
00047 
00048 /*
00049         Create and Delete a Physics SDK 
00050 */
00051 
00052 struct  btPhysicsSdk
00053 {
00054 
00055 //      btDispatcher*                           m_dispatcher;
00056 //      btOverlappingPairCache*         m_pairCache;
00057 //      btConstraintSolver*                     m_constraintSolver
00058 
00059         btVector3       m_worldAabbMin;
00060         btVector3       m_worldAabbMax;
00061 
00062 
00063         //todo: version, hardware/optimization settings etc?
00064         btPhysicsSdk()
00065                 :m_worldAabbMin(-1000,-1000,-1000),
00066                 m_worldAabbMax(1000,1000,1000)
00067         {
00068 
00069         }
00070 
00071         
00072 };
00073 
00074 plPhysicsSdkHandle      plNewBulletSdk()
00075 {
00076         void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16);
00077         return (plPhysicsSdkHandle)new (mem)btPhysicsSdk;
00078 }
00079 
00080 void            plDeletePhysicsSdk(plPhysicsSdkHandle   physicsSdk)
00081 {
00082         btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk);
00083         btAlignedFree(phys);    
00084 }
00085 
00086 
00087 /* Dynamics World */
00088 plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
00089 {
00090         btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
00091         void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16);
00092         btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration();
00093         mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16);
00094         btDispatcher*                           dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration);
00095         mem = btAlignedAlloc(sizeof(btAxisSweep3),16);
00096         btBroadphaseInterface*          pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
00097         mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
00098         btConstraintSolver*                     constraintSolver = new(mem) btSequentialImpulseConstraintSolver();
00099 
00100         mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16);
00101         return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration);
00102 }
00103 void           plDeleteDynamicsWorld(plDynamicsWorldHandle world)
00104 {
00105         //todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration
00106         btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
00107         btAlignedFree(dynamicsWorld);
00108 }
00109 
00110 void    plStepSimulation(plDynamicsWorldHandle world,   plReal  timeStep)
00111 {
00112         btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
00113         btAssert(dynamicsWorld);
00114         dynamicsWorld->stepSimulation(timeStep);
00115 }
00116 
00117 void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
00118 {
00119         btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
00120         btAssert(dynamicsWorld);
00121         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
00122         btAssert(body);
00123 
00124         dynamicsWorld->addRigidBody(body);
00125 }
00126 
00127 void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
00128 {
00129         btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
00130         btAssert(dynamicsWorld);
00131         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
00132         btAssert(body);
00133 
00134         dynamicsWorld->removeRigidBody(body);
00135 }
00136 
00137 /* Rigid Body  */
00138 
00139 plRigidBodyHandle plCreateRigidBody(    void* user_data,  float mass, plCollisionShapeHandle cshape )
00140 {
00141         btTransform trans;
00142         trans.setIdentity();
00143         btVector3 localInertia(0,0,0);
00144         btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
00145         btAssert(shape);
00146         if (mass)
00147         {
00148                 shape->calculateLocalInertia(mass,localInertia);
00149         }
00150         void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
00151         btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
00152         btRigidBody* body = new (mem)btRigidBody(rbci);
00153         body->setWorldTransform(trans);
00154         body->setUserPointer(user_data);
00155         return (plRigidBodyHandle) body;
00156 }
00157 
00158 void plDeleteRigidBody(plRigidBodyHandle cbody)
00159 {
00160         btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
00161         btAssert(body);
00162         btAlignedFree( body);
00163 }
00164 
00165 
00166 /* Collision Shape definition */
00167 
00168 plCollisionShapeHandle plNewSphereShape(plReal radius)
00169 {
00170         void* mem = btAlignedAlloc(sizeof(btSphereShape),16);
00171         return (plCollisionShapeHandle) new (mem)btSphereShape(radius);
00172         
00173 }
00174         
00175 plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
00176 {
00177         void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
00178         return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
00179 }
00180 
00181 plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height)
00182 {
00183         //capsule is convex hull of 2 spheres, so use btMultiSphereShape
00184         
00185         const int numSpheres = 2;
00186         btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
00187         btScalar radi[numSpheres] = {radius,radius};
00188         void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
00189         return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres);
00190 }
00191 plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
00192 {
00193         void* mem = btAlignedAlloc(sizeof(btConeShape),16);
00194         return (plCollisionShapeHandle) new (mem)btConeShape(radius,height);
00195 }
00196 
00197 plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
00198 {
00199         void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
00200         return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
00201 }
00202 
00203 /* Convex Meshes */
00204 plCollisionShapeHandle plNewConvexHullShape()
00205 {
00206         void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
00207         return (plCollisionShapeHandle) new (mem)btConvexHullShape();
00208 }
00209 
00210 
00211 /* Concave static triangle meshes */
00212 plMeshInterfaceHandle              plNewMeshInterface()
00213 {
00214         return 0;
00215 }
00216 
00217 plCollisionShapeHandle plNewCompoundShape()
00218 {
00219         void* mem = btAlignedAlloc(sizeof(btCompoundShape),16);
00220         return (plCollisionShapeHandle) new (mem)btCompoundShape();
00221 }
00222 
00223 void    plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn)
00224 {
00225         btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle);
00226         btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE);
00227         btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape);
00228         btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle);
00229         btTransform     localTrans;
00230         localTrans.setIdentity();
00231         localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2]));
00232         localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3]));
00233         compoundShape->addChildShape(localTrans,childShape);
00234 }
00235 
00236 void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient)
00237 {
00238         btQuaternion orn;
00239         orn.setEuler(yaw,pitch,roll);
00240         orient[0] = orn.getX();
00241         orient[1] = orn.getY();
00242         orient[2] = orn.getZ();
00243         orient[3] = orn.getW();
00244 
00245 }
00246 
00247 
00248 //      extern  void            plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
00249 //      extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
00250 
00251 
00252 void            plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z)
00253 {
00254         btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape);
00255         (void)colShape;
00256         btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
00257         btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
00258         convexHullShape->addPoint(btVector3(x,y,z));
00259 
00260 }
00261 
00262 void plDeleteShape(plCollisionShapeHandle cshape)
00263 {
00264         btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
00265         btAssert(shape);
00266         btAlignedFree(shape);
00267 }
00268 void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
00269 {
00270         btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
00271         btAssert(shape);
00272         btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
00273         shape->setLocalScaling(scaling);        
00274 }
00275 
00276 
00277 
00278 void plSetPosition(plRigidBodyHandle object, const plVector3 position)
00279 {
00280         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
00281         btAssert(body);
00282         btVector3 pos(position[0],position[1],position[2]);
00283         btTransform worldTrans = body->getWorldTransform();
00284         worldTrans.setOrigin(pos);
00285         body->setWorldTransform(worldTrans);
00286 }
00287 
00288 void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
00289 {
00290         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
00291         btAssert(body);
00292         btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
00293         btTransform worldTrans = body->getWorldTransform();
00294         worldTrans.setRotation(orn);
00295         body->setWorldTransform(worldTrans);
00296 }
00297 
00298 void    plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
00299 {
00300         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
00301         btAssert(body);
00302         btTransform& worldTrans = body->getWorldTransform();
00303         worldTrans.setFromOpenGLMatrix(matrix);
00304 }
00305 
00306 void    plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
00307 {
00308         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
00309         btAssert(body);
00310         body->getWorldTransform().getOpenGLMatrix(matrix);
00311 
00312 }
00313 
00314 void    plGetPosition(plRigidBodyHandle object,plVector3 position)
00315 {
00316         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
00317         btAssert(body);
00318         const btVector3& pos = body->getWorldTransform().getOrigin();
00319         position[0] = pos.getX();
00320         position[1] = pos.getY();
00321         position[2] = pos.getZ();
00322 }
00323 
00324 void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
00325 {
00326         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
00327         btAssert(body);
00328         const btQuaternion& orn = body->getWorldTransform().getRotation();
00329         orientation[0] = orn.getX();
00330         orientation[1] = orn.getY();
00331         orientation[2] = orn.getZ();
00332         orientation[3] = orn.getW();
00333 }
00334 
00335 
00336 
00337 //plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
00338 
00339 //      extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
00340 
00341 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])
00342 {
00343         btVector3 vp(p1[0], p1[1], p1[2]);
00344         btTriangleShape trishapeA(vp, 
00345                                   btVector3(p2[0], p2[1], p2[2]), 
00346                                   btVector3(p3[0], p3[1], p3[2]));
00347         trishapeA.setMargin(0.000001f);
00348         btVector3 vq(q1[0], q1[1], q1[2]);
00349         btTriangleShape trishapeB(vq, 
00350                                   btVector3(q2[0], q2[1], q2[2]), 
00351                                   btVector3(q3[0], q3[1], q3[2]));
00352         trishapeB.setMargin(0.000001f);
00353         
00354         // btVoronoiSimplexSolver sGjkSimplexSolver;
00355         // btGjkEpaPenetrationDepthSolver penSolverPtr; 
00356         
00357         static btSimplexSolverInterface sGjkSimplexSolver;
00358         sGjkSimplexSolver.reset();
00359         
00360         static btGjkEpaPenetrationDepthSolver Solver0;
00361         static btMinkowskiPenetrationDepthSolver Solver1;
00362                 
00363         btConvexPenetrationDepthSolver* Solver = NULL;
00364         
00365         Solver = &Solver1;      
00366                 
00367         btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
00368         
00369         convexConvex.m_catchDegeneracies = 1;
00370         
00371         // btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
00372         
00373         btPointCollector gjkOutput;
00374         btGjkPairDetector::ClosestPointInput input;
00375         
00376                 
00377         btTransform tr;
00378         tr.setIdentity();
00379         
00380         input.m_transformA = tr;
00381         input.m_transformB = tr;
00382         
00383         convexConvex.getClosestPoints(input, gjkOutput, 0);
00384         
00385         
00386         if (gjkOutput.m_hasResult)
00387         {
00388                 
00389                 pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
00390                 pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
00391                 pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
00392 
00393                 pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
00394                 pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
00395                 pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
00396                 
00397                 normal[0] = gjkOutput.m_normalOnBInWorld[0];
00398                 normal[1] = gjkOutput.m_normalOnBInWorld[1];
00399                 normal[2] = gjkOutput.m_normalOnBInWorld[2];
00400 
00401                 return gjkOutput.m_distance;
00402         }
00403         return -1.0f;   
00404 }
00405