Patch Bullet to make it's convex hull implementation usable in BMesh
[blender.git] / extern / bullet2 / 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 #include "LinearMath/btConvexHullComputer.h"
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
406 // Convex hull
407 plConvexHull plConvexHullCompute(float (*coords)[3], int count)
408 {
409         btConvexHullComputer *computer = new btConvexHullComputer;
410         computer->compute(reinterpret_cast< float* >(coords),
411                                           sizeof(*coords), count, 0, 0);
412         return reinterpret_cast<plConvexHull>(computer);
413 }
414
415 int plConvexHullNumVertices(plConvexHull hull)
416 {
417         btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
418         return computer->vertices.size();
419 }
420
421 int plConvexHullNumFaces(plConvexHull hull)
422 {
423         btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
424         return computer->faces.size();
425 }
426
427 void plConvexHullGetVertex(plConvexHull hull, int n, float coords[3],
428                                                    int *original_index)
429 {
430         btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
431         const btVector3 &v(computer->vertices[n]);
432         coords[0] = v[0];
433         coords[1] = v[1];
434         coords[2] = v[2];
435         (*original_index) = computer->original_vertex_index[n];
436 }
437
438 int plConvexHullGetFaceSize(plConvexHull hull, int n)
439 {
440         btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
441         const btConvexHullComputer::Edge *e_orig, *e;
442         int count;
443
444         for (e_orig = &computer->edges[computer->faces[n]], e = e_orig, count = 0;
445                  count == 0 || e != e_orig;
446                  e = e->getNextEdgeOfFace(), count++);
447         return count;
448 }
449
450 void plConvexHullGetFaceVertices(plConvexHull hull, int n, int *vertices)
451 {
452         btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull));
453         const btConvexHullComputer::Edge *e_orig, *e;
454         int count;
455
456         for (e_orig = &computer->edges[computer->faces[n]], e = e_orig, count = 0;
457                  count == 0 || e != e_orig;
458                  e = e->getNextEdgeOfFace(), count++)
459         {
460                 vertices[count] = e->getTargetVertex();
461         }
462 }