bullet: Update to current svn, r2636
[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 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
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,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
195 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
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)body0Wrap;
209         (void)body1Wrap;
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 (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
293 {
294
295         if (!m_manifoldPtr)
296         {
297                 //swapped?
298                 m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
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         const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
308         const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->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                         body0Wrap->getWorldTransform(),body1Wrap->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 = body0Wrap->getWorldTransform();
378         input.m_transformB = body1Wrap->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                 
412                 struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
413                 {
414                         btDiscreteCollisionDetectorInterface::Result* m_originalResult;
415                         btVector3       m_reportedNormalOnWorld;
416                         btScalar m_marginOnA;
417                         btScalar m_marginOnB;
418                         btScalar        m_reportedDistance;
419                         
420                         bool            m_foundResult;
421                         btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
422                         :m_originalResult(result),
423                         m_marginOnA(marginOnA),
424                         m_marginOnB(marginOnB),
425                         m_foundResult(false)
426                         {
427                         }
428                         
429                         virtual void setShapeIdentifiersA(int partId0,int index0){}
430                         virtual void setShapeIdentifiersB(int partId1,int index1){}
431                         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg) 
432                         {
433                                 m_reportedDistance = depthOrg;
434                                 m_reportedNormalOnWorld = normalOnBInWorld;
435                                 
436                                 btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
437                                 m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
438                                 if (m_reportedDistance<0.f)
439                                 {
440                                         m_foundResult = true;                                   
441                                 }
442                                 m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
443                         }
444                 };
445
446                 
447                 btDummyResult dummy;
448
449 ///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
450
451                 btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
452                 btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
453
454                 btWithoutMarginResult   withoutMargin(resultOut, min0Margin,min1Margin);
455
456                 btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
457                 btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
458                 if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
459                 {
460
461
462                         
463
464                         btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
465
466                         btScalar minDist = -1e30f;
467                         btVector3 sepNormalWorldSpace;
468                         bool foundSepAxis  = true;
469
470                         if (dispatchInfo.m_enableSatConvex)
471                         {
472                                 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
473                                         *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
474                                         body0Wrap->getWorldTransform(), 
475                                         body1Wrap->getWorldTransform(),
476                                         sepNormalWorldSpace,*resultOut);
477                         } else
478                         {
479 #ifdef ZERO_MARGIN
480                                 gjkPairDetector.setIgnoreMargin(true);
481                                 gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
482 #else
483
484
485                                 gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
486                                 //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
487 #endif //ZERO_MARGIN
488                                 //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
489                                 //if (l2>SIMD_EPSILON)
490                                 {
491                                         sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
492                                         //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
493                                         minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
494         
495 #ifdef ZERO_MARGIN
496                                         foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
497 #else
498                                         foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
499 #endif
500                                 }
501                         }
502                         if (foundSepAxis)
503                         {
504                                 
505 //                              printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
506
507                                 btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
508                                         body0Wrap->getWorldTransform(), 
509                                         body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut);
510                                 
511                         }
512                         if (m_ownManifold)
513                         {
514                                 resultOut->refreshContactPoints();
515                         }
516                         return;
517
518                 } else
519                 {
520                         //we can also deal with convex versus triangle (without connectivity data)
521                         if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
522                         {
523
524                                 btVertexArray vertices;
525                                 btTriangleShape* tri = (btTriangleShape*)polyhedronB;
526                                 vertices.push_back(     body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
527                                 vertices.push_back(     body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
528                                 vertices.push_back(     body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
529                                 
530                                 //tri->initializePolyhedralFeatures();
531
532                                 btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
533
534                                 btVector3 sepNormalWorldSpace;
535                                 btScalar minDist =-1e30f;
536                                 btScalar maxDist = threshold;
537                                 
538                                 bool foundSepAxis = false;
539                                 if (0)
540                                 {
541                                         polyhedronB->initializePolyhedralFeatures();
542                                          foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
543                                         *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
544                                         body0Wrap->getWorldTransform(), 
545                                         body1Wrap->getWorldTransform(),
546                                         sepNormalWorldSpace,*resultOut);
547                                 //       printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
548
549                                 } else
550                                 {
551 #ifdef ZERO_MARGIN
552                                         gjkPairDetector.setIgnoreMargin(true);
553                                         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
554 #else
555                                         gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
556 #endif//ZERO_MARGIN
557                                         
558                                         btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
559                                         if (l2>SIMD_EPSILON)
560                                         {
561                                                 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
562                                                 //minDist = gjkPairDetector.getCachedSeparatingDistance();
563                                                 //maxDist = threshold;
564                                                 minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
565                                                 foundSepAxis = true;
566                                         }
567                                 }
568
569                                 
570                         if (foundSepAxis)
571                         {
572                                 btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
573                                         body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
574                         }
575                                 
576                                 
577                                 if (m_ownManifold)
578                                 {
579                                         resultOut->refreshContactPoints();
580                                 }
581                                 
582                                 return;
583                         }
584                         
585                 }
586
587
588         }
589         
590         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
591
592         //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
593         
594         //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
595         if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
596         {
597                 
598                 int i;
599                 btVector3 v0,v1;
600                 btVector3 sepNormalWorldSpace;
601                 btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
602         
603                 if (l2>SIMD_EPSILON)
604                 {
605                         sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
606                         
607                         btPlaneSpace1(sepNormalWorldSpace,v0,v1);
608
609
610                         bool perturbeA = true;
611                         const btScalar angleLimit = 0.125f * SIMD_PI;
612                         btScalar perturbeAngle;
613                         btScalar radiusA = min0->getAngularMotionDisc();
614                         btScalar radiusB = min1->getAngularMotionDisc();
615                         if (radiusA < radiusB)
616                         {
617                                 perturbeAngle = gContactBreakingThreshold /radiusA;
618                                 perturbeA = true;
619                         } else
620                         {
621                                 perturbeAngle = gContactBreakingThreshold / radiusB;
622                                 perturbeA = false;
623                         }
624                         if ( perturbeAngle > angleLimit ) 
625                                         perturbeAngle = angleLimit;
626
627                         btTransform unPerturbedTransform;
628                         if (perturbeA)
629                         {
630                                 unPerturbedTransform = input.m_transformA;
631                         } else
632                         {
633                                 unPerturbedTransform = input.m_transformB;
634                         }
635                         
636                         for ( i=0;i<m_numPerturbationIterations;i++)
637                         {
638                                 if (v0.length2()>SIMD_EPSILON)
639                                 {
640                                 btQuaternion perturbeRot(v0,perturbeAngle);
641                                 btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
642                                 btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
643                                 
644                                 
645                                 if (perturbeA)
646                                 {
647                                         input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis());
648                                         input.m_transformB = body1Wrap->getWorldTransform();
649         #ifdef DEBUG_CONTACTS
650                                         dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
651         #endif //DEBUG_CONTACTS
652                                 } else
653                                 {
654                                         input.m_transformA = body0Wrap->getWorldTransform();
655                                         input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis());
656         #ifdef DEBUG_CONTACTS
657                                         dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
658         #endif
659                                 }
660                                 
661                                 btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
662                                 gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
663                                 }
664                         }
665                 }
666         }
667
668         
669
670 #ifdef USE_SEPDISTANCE_UTIL2
671         if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
672         {
673                 m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
674         }
675 #endif //USE_SEPDISTANCE_UTIL2
676
677
678         }
679
680         if (m_ownManifold)
681         {
682                 resultOut->refreshContactPoints();
683         }
684
685 }
686
687
688
689 bool disableCcd = false;
690 btScalar        btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
691 {
692         (void)resultOut;
693         (void)dispatchInfo;
694         ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
695     
696         ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
697         ///col0->m_worldTransform,
698         btScalar resultFraction = btScalar(1.);
699
700
701         btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
702         btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
703     
704         if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
705                 squareMot1 < col1->getCcdSquareMotionThreshold())
706                 return resultFraction;
707
708         if (disableCcd)
709                 return btScalar(1.);
710
711
712         //An adhoc way of testing the Continuous Collision Detection algorithms
713         //One object is approximated as a sphere, to simplify things
714         //Starting in penetration should report no time of impact
715         //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
716         //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
717
718                 
719         /// Convex0 against sphere for Convex1
720         {
721                 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
722
723                 btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
724                 btConvexCast::CastResult result;
725                 btVoronoiSimplexSolver voronoiSimplex;
726                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
727                 ///Simplification, one object is simplified as a sphere
728                 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
729                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
730                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
731                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
732                 {
733                 
734                         //store result.m_fraction in both bodies
735                 
736                         if (col0->getHitFraction()> result.m_fraction)
737                                 col0->setHitFraction( result.m_fraction );
738
739                         if (col1->getHitFraction() > result.m_fraction)
740                                 col1->setHitFraction( result.m_fraction);
741
742                         if (resultFraction > result.m_fraction)
743                                 resultFraction = result.m_fraction;
744
745                 }
746                 
747                 
748
749
750         }
751
752         /// Sphere (for convex0) against Convex1
753         {
754                 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
755
756                 btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
757                 btConvexCast::CastResult result;
758                 btVoronoiSimplexSolver voronoiSimplex;
759                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
760                 ///Simplification, one object is simplified as a sphere
761                 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
762                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
763                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
764                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
765                 {
766                 
767                         //store result.m_fraction in both bodies
768                 
769                         if (col0->getHitFraction()      > result.m_fraction)
770                                 col0->setHitFraction( result.m_fraction);
771
772                         if (col1->getHitFraction() > result.m_fraction)
773                                 col1->setHitFraction( result.m_fraction);
774
775                         if (resultFraction > result.m_fraction)
776                                 resultFraction = result.m_fraction;
777
778                 }
779         }
780         
781         return resultFraction;
782
783 }
784