00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00050
00051
00052 struct btPhysicsSdk
00053 {
00054
00055
00056
00057
00058
00059 btVector3 m_worldAabbMin;
00060 btVector3 m_worldAabbMax;
00061
00062
00063
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
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
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
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
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
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
00204 plCollisionShapeHandle plNewConvexHullShape()
00205 {
00206 void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
00207 return (plCollisionShapeHandle) new (mem)btConvexHullShape();
00208 }
00209
00210
00211
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
00249
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
00338
00339
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
00355
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
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