Fix for bug #19817: cloth simulation with collision slow on Mac.
[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
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 #include "LinearMath/btStackAlloc.h"
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         btVector3 inertiaHalfExtents(radius,height,radius);
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(inertiaHalfExtents,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    plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
299 {
300         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
301         btAssert(body);
302         body->getWorldTransform().getOpenGLMatrix(matrix);
303
304 }
305
306 void    plGetPosition(plRigidBodyHandle object,plVector3 position)
307 {
308         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
309         btAssert(body);
310         const btVector3& pos = body->getWorldTransform().getOrigin();
311         position[0] = pos.getX();
312         position[1] = pos.getY();
313         position[2] = pos.getZ();
314 }
315
316 void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
317 {
318         btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
319         btAssert(body);
320         const btQuaternion& orn = body->getWorldTransform().getRotation();
321         orientation[0] = orn.getX();
322         orientation[1] = orn.getY();
323         orientation[2] = orn.getZ();
324         orientation[3] = orn.getW();
325 }
326
327
328
329 //plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
330
331 //      extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
332
333 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])
334 {
335         btVector3 vp(p1[0], p1[1], p1[2]);
336         btTriangleShape trishapeA(vp, 
337                                   btVector3(p2[0], p2[1], p2[2]), 
338                                   btVector3(p3[0], p3[1], p3[2]));
339         trishapeA.setMargin(0.000001f);
340         btVector3 vq(q1[0], q1[1], q1[2]);
341         btTriangleShape trishapeB(vq, 
342                                   btVector3(q2[0], q2[1], q2[2]), 
343                                   btVector3(q3[0], q3[1], q3[2]));
344         trishapeB.setMargin(0.000001f);
345         
346         // btVoronoiSimplexSolver sGjkSimplexSolver;
347         // btGjkEpaPenetrationDepthSolver penSolverPtr; 
348         
349         static btSimplexSolverInterface sGjkSimplexSolver;
350         sGjkSimplexSolver.reset();
351         
352         static btGjkEpaPenetrationDepthSolver Solver0;
353         static btMinkowskiPenetrationDepthSolver Solver1;
354                 
355         btConvexPenetrationDepthSolver* Solver = NULL;
356         
357         Solver = &Solver1;      
358                 
359         btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
360         
361         convexConvex.m_catchDegeneracies = 1;
362         
363         // btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
364         
365         btPointCollector gjkOutput;
366         btGjkPairDetector::ClosestPointInput input;
367         
368         btTransform tr;
369         tr.setIdentity();
370         
371         input.m_transformA = tr;
372         input.m_transformB = tr;
373         
374         convexConvex.getClosestPoints(input, gjkOutput, 0);
375         
376         
377         if (gjkOutput.m_hasResult)
378         {
379                 
380                 pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
381                 pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
382                 pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
383
384                 pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
385                 pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
386                 pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
387                 
388                 normal[0] = gjkOutput.m_normalOnBInWorld[0];
389                 normal[1] = gjkOutput.m_normalOnBInWorld[1];
390                 normal[2] = gjkOutput.m_normalOnBInWorld[2];
391
392                 return gjkOutput.m_distance;
393         }
394         return -1.0f;   
395 }
396