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