274c5f5bdc6b03bec294ca6ceb87544c458c2d79
[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_stackAlloc = dispatchInfo.m_stackAllocator;
206         input.m_transformA = body0->getWorldTransform();
207         input.m_transformB = body1->getWorldTransform();
208
209         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
210         btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
211
212         //now perturbe directions to get multiple contact points
213         btVector3 v0,v1;
214         btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
215         btPlaneSpace1(sepNormalWorldSpace,v0,v1);
216         //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
217         
218         //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
219         if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
220         {
221                 
222                 int i;
223
224                 bool perturbeA = true;
225                 const btScalar angleLimit = 0.125f * SIMD_PI;
226                 btScalar perturbeAngle;
227                 btScalar radiusA = min0->getAngularMotionDisc();
228                 btScalar radiusB = min1->getAngularMotionDisc();
229                 if (radiusA < radiusB)
230                 {
231                         perturbeAngle = gContactBreakingThreshold /radiusA;
232                         perturbeA = true;
233                 } else
234                 {
235                         perturbeAngle = gContactBreakingThreshold / radiusB;
236                         perturbeA = false;
237                 }
238                 if ( perturbeAngle > angleLimit ) 
239                                 perturbeAngle = angleLimit;
240
241                 btTransform unPerturbedTransform;
242                 if (perturbeA)
243                 {
244                         unPerturbedTransform = input.m_transformA;
245                 } else
246                 {
247                         unPerturbedTransform = input.m_transformB;
248                 }
249                 
250                 for ( i=0;i<m_numPerturbationIterations;i++)
251                 {
252                         btQuaternion perturbeRot(v0,perturbeAngle);
253                         btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
254                         btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
255                         
256                         
257                         if (perturbeA)
258                         {
259                                 input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
260                                 input.m_transformB = body1->getWorldTransform();
261 #ifdef DEBUG_CONTACTS
262                                 dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
263 #endif //DEBUG_CONTACTS
264                         } else
265                         {
266                                 input.m_transformA = body0->getWorldTransform();
267                                 input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
268 #ifdef DEBUG_CONTACTS
269                                 dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
270 #endif
271                         }
272                         
273                         btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
274                         gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
275                         
276                         
277                 }
278         }
279
280         
281
282 #ifdef USE_SEPDISTANCE_UTIL2
283         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
284         {
285                 m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
286         }
287 #endif //USE_SEPDISTANCE_UTIL2
288
289
290         }
291
292         if (m_ownManifold)
293         {
294                 resultOut->refreshContactPoints();
295         }
296
297 }
298
299
300
301 bool disableCcd = false;
302 btScalar        btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
303 {
304         (void)resultOut;
305         (void)dispatchInfo;
306         ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
307     
308         ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
309         ///col0->m_worldTransform,
310         btScalar resultFraction = btScalar(1.);
311
312
313         btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
314         btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
315     
316         if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
317                 squareMot1 < col1->getCcdSquareMotionThreshold())
318                 return resultFraction;
319
320         if (disableCcd)
321                 return btScalar(1.);
322
323
324         //An adhoc way of testing the Continuous Collision Detection algorithms
325         //One object is approximated as a sphere, to simplify things
326         //Starting in penetration should report no time of impact
327         //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
328         //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
329
330                 
331         /// Convex0 against sphere for Convex1
332         {
333                 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
334
335                 btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
336                 btConvexCast::CastResult result;
337                 btVoronoiSimplexSolver voronoiSimplex;
338                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
339                 ///Simplification, one object is simplified as a sphere
340                 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
341                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
342                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
343                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
344                 {
345                 
346                         //store result.m_fraction in both bodies
347                 
348                         if (col0->getHitFraction()> result.m_fraction)
349                                 col0->setHitFraction( result.m_fraction );
350
351                         if (col1->getHitFraction() > result.m_fraction)
352                                 col1->setHitFraction( result.m_fraction);
353
354                         if (resultFraction > result.m_fraction)
355                                 resultFraction = result.m_fraction;
356
357                 }
358                 
359                 
360
361
362         }
363
364         /// Sphere (for convex0) against Convex1
365         {
366                 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
367
368                 btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
369                 btConvexCast::CastResult result;
370                 btVoronoiSimplexSolver voronoiSimplex;
371                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
372                 ///Simplification, one object is simplified as a sphere
373                 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
374                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
375                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
376                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
377                 {
378                 
379                         //store result.m_fraction in both bodies
380                 
381                         if (col0->getHitFraction()      > result.m_fraction)
382                                 col0->setHitFraction( result.m_fraction);
383
384                         if (col1->getHitFraction() > result.m_fraction)
385                                 col1->setHitFraction( result.m_fraction);
386
387                         if (resultFraction > result.m_fraction)
388                                 resultFraction = result.m_fraction;
389
390                 }
391         }
392         
393         return resultFraction;
394
395 }
396