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
segmentsClosestPoints(btVector3 & ptsVector,btVector3 & offsetA,btVector3 & offsetB,btScalar & tA,btScalar & tB,const btVector3 & translation,const btVector3 & dirA,btScalar hlenA,const btVector3 & dirB,btScalar hlenB)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
capsuleCapsuleDistance(btVector3 & normalOnB,btVector3 & pointOnB,btScalar capsuleLengthA,btScalar capsuleRadiusA,btScalar capsuleLengthB,btScalar capsuleRadiusB,int capsuleAxisA,int capsuleAxisB,const btTransform & transformA,const btTransform & transformB,btScalar distanceThreshold)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
CreateFunc(btSimplexSolverInterface * simplexSolver,btConvexPenetrationDepthSolver * pdSolver)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
~CreateFunc()190 btConvexConvexAlgorithm::CreateFunc::~CreateFunc()
191 {
192 }
193
btConvexConvexAlgorithm(btPersistentManifold * mf,const btCollisionAlgorithmConstructionInfo & ci,btCollisionObject * body0,btCollisionObject * body1,btSimplexSolverInterface * simplexSolver,btConvexPenetrationDepthSolver * pdSolver,int numPerturbationIterations,int minimumPointsPerturbationThreshold)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
~btConvexConvexAlgorithm()215 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
216 {
217 if (m_ownManifold)
218 {
219 if (m_manifoldPtr)
220 m_dispatcher->releaseManifold(m_manifoldPtr);
221 }
222 }
223
setLowLevelOfDetail(bool useLowLevel)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
btPerturbedContactResultbtPerturbedContactResult240 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 }
~btPerturbedContactResultbtPerturbedContactResult249 virtual ~ btPerturbedContactResult()
250 {
251 }
252
addContactPointbtPerturbedContactResult253 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 //
processCollision(btCollisionObject * body0,btCollisionObject * body1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)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;
calculateTimeOfImpact(btCollisionObject * col0,btCollisionObject * col1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)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
740