dd1f3e2490f2746c2d21f6d85f2d895558a04a5a
[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 ///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
17 ///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
18 ///with reproduction case
19 //define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
20 //#define ZERO_MARGIN
21
22 #include "btConvexConvexAlgorithm.h"
23
24 //#include <stdio.h>
25 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
26 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
27 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
28 #include "BulletCollision/CollisionShapes/btConvexShape.h"
29 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
30 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
31
32
33
34 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
35 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
36 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
37 #include "BulletCollision/CollisionShapes/btBoxShape.h"
38 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
39
40 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
41 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
42 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
43 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
44
45
46
47 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
48 #include "BulletCollision/CollisionShapes/btSphereShape.h"
49
50 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
51
52 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
53 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
54 #include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
55
56
57 ///////////
58
59
60
61 static SIMD_FORCE_INLINE void segmentsClosestPoints(
62         btVector3& ptsVector,
63         btVector3& offsetA,
64         btVector3& offsetB,
65         btScalar& tA, btScalar& tB,
66         const btVector3& translation,
67         const btVector3& dirA, btScalar hlenA,
68         const btVector3& dirB, btScalar hlenB )
69 {
70         // compute the parameters of the closest points on each line segment
71
72         btScalar dirA_dot_dirB = btDot(dirA,dirB);
73         btScalar dirA_dot_trans = btDot(dirA,translation);
74         btScalar dirB_dot_trans = btDot(dirB,translation);
75
76         btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
77
78         if ( denom == 0.0f ) {
79                 tA = 0.0f;
80         } else {
81                 tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
82                 if ( tA < -hlenA )
83                         tA = -hlenA;
84                 else if ( tA > hlenA )
85                         tA = hlenA;
86         }
87
88         tB = tA * dirA_dot_dirB - dirB_dot_trans;
89
90         if ( tB < -hlenB ) {
91                 tB = -hlenB;
92                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
93
94                 if ( tA < -hlenA )
95                         tA = -hlenA;
96                 else if ( tA > hlenA )
97                         tA = hlenA;
98         } else if ( tB > hlenB ) {
99                 tB = hlenB;
100                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
101
102                 if ( tA < -hlenA )
103                         tA = -hlenA;
104                 else if ( tA > hlenA )
105                         tA = hlenA;
106         }
107
108         // compute the closest points relative to segment centers.
109
110         offsetA = dirA * tA;
111         offsetB = dirB * tB;
112
113         ptsVector = translation - offsetA + offsetB;
114 }
115
116
117 static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
118         btVector3& normalOnB,
119         btVector3& pointOnB,
120         btScalar capsuleLengthA,
121         btScalar        capsuleRadiusA,
122         btScalar capsuleLengthB,
123         btScalar        capsuleRadiusB,
124         int capsuleAxisA,
125         int capsuleAxisB,
126         const btTransform& transformA,
127         const btTransform& transformB,
128         btScalar distanceThreshold )
129 {
130         btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
131         btVector3 translationA = transformA.getOrigin();
132         btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
133         btVector3 translationB = transformB.getOrigin();
134
135         // translation between centers
136
137         btVector3 translation = translationB - translationA;
138
139         // compute the closest points of the capsule line segments
140
141         btVector3 ptsVector;           // the vector between the closest points
142         
143         btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
144         btScalar tA, tB;              // parameters on line segment
145
146         segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
147                                                    directionA, capsuleLengthA, directionB, capsuleLengthB );
148
149         btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
150
151         if ( distance > distanceThreshold )
152                 return distance;
153
154         btScalar lenSqr = ptsVector.length2();
155         if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
156         {
157                 //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
158                 btVector3 q;
159                 btPlaneSpace1(directionA,normalOnB,q);
160         } else
161         {
162                 // compute the contact normal
163                 normalOnB = ptsVector*-btRecipSqrt(lenSqr);
164         }
165         pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
166
167         return distance;
168 }
169
170
171
172
173
174
175
176 //////////
177
178
179
180
181
182 btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                       simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
183 {
184         m_numPerturbationIterations = 0;
185         m_minimumPointsPerturbationThreshold = 3;
186         m_simplexSolver = simplexSolver;
187         m_pdSolver = pdSolver;
188 }
189
190 btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
191
192 }
193
194 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
195 : btActivatingCollisionAlgorithm(ci,body0,body1),
196 m_simplexSolver(simplexSolver),
197 m_pdSolver(pdSolver),
198 m_ownManifold (false),
199 m_manifoldPtr(mf),
200 m_lowLevelOfDetail(false),
201 #ifdef USE_SEPDISTANCE_UTIL2
202 m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
203                           (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
204 #endif
205 m_numPerturbationIterations(numPerturbationIterations),
206 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
207 {
208         (void)body0;
209         (void)body1;
210 }
211
212
213
214
215 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
216 {
217         if (m_ownManifold)
218         {
219                 if (m_manifoldPtr)
220                         m_dispatcher->releaseManifold(m_manifoldPtr);
221         }
222 }
223
224 void    btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
225 {
226         m_lowLevelOfDetail = useLowLevel;
227 }
228
229
230 struct btPerturbedContactResult : public btManifoldResult
231 {
232         btManifoldResult* m_originalManifoldResult;
233         btTransform m_transformA;
234         btTransform m_transformB;
235         btTransform     m_unPerturbedTransform;
236         bool    m_perturbA;
237         btIDebugDraw*   m_debugDrawer;
238
239
240         btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
241                 :m_originalManifoldResult(originalResult),
242                 m_transformA(transformA),
243                 m_transformB(transformB),
244                 m_unPerturbedTransform(unPerturbedTransform),
245                 m_perturbA(perturbA),
246                 m_debugDrawer(debugDrawer)
247         {
248         }
249         virtual ~ btPerturbedContactResult()
250         {
251         }
252
253         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
254         {
255                 btVector3 endPt,startPt;
256                 btScalar newDepth;
257                 btVector3 newNormal;
258
259                 if (m_perturbA)
260                 {
261                         btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
262                         endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
263                         newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
264                         startPt = endPt+normalOnBInWorld*newDepth;
265                 } else
266                 {
267                         endPt = pointInWorld + normalOnBInWorld*orgDepth;
268                         startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
269                         newDepth = (endPt -  startPt).dot(normalOnBInWorld);
270                         
271                 }
272
273 //#define DEBUG_CONTACTS 1
274 #ifdef DEBUG_CONTACTS
275                 m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
276                 m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
277                 m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
278 #endif //DEBUG_CONTACTS
279
280                 
281                 m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
282         }
283
284 };
285
286 extern btScalar gContactBreakingThreshold;
287
288
289 //
290 // Convex-Convex collision algorithm
291 //
292 void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
293 {
294
295         if (!m_manifoldPtr)
296         {
297                 //swapped?
298                 m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
299                 m_ownManifold = true;
300         }
301         resultOut->setPersistentManifold(m_manifoldPtr);
302
303         //comment-out next line to test multi-contact generation
304         //resultOut->getPersistentManifold()->clearManifold();
305         
306
307         btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
308         btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
309
310         btVector3  normalOnB;
311                 btVector3  pointOnBWorld;
312 #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
313         if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
314         {
315                 btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
316                 btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
317                 btVector3 localScalingA = capsuleA->getLocalScaling();
318                 btVector3 localScalingB = capsuleB->getLocalScaling();
319                 
320                 btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
321
322                 btScalar dist = capsuleCapsuleDistance(normalOnB,       pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
323                         capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
324                         body0->getWorldTransform(),body1->getWorldTransform(),threshold);
325
326                 if (dist<threshold)
327                 {
328                         btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
329                         resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);       
330                 }
331                 resultOut->refreshContactPoints();
332                 return;
333         }
334 #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
335
336
337
338
339 #ifdef USE_SEPDISTANCE_UTIL2
340         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
341         {
342                 m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
343         }
344
345         if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
346 #endif //USE_SEPDISTANCE_UTIL2
347
348         {
349
350         
351         btGjkPairDetector::ClosestPointInput input;
352
353         btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
354         //TODO: if (dispatchInfo.m_useContinuous)
355         gjkPairDetector.setMinkowskiA(min0);
356         gjkPairDetector.setMinkowskiB(min1);
357
358 #ifdef USE_SEPDISTANCE_UTIL2
359         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
360         {
361                 input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
362         } else
363 #endif //USE_SEPDISTANCE_UTIL2
364         {
365                 //if (dispatchInfo.m_convexMaxDistanceUseCPT)
366                 //{
367                 //      input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
368                 //} else
369                 //{
370                 input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
371 //              }
372
373                 input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
374         }
375
376         input.m_stackAlloc = dispatchInfo.m_stackAllocator;
377         input.m_transformA = body0->getWorldTransform();
378         input.m_transformB = body1->getWorldTransform();
379
380
381
382         
383
384 #ifdef USE_SEPDISTANCE_UTIL2
385         btScalar sepDist = 0.f;
386         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
387         {
388                 sepDist = gjkPairDetector.getCachedSeparatingDistance();
389                 if (sepDist>SIMD_EPSILON)
390                 {
391                         sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
392                         //now perturbe directions to get multiple contact points
393                         
394                 }
395         }
396 #endif //USE_SEPDISTANCE_UTIL2
397
398         if (min0->isPolyhedral() && min1->isPolyhedral())
399         {
400
401
402                 struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
403                 {
404                         virtual void setShapeIdentifiersA(int partId0,int index0){}
405                         virtual void setShapeIdentifiersB(int partId1,int index1){}
406                         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) 
407                         {
408                         }
409                 };
410                 
411                 btDummyResult dummy;
412
413
414                 btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
415                 btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
416                 if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
417                 {
418
419
420                         
421
422                         btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
423
424                         btScalar minDist = -1e30f;
425                         btVector3 sepNormalWorldSpace;
426                         bool foundSepAxis  = true;
427
428                         if (dispatchInfo.m_enableSatConvex)
429                         {
430                                 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
431                                         *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
432                                         body0->getWorldTransform(), 
433                                         body1->getWorldTransform(),
434                                         sepNormalWorldSpace);
435                         } else
436                         {
437 #ifdef ZERO_MARGIN
438                                 gjkPairDetector.setIgnoreMargin(true);
439                                 gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
440 #else
441                                 //gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
442                                 gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
443 #endif //ZERO_MARGIN
444                                 btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
445                                 if (l2>SIMD_EPSILON)
446                                 {
447                                         sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
448                                         //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
449                                         minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
450         
451 #ifdef ZERO_MARGIN
452                                         foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
453 #else
454                                         foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(min0->getMargin()+min1->getMargin());
455 #endif
456                                 }
457                         }
458                         if (foundSepAxis)
459                         {
460 //                              printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
461
462                                 btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
463                                         body0->getWorldTransform(), 
464                                         body1->getWorldTransform(), minDist-threshold, threshold, *resultOut);
465                                 
466                         }
467                         if (m_ownManifold)
468                         {
469                                 resultOut->refreshContactPoints();
470                         }
471                         return;
472
473                 } else
474                 {
475                         //we can also deal with convex versus triangle (without connectivity data)
476                         if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
477                         {
478
479                                 btVertexArray vertices;
480                                 btTriangleShape* tri = (btTriangleShape*)polyhedronB;
481                                 vertices.push_back(     body1->getWorldTransform()*tri->m_vertices1[0]);
482                                 vertices.push_back(     body1->getWorldTransform()*tri->m_vertices1[1]);
483                                 vertices.push_back(     body1->getWorldTransform()*tri->m_vertices1[2]);
484                                 
485                                 //tri->initializePolyhedralFeatures();
486
487                                 btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
488
489                                 btVector3 sepNormalWorldSpace;
490                                 btScalar minDist =-1e30f;
491                                 btScalar maxDist = threshold;
492                                 
493                                 bool foundSepAxis = false;
494                                 if (0)
495                                 {
496                                         polyhedronB->initializePolyhedralFeatures();
497                                          foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
498                                         *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
499                                         body0->getWorldTransform(), 
500                                         body1->getWorldTransform(),
501                                         sepNormalWorldSpace);
502                                 //       printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
503
504                                 } else
505                                 {
506 #ifdef ZERO_MARGIN
507                                         gjkPairDetector.setIgnoreMargin(true);
508                                         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
509 #else
510                                         gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
511 #endif//ZERO_MARGIN
512                                         
513                                         btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
514                                         if (l2>SIMD_EPSILON)
515                                         {
516                                                 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
517                                                 //minDist = gjkPairDetector.getCachedSeparatingDistance();
518                                                 //maxDist = threshold;
519                                                 minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
520                                                 foundSepAxis = true;
521                                         }
522                                 }
523
524                                 
525                         if (foundSepAxis)
526                         {
527                                 btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
528                                         body0->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
529                         }
530                                 
531                                 
532                                 if (m_ownManifold)
533                                 {
534                                         resultOut->refreshContactPoints();
535                                 }
536                                 
537                                 return;
538                         }
539                         
540                 }
541
542
543         }
544         
545         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
546
547         //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
548         
549         //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
550         if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
551         {
552                 
553                 int i;
554                 btVector3 v0,v1;
555                 btVector3 sepNormalWorldSpace;
556                 btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
557         
558                 if (l2>SIMD_EPSILON)
559                 {
560                         sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
561                         
562                         btPlaneSpace1(sepNormalWorldSpace,v0,v1);
563
564
565                         bool perturbeA = true;
566                         const btScalar angleLimit = 0.125f * SIMD_PI;
567                         btScalar perturbeAngle;
568                         btScalar radiusA = min0->getAngularMotionDisc();
569                         btScalar radiusB = min1->getAngularMotionDisc();
570                         if (radiusA < radiusB)
571                         {
572                                 perturbeAngle = gContactBreakingThreshold /radiusA;
573                                 perturbeA = true;
574                         } else
575                         {
576                                 perturbeAngle = gContactBreakingThreshold / radiusB;
577                                 perturbeA = false;
578                         }
579                         if ( perturbeAngle > angleLimit ) 
580                                         perturbeAngle = angleLimit;
581
582                         btTransform unPerturbedTransform;
583                         if (perturbeA)
584                         {
585                                 unPerturbedTransform = input.m_transformA;
586                         } else
587                         {
588                                 unPerturbedTransform = input.m_transformB;
589                         }
590                         
591                         for ( i=0;i<m_numPerturbationIterations;i++)
592                         {
593                                 if (v0.length2()>SIMD_EPSILON)
594                                 {
595                                 btQuaternion perturbeRot(v0,perturbeAngle);
596                                 btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
597                                 btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
598                                 
599                                 
600                                 if (perturbeA)
601                                 {
602                                         input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
603                                         input.m_transformB = body1->getWorldTransform();
604         #ifdef DEBUG_CONTACTS
605                                         dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
606         #endif //DEBUG_CONTACTS
607                                 } else
608                                 {
609                                         input.m_transformA = body0->getWorldTransform();
610                                         input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
611         #ifdef DEBUG_CONTACTS
612                                         dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
613         #endif
614                                 }
615                                 
616                                 btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
617                                 gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
618                                 }
619                         }
620                 }
621         }
622
623         
624
625 #ifdef USE_SEPDISTANCE_UTIL2
626         if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
627         {
628                 m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
629         }
630 #endif //USE_SEPDISTANCE_UTIL2
631
632
633         }
634
635         if (m_ownManifold)
636         {
637                 resultOut->refreshContactPoints();
638         }
639
640 }
641
642
643
644 bool disableCcd = false;
645 btScalar        btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
646 {
647         (void)resultOut;
648         (void)dispatchInfo;
649         ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
650     
651         ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
652         ///col0->m_worldTransform,
653         btScalar resultFraction = btScalar(1.);
654
655
656         btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
657         btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
658     
659         if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
660                 squareMot1 < col1->getCcdSquareMotionThreshold())
661                 return resultFraction;
662
663         if (disableCcd)
664                 return btScalar(1.);
665
666
667         //An adhoc way of testing the Continuous Collision Detection algorithms
668         //One object is approximated as a sphere, to simplify things
669         //Starting in penetration should report no time of impact
670         //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
671         //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
672
673                 
674         /// Convex0 against sphere for Convex1
675         {
676                 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
677
678                 btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
679                 btConvexCast::CastResult result;
680                 btVoronoiSimplexSolver voronoiSimplex;
681                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
682                 ///Simplification, one object is simplified as a sphere
683                 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
684                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
685                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
686                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
687                 {
688                 
689                         //store result.m_fraction in both bodies
690                 
691                         if (col0->getHitFraction()> result.m_fraction)
692                                 col0->setHitFraction( result.m_fraction );
693
694                         if (col1->getHitFraction() > result.m_fraction)
695                                 col1->setHitFraction( result.m_fraction);
696
697                         if (resultFraction > result.m_fraction)
698                                 resultFraction = result.m_fraction;
699
700                 }
701                 
702                 
703
704
705         }
706
707         /// Sphere (for convex0) against Convex1
708         {
709                 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
710
711                 btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
712                 btConvexCast::CastResult result;
713                 btVoronoiSimplexSolver voronoiSimplex;
714                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
715                 ///Simplification, one object is simplified as a sphere
716                 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
717                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
718                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
719                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
720                 {
721                 
722                         //store result.m_fraction in both bodies
723                 
724                         if (col0->getHitFraction()      > result.m_fraction)
725                                 col0->setHitFraction( result.m_fraction);
726
727                         if (col1->getHitFraction() > result.m_fraction)
728                                 col1->setHitFraction( result.m_fraction);
729
730                         if (resultFraction > result.m_fraction)
731                                 resultFraction = result.m_fraction;
732
733                 }
734         }
735         
736         return resultFraction;
737
738 }
739