Blender V2.61 - r43446

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