00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00019
00020
00021
00022 #include "btConvexConvexAlgorithm.h"
00023
00024
00025 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
00026 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
00027 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00028 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00029 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
00030 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00031
00032
00033
00034 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
00035 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
00036 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00037 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00038 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
00039
00040 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
00041 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
00042 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00043 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
00044
00045
00046
00047 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
00048 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00049
00050 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
00051
00052 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
00053 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
00054 #include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
00055 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00056
00058
00059
00060
00061 static SIMD_FORCE_INLINE void segmentsClosestPoints(
00062 btVector3& ptsVector,
00063 btVector3& offsetA,
00064 btVector3& offsetB,
00065 btScalar& tA, btScalar& tB,
00066 const btVector3& translation,
00067 const btVector3& dirA, btScalar hlenA,
00068 const btVector3& dirB, btScalar hlenB )
00069 {
00070
00071
00072 btScalar dirA_dot_dirB = btDot(dirA,dirB);
00073 btScalar dirA_dot_trans = btDot(dirA,translation);
00074 btScalar dirB_dot_trans = btDot(dirB,translation);
00075
00076 btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
00077
00078 if ( denom == 0.0f ) {
00079 tA = 0.0f;
00080 } else {
00081 tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
00082 if ( tA < -hlenA )
00083 tA = -hlenA;
00084 else if ( tA > hlenA )
00085 tA = hlenA;
00086 }
00087
00088 tB = tA * dirA_dot_dirB - dirB_dot_trans;
00089
00090 if ( tB < -hlenB ) {
00091 tB = -hlenB;
00092 tA = tB * dirA_dot_dirB + dirA_dot_trans;
00093
00094 if ( tA < -hlenA )
00095 tA = -hlenA;
00096 else if ( tA > hlenA )
00097 tA = hlenA;
00098 } else if ( tB > hlenB ) {
00099 tB = hlenB;
00100 tA = tB * dirA_dot_dirB + dirA_dot_trans;
00101
00102 if ( tA < -hlenA )
00103 tA = -hlenA;
00104 else if ( tA > hlenA )
00105 tA = hlenA;
00106 }
00107
00108
00109
00110 offsetA = dirA * tA;
00111 offsetB = dirB * tB;
00112
00113 ptsVector = translation - offsetA + offsetB;
00114 }
00115
00116
00117 static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
00118 btVector3& normalOnB,
00119 btVector3& pointOnB,
00120 btScalar capsuleLengthA,
00121 btScalar capsuleRadiusA,
00122 btScalar capsuleLengthB,
00123 btScalar capsuleRadiusB,
00124 int capsuleAxisA,
00125 int capsuleAxisB,
00126 const btTransform& transformA,
00127 const btTransform& transformB,
00128 btScalar distanceThreshold )
00129 {
00130 btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
00131 btVector3 translationA = transformA.getOrigin();
00132 btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
00133 btVector3 translationB = transformB.getOrigin();
00134
00135
00136
00137 btVector3 translation = translationB - translationA;
00138
00139
00140
00141 btVector3 ptsVector;
00142
00143 btVector3 offsetA, offsetB;
00144 btScalar tA, tB;
00145
00146 segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
00147 directionA, capsuleLengthA, directionB, capsuleLengthB );
00148
00149 btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
00150
00151 if ( distance > distanceThreshold )
00152 return distance;
00153
00154 btScalar lenSqr = ptsVector.length2();
00155 if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
00156 {
00157
00158 btVector3 q;
00159 btPlaneSpace1(directionA,normalOnB,q);
00160 } else
00161 {
00162
00163 normalOnB = ptsVector*-btRecipSqrt(lenSqr);
00164 }
00165 pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
00166
00167 return distance;
00168 }
00169
00170
00171
00172
00173
00174
00175
00177
00178
00179
00180
00181
00182 btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
00183 {
00184 m_numPerturbationIterations = 0;
00185 m_minimumPointsPerturbationThreshold = 3;
00186 m_simplexSolver = simplexSolver;
00187 m_pdSolver = pdSolver;
00188 }
00189
00190 btConvexConvexAlgorithm::CreateFunc::~CreateFunc()
00191 {
00192 }
00193
00194 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
00195 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
00196 m_simplexSolver(simplexSolver),
00197 m_pdSolver(pdSolver),
00198 m_ownManifold (false),
00199 m_manifoldPtr(mf),
00200 m_lowLevelOfDetail(false),
00201 #ifdef USE_SEPDISTANCE_UTIL2
00202 m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
00203 (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
00204 #endif
00205 m_numPerturbationIterations(numPerturbationIterations),
00206 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
00207 {
00208 (void)body0Wrap;
00209 (void)body1Wrap;
00210 }
00211
00212
00213
00214
00215 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
00216 {
00217 if (m_ownManifold)
00218 {
00219 if (m_manifoldPtr)
00220 m_dispatcher->releaseManifold(m_manifoldPtr);
00221 }
00222 }
00223
00224 void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
00225 {
00226 m_lowLevelOfDetail = useLowLevel;
00227 }
00228
00229
00230 struct btPerturbedContactResult : public btManifoldResult
00231 {
00232 btManifoldResult* m_originalManifoldResult;
00233 btTransform m_transformA;
00234 btTransform m_transformB;
00235 btTransform m_unPerturbedTransform;
00236 bool m_perturbA;
00237 btIDebugDraw* m_debugDrawer;
00238
00239
00240 btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
00241 :m_originalManifoldResult(originalResult),
00242 m_transformA(transformA),
00243 m_transformB(transformB),
00244 m_unPerturbedTransform(unPerturbedTransform),
00245 m_perturbA(perturbA),
00246 m_debugDrawer(debugDrawer)
00247 {
00248 }
00249 virtual ~ btPerturbedContactResult()
00250 {
00251 }
00252
00253 virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
00254 {
00255 btVector3 endPt,startPt;
00256 btScalar newDepth;
00257 btVector3 newNormal;
00258
00259 if (m_perturbA)
00260 {
00261 btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
00262 endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
00263 newDepth = (endPt - pointInWorld).dot(normalOnBInWorld);
00264 startPt = endPt+normalOnBInWorld*newDepth;
00265 } else
00266 {
00267 endPt = pointInWorld + normalOnBInWorld*orgDepth;
00268 startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
00269 newDepth = (endPt - startPt).dot(normalOnBInWorld);
00270
00271 }
00272
00273
00274 #ifdef DEBUG_CONTACTS
00275 m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
00276 m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
00277 m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
00278 #endif //DEBUG_CONTACTS
00279
00280
00281 m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
00282 }
00283
00284 };
00285
00286 extern btScalar gContactBreakingThreshold;
00287
00288
00289
00290
00291
00292 void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00293 {
00294
00295 if (!m_manifoldPtr)
00296 {
00297
00298 m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
00299 m_ownManifold = true;
00300 }
00301 resultOut->setPersistentManifold(m_manifoldPtr);
00302
00303
00304
00305
00306
00307 const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
00308 const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
00309
00310 btVector3 normalOnB;
00311 btVector3 pointOnBWorld;
00312 #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
00313 if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
00314 {
00315 btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
00316 btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
00317
00318
00319
00320 btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
00321
00322 btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
00323 capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
00324 body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
00325
00326 if (dist<threshold)
00327 {
00328 btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
00329 resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);
00330 }
00331 resultOut->refreshContactPoints();
00332 return;
00333 }
00334 #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
00335
00336
00337
00338
00339 #ifdef USE_SEPDISTANCE_UTIL2
00340 if (dispatchInfo.m_useConvexConservativeDistanceUtil)
00341 {
00342 m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
00343 }
00344
00345 if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
00346 #endif
00347
00348 {
00349
00350
00351 btGjkPairDetector::ClosestPointInput input;
00352
00353 btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
00354
00355 gjkPairDetector.setMinkowskiA(min0);
00356 gjkPairDetector.setMinkowskiB(min1);
00357
00358 #ifdef USE_SEPDISTANCE_UTIL2
00359 if (dispatchInfo.m_useConvexConservativeDistanceUtil)
00360 {
00361 input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
00362 } else
00363 #endif //USE_SEPDISTANCE_UTIL2
00364 {
00365
00366
00367
00368
00369
00370 input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
00371
00372
00373 input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
00374 }
00375
00376 input.m_stackAlloc = dispatchInfo.m_stackAllocator;
00377 input.m_transformA = body0Wrap->getWorldTransform();
00378 input.m_transformB = body1Wrap->getWorldTransform();
00379
00380
00381
00382
00383
00384 #ifdef USE_SEPDISTANCE_UTIL2
00385 btScalar sepDist = 0.f;
00386 if (dispatchInfo.m_useConvexConservativeDistanceUtil)
00387 {
00388 sepDist = gjkPairDetector.getCachedSeparatingDistance();
00389 if (sepDist>SIMD_EPSILON)
00390 {
00391 sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
00392
00393
00394 }
00395 }
00396 #endif //USE_SEPDISTANCE_UTIL2
00397
00398 if (min0->isPolyhedral() && min1->isPolyhedral())
00399 {
00400
00401
00402 struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
00403 {
00404 virtual void setShapeIdentifiersA(int partId0,int index0){}
00405 virtual void setShapeIdentifiersB(int partId1,int index1){}
00406 virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
00407 {
00408 }
00409 };
00410
00411
00412 struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
00413 {
00414 btDiscreteCollisionDetectorInterface::Result* m_originalResult;
00415 btVector3 m_reportedNormalOnWorld;
00416 btScalar m_marginOnA;
00417 btScalar m_marginOnB;
00418 btScalar m_reportedDistance;
00419
00420 bool m_foundResult;
00421 btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
00422 :m_originalResult(result),
00423 m_marginOnA(marginOnA),
00424 m_marginOnB(marginOnB),
00425 m_foundResult(false)
00426 {
00427 }
00428
00429 virtual void setShapeIdentifiersA(int partId0,int index0){}
00430 virtual void setShapeIdentifiersB(int partId1,int index1){}
00431 virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg)
00432 {
00433 m_reportedDistance = depthOrg;
00434 m_reportedNormalOnWorld = normalOnBInWorld;
00435
00436 btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
00437 m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
00438 if (m_reportedDistance<0.f)
00439 {
00440 m_foundResult = true;
00441 }
00442 m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
00443 }
00444 };
00445
00446
00447 btDummyResult dummy;
00448
00450
00451 btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
00452 btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
00453
00454 btWithoutMarginResult withoutMargin(resultOut, min0Margin,min1Margin);
00455
00456 btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
00457 btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
00458 if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
00459 {
00460
00461
00462
00463
00464 btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
00465
00466 btScalar minDist = -1e30f;
00467 btVector3 sepNormalWorldSpace;
00468 bool foundSepAxis = true;
00469
00470 if (dispatchInfo.m_enableSatConvex)
00471 {
00472 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
00473 *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
00474 body0Wrap->getWorldTransform(),
00475 body1Wrap->getWorldTransform(),
00476 sepNormalWorldSpace,*resultOut);
00477 } else
00478 {
00479 #ifdef ZERO_MARGIN
00480 gjkPairDetector.setIgnoreMargin(true);
00481 gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
00482 #else
00483
00484
00485 gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
00486
00487 #endif //ZERO_MARGIN
00488
00489
00490 {
00491 sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;
00492
00493 minDist = withoutMargin.m_reportedDistance;
00494
00495 #ifdef ZERO_MARGIN
00496 foundSepAxis = true;
00497 #else
00498 foundSepAxis = withoutMargin.m_foundResult && minDist<0;
00499 #endif
00500 }
00501 }
00502 if (foundSepAxis)
00503 {
00504
00505
00506
00507 btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
00508 body0Wrap->getWorldTransform(),
00509 body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut);
00510
00511 }
00512 if (m_ownManifold)
00513 {
00514 resultOut->refreshContactPoints();
00515 }
00516 return;
00517
00518 } else
00519 {
00520
00521 if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
00522 {
00523
00524 btVertexArray vertices;
00525 btTriangleShape* tri = (btTriangleShape*)polyhedronB;
00526 vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
00527 vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
00528 vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
00529
00530
00531
00532 btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
00533
00534 btVector3 sepNormalWorldSpace;
00535 btScalar minDist =-1e30f;
00536 btScalar maxDist = threshold;
00537
00538 bool foundSepAxis = false;
00539 if (0)
00540 {
00541 polyhedronB->initializePolyhedralFeatures();
00542 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
00543 *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
00544 body0Wrap->getWorldTransform(),
00545 body1Wrap->getWorldTransform(),
00546 sepNormalWorldSpace,*resultOut);
00547
00548
00549 } else
00550 {
00551 #ifdef ZERO_MARGIN
00552 gjkPairDetector.setIgnoreMargin(true);
00553 gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
00554 #else
00555 gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
00556 #endif//ZERO_MARGIN
00557
00558 btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
00559 if (l2>SIMD_EPSILON)
00560 {
00561 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
00562
00563
00564 minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
00565 foundSepAxis = true;
00566 }
00567 }
00568
00569
00570 if (foundSepAxis)
00571 {
00572 btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
00573 body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
00574 }
00575
00576
00577 if (m_ownManifold)
00578 {
00579 resultOut->refreshContactPoints();
00580 }
00581
00582 return;
00583 }
00584
00585 }
00586
00587
00588 }
00589
00590 gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
00591
00592
00593
00594
00595 if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
00596 {
00597
00598 int i;
00599 btVector3 v0,v1;
00600 btVector3 sepNormalWorldSpace;
00601 btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
00602
00603 if (l2>SIMD_EPSILON)
00604 {
00605 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
00606
00607 btPlaneSpace1(sepNormalWorldSpace,v0,v1);
00608
00609
00610 bool perturbeA = true;
00611 const btScalar angleLimit = 0.125f * SIMD_PI;
00612 btScalar perturbeAngle;
00613 btScalar radiusA = min0->getAngularMotionDisc();
00614 btScalar radiusB = min1->getAngularMotionDisc();
00615 if (radiusA < radiusB)
00616 {
00617 perturbeAngle = gContactBreakingThreshold /radiusA;
00618 perturbeA = true;
00619 } else
00620 {
00621 perturbeAngle = gContactBreakingThreshold / radiusB;
00622 perturbeA = false;
00623 }
00624 if ( perturbeAngle > angleLimit )
00625 perturbeAngle = angleLimit;
00626
00627 btTransform unPerturbedTransform;
00628 if (perturbeA)
00629 {
00630 unPerturbedTransform = input.m_transformA;
00631 } else
00632 {
00633 unPerturbedTransform = input.m_transformB;
00634 }
00635
00636 for ( i=0;i<m_numPerturbationIterations;i++)
00637 {
00638 if (v0.length2()>SIMD_EPSILON)
00639 {
00640 btQuaternion perturbeRot(v0,perturbeAngle);
00641 btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
00642 btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
00643
00644
00645 if (perturbeA)
00646 {
00647 input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis());
00648 input.m_transformB = body1Wrap->getWorldTransform();
00649 #ifdef DEBUG_CONTACTS
00650 dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
00651 #endif //DEBUG_CONTACTS
00652 } else
00653 {
00654 input.m_transformA = body0Wrap->getWorldTransform();
00655 input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis());
00656 #ifdef DEBUG_CONTACTS
00657 dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
00658 #endif
00659 }
00660
00661 btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
00662 gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
00663 }
00664 }
00665 }
00666 }
00667
00668
00669
00670 #ifdef USE_SEPDISTANCE_UTIL2
00671 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
00672 {
00673 m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
00674 }
00675 #endif //USE_SEPDISTANCE_UTIL2
00676
00677
00678 }
00679
00680 if (m_ownManifold)
00681 {
00682 resultOut->refreshContactPoints();
00683 }
00684
00685 }
00686
00687
00688
00689 bool disableCcd = false;
00690 btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00691 {
00692 (void)resultOut;
00693 (void)dispatchInfo;
00695
00698 btScalar resultFraction = btScalar(1.);
00699
00700
00701 btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
00702 btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
00703
00704 if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
00705 squareMot1 < col1->getCcdSquareMotionThreshold())
00706 return resultFraction;
00707
00708 if (disableCcd)
00709 return btScalar(1.);
00710
00711
00712
00713
00714
00715
00716
00717
00718
00720 {
00721 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
00722
00723 btSphereShape sphere1(col1->getCcdSweptSphereRadius());
00724 btConvexCast::CastResult result;
00725 btVoronoiSimplexSolver voronoiSimplex;
00726
00728 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
00729
00730 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
00731 col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
00732 {
00733
00734
00735
00736 if (col0->getHitFraction()> result.m_fraction)
00737 col0->setHitFraction( result.m_fraction );
00738
00739 if (col1->getHitFraction() > result.m_fraction)
00740 col1->setHitFraction( result.m_fraction);
00741
00742 if (resultFraction > result.m_fraction)
00743 resultFraction = result.m_fraction;
00744
00745 }
00746
00747
00748
00749
00750 }
00751
00753 {
00754 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
00755
00756 btSphereShape sphere0(col0->getCcdSweptSphereRadius());
00757 btConvexCast::CastResult result;
00758 btVoronoiSimplexSolver voronoiSimplex;
00759
00761 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
00762
00763 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
00764 col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
00765 {
00766
00767
00768
00769 if (col0->getHitFraction() > result.m_fraction)
00770 col0->setHitFraction( result.m_fraction);
00771
00772 if (col1->getHitFraction() > result.m_fraction)
00773 col1->setHitFraction( result.m_fraction);
00774
00775 if (resultFraction > result.m_fraction)
00776 resultFraction = result.m_fraction;
00777
00778 }
00779 }
00780
00781 return resultFraction;
00782
00783 }
00784