Fix for bug #19817: cloth simulation with collision slow on Mac.
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btConvexConvexAlgorithm.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 #include "btConvexConvexAlgorithm.h"
17
18 //#include <stdio.h>
19 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
20 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22 #include "BulletCollision/CollisionShapes/btConvexShape.h"
23 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
24 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
25 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
26 #include "BulletCollision/CollisionShapes/btBoxShape.h"
27 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
28
29 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
30 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
31 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
32 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
33
34
35
36 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
37 #include "BulletCollision/CollisionShapes/btSphereShape.h"
38
39 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
40
41 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
42 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
43
44
45
46
47
48
49
50
51
52 btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                       simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
53 {
54         m_numPerturbationIterations = 0;
55         m_minimumPointsPerturbationThreshold = 3;
56         m_simplexSolver = simplexSolver;
57         m_pdSolver = pdSolver;
58 }
59
60 btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
61
62 }
63
64 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
65 : btActivatingCollisionAlgorithm(ci,body0,body1),
66 m_simplexSolver(simplexSolver),
67 m_pdSolver(pdSolver),
68 m_ownManifold (false),
69 m_manifoldPtr(mf),
70 m_lowLevelOfDetail(false),
71 #ifdef USE_SEPDISTANCE_UTIL2
72 ,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
73                           (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
74 #endif
75 m_numPerturbationIterations(numPerturbationIterations),
76 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
77 {
78         (void)body0;
79         (void)body1;
80 }
81
82
83
84
85 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
86 {
87         if (m_ownManifold)
88         {
89                 if (m_manifoldPtr)
90                         m_dispatcher->releaseManifold(m_manifoldPtr);
91         }
92 }
93
94 void    btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
95 {
96         m_lowLevelOfDetail = useLowLevel;
97 }
98
99
100 struct btPerturbedContactResult : public btManifoldResult
101 {
102         btManifoldResult* m_originalManifoldResult;
103         btTransform m_transformA;
104         btTransform m_transformB;
105         btTransform     m_unPerturbedTransform;
106         bool    m_perturbA;
107         btIDebugDraw*   m_debugDrawer;
108
109
110         btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
111                 :m_originalManifoldResult(originalResult),
112                 m_transformA(transformA),
113                 m_transformB(transformB),
114                 m_perturbA(perturbA),
115                 m_unPerturbedTransform(unPerturbedTransform),
116                 m_debugDrawer(debugDrawer)
117         {
118         }
119         virtual ~ btPerturbedContactResult()
120         {
121         }
122
123         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
124         {
125                 btVector3 endPt,startPt;
126                 btScalar newDepth;
127                 btVector3 newNormal;
128
129                 if (m_perturbA)
130                 {
131                         btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
132                         endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
133                         newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
134                         startPt = endPt+normalOnBInWorld*newDepth;
135                 } else
136                 {
137                         endPt = pointInWorld + normalOnBInWorld*orgDepth;
138                         startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
139                         newDepth = (endPt -  startPt).dot(normalOnBInWorld);
140                         
141                 }
142
143 //#define DEBUG_CONTACTS 1
144 #ifdef DEBUG_CONTACTS
145                 m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
146                 m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
147                 m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
148 #endif //DEBUG_CONTACTS
149
150                 
151                 m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
152         }
153
154 };
155
156 extern btScalar gContactBreakingThreshold;
157
158 //
159 // Convex-Convex collision algorithm
160 //
161 void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
162 {
163
164         if (!m_manifoldPtr)
165         {
166                 //swapped?
167                 m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
168                 m_ownManifold = true;
169         }
170         resultOut->setPersistentManifold(m_manifoldPtr);
171
172         //comment-out next line to test multi-contact generation
173         //resultOut->getPersistentManifold()->clearManifold();
174         
175
176         btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
177         btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
178
179 #ifdef USE_SEPDISTANCE_UTIL2
180         m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
181         if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
182 #endif //USE_SEPDISTANCE_UTIL2
183
184         {
185
186         
187         btGjkPairDetector::ClosestPointInput input;
188
189         btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
190         //TODO: if (dispatchInfo.m_useContinuous)
191         gjkPairDetector.setMinkowskiA(min0);
192         gjkPairDetector.setMinkowskiB(min1);
193
194 #ifdef USE_SEPDISTANCE_UTIL2
195         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
196         {
197                 input.m_maximumDistanceSquared = 1e30f;
198         } else
199 #endif //USE_SEPDISTANCE_UTIL2
200         {
201                 input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
202                 input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
203         }
204
205         input.m_transformA = body0->getWorldTransform();
206         input.m_transformB = body1->getWorldTransform();
207
208         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
209         btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
210
211         //now perturbe directions to get multiple contact points
212         btVector3 v0,v1;
213         btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
214         btPlaneSpace1(sepNormalWorldSpace,v0,v1);
215         //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
216         
217         //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
218         if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
219         {
220                 
221                 int i;
222
223                 bool perturbeA = true;
224                 const btScalar angleLimit = 0.125f * SIMD_PI;
225                 btScalar perturbeAngle;
226                 btScalar radiusA = min0->getAngularMotionDisc();
227                 btScalar radiusB = min1->getAngularMotionDisc();
228                 if (radiusA < radiusB)
229                 {
230                         perturbeAngle = gContactBreakingThreshold /radiusA;
231                         perturbeA = true;
232                 } else
233                 {
234                         perturbeAngle = gContactBreakingThreshold / radiusB;
235                         perturbeA = false;
236                 }
237                 if ( perturbeAngle > angleLimit ) 
238                                 perturbeAngle = angleLimit;
239
240                 btTransform unPerturbedTransform;
241                 if (perturbeA)
242                 {
243                         unPerturbedTransform = input.m_transformA;
244                 } else
245                 {
246                         unPerturbedTransform = input.m_transformB;
247                 }
248                 
249                 for ( i=0;i<m_numPerturbationIterations;i++)
250                 {
251                         btQuaternion perturbeRot(v0,perturbeAngle);
252                         btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
253                         btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
254                         
255                         
256                         if (perturbeA)
257                         {
258                                 input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
259                                 input.m_transformB = body1->getWorldTransform();
260 #ifdef DEBUG_CONTACTS
261                                 dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
262 #endif //DEBUG_CONTACTS
263                         } else
264                         {
265                                 input.m_transformA = body0->getWorldTransform();
266                                 input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
267 #ifdef DEBUG_CONTACTS
268                                 dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
269 #endif
270                         }
271                         
272                         btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
273                         gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
274                         
275                         
276                 }
277         }
278
279         
280
281 #ifdef USE_SEPDISTANCE_UTIL2
282         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
283         {
284                 m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
285         }
286 #endif //USE_SEPDISTANCE_UTIL2
287
288
289         }
290
291         if (m_ownManifold)
292         {
293                 resultOut->refreshContactPoints();
294         }
295
296 }
297
298
299
300 bool disableCcd = false;
301 btScalar        btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
302 {
303         (void)resultOut;
304         (void)dispatchInfo;
305         ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
306     
307         ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
308         ///col0->m_worldTransform,
309         btScalar resultFraction = btScalar(1.);
310
311
312         btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
313         btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
314     
315         if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
316                 squareMot1 < col1->getCcdSquareMotionThreshold())
317                 return resultFraction;
318
319         if (disableCcd)
320                 return btScalar(1.);
321
322
323         //An adhoc way of testing the Continuous Collision Detection algorithms
324         //One object is approximated as a sphere, to simplify things
325         //Starting in penetration should report no time of impact
326         //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
327         //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
328
329                 
330         /// Convex0 against sphere for Convex1
331         {
332                 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
333
334                 btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
335                 btConvexCast::CastResult result;
336                 btVoronoiSimplexSolver voronoiSimplex;
337                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
338                 ///Simplification, one object is simplified as a sphere
339                 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
340                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
341                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
342                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
343                 {
344                 
345                         //store result.m_fraction in both bodies
346                 
347                         if (col0->getHitFraction()> result.m_fraction)
348                                 col0->setHitFraction( result.m_fraction );
349
350                         if (col1->getHitFraction() > result.m_fraction)
351                                 col1->setHitFraction( result.m_fraction);
352
353                         if (resultFraction > result.m_fraction)
354                                 resultFraction = result.m_fraction;
355
356                 }
357                 
358                 
359
360
361         }
362
363         /// Sphere (for convex0) against Convex1
364         {
365                 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
366
367                 btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
368                 btConvexCast::CastResult result;
369                 btVoronoiSimplexSolver voronoiSimplex;
370                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
371                 ///Simplification, one object is simplified as a sphere
372                 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
373                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
374                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
375                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
376                 {
377                 
378                         //store result.m_fraction in both bodies
379                 
380                         if (col0->getHitFraction()      > result.m_fraction)
381                                 col0->setHitFraction( result.m_fraction);
382
383                         if (col1->getHitFraction() > result.m_fraction)
384                                 col1->setHitFraction( result.m_fraction);
385
386                         if (resultFraction > result.m_fraction)
387                                 resultFraction = result.m_fraction;
388
389                 }
390         }
391         
392         return resultFraction;
393
394 }
395