void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { (void)dispatchInfo; if (!m_manifoldPtr) return; btCollisionObject* convexObj = m_isSwapped? body1 : body0; btCollisionObject* planeObj = m_isSwapped? body0: body1; btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); const btVector3& planeNormal = planeShape->getPlaneNormal(); //const btScalar& planeConstant = planeShape->getPlaneConstant(); //first perform a collision query with the non-perturbated collision objects { btQuaternion rotq(0,0,0,1); collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut); } if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) { btVector3 v0,v1; btPlaneSpace1(planeNormal,v0,v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radius = convexShape->getAngularMotionDisc(); perturbeAngle = gContactBreakingThreshold / radius; if ( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btQuaternion perturbeRot(v0,perturbeAngle); for (int i=0;i<m_numPerturbationIterations;i++) { btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); btQuaternion rotq(planeNormal,iterationAngle); collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0,body1,dispatchInfo,resultOut); } } if (m_ownManifold) { if (m_manifoldPtr->getNumContacts()) { resultOut->refreshContactPoints(); } } }
void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { (void)dispatchInfo; if (!m_manifoldPtr) return; const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap; const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap; btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); btTransform planeInConvex; planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform(); btTransform convexInPlaneTrans; convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform(); btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); resultOut->setPersistentManifold(m_manifoldPtr); if (hasCollision) { /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; btVector3 pOnB = vtxInPlaneWorld; resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); } //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones: //they keep on rolling forever because of the additional off-center contact points //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc) if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) { btVector3 v0,v1; btPlaneSpace1(planeNormal,v0,v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radius = convexShape->getAngularMotionDisc(); perturbeAngle = gContactBreakingThreshold / radius; if ( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btQuaternion perturbeRot(v0,perturbeAngle); for (int i=0;i<m_numPerturbationIterations;i++) { btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); btQuaternion rotq(planeNormal,iterationAngle); collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut); } } if (m_ownManifold) { if (m_manifoldPtr->getNumContacts()) { resultOut->refreshContactPoints(); } } }
// // Convex-Convex collision algorithm // void btConvexConvexAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) { //swapped? m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape()); const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape()); btVector3 normalOnB; btVector3 pointOnBWorld; #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) { btCapsuleShape* capsuleA = (btCapsuleShape*) min0; btCapsuleShape* capsuleB = (btCapsuleShape*) min1; // btVector3 localScalingA = capsuleA->getLocalScaling(); // btVector3 localScalingB = capsuleB->getLocalScaling(); btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(), capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(), body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); if (dist<threshold) { btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); } resultOut->refreshContactPoints(); return; } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(), body1->getWorldTransform()); } if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) #endif //USE_SEPDISTANCE_UTIL2 btGjkPairDetector::ClosestPointInput input; btGjkPairDetector gjkPairDetector(min0, min1, &m_simplexSolver, m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA(min0); gjkPairDetector.setMinkowskiB(min1); #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } else #endif //USE_SEPDISTANCE_UTIL2 { //if (dispatchInfo.m_convexMaxDistanceUseCPT) //{ // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); //} else //{ input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); // } input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB = body1Wrap->getWorldTransform(); #ifdef USE_SEPDISTANCE_UTIL2 btScalar sepDist = 0.f; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist>SIMD_EPSILON) { sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; //now perturbe directions to get multiple contact points } } #endif //USE_SEPDISTANCE_UTIL2 if (min0->isPolyhedral() && min1->isPolyhedral()) { struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result { virtual void setShapeIdentifiersA(int partId0, int index0){} virtual void setShapeIdentifiersB(int partId1, int index1){} virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { } }; struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result { btDiscreteCollisionDetectorInterface::Result* m_originalResult; btVector3 m_reportedNormalOnWorld; btScalar m_marginOnA; btScalar m_marginOnB; btScalar m_reportedDistance; bool m_foundResult; btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB) :m_originalResult(result), m_marginOnA(marginOnA), m_marginOnB(marginOnB), m_foundResult(false) { } virtual void setShapeIdentifiersA(int partId0, int index0){} virtual void setShapeIdentifiersB(int partId1, int index1){} virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorldOrg, btScalar depthOrg) { m_reportedDistance = depthOrg; m_reportedNormalOnWorld = normalOnBInWorld; btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB; m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB); if (m_reportedDistance<0.f) { m_foundResult = true; } m_originalResult->addContactPoint(normalOnBInWorld, adjustedPointB, m_reportedDistance); } }; btDummyResult dummy; ///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin(); btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin(); btWithoutMarginResult withoutMargin(resultOut, min0Margin, min1Margin); btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0; btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1; if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) { btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); btScalar minDist = -1e30f; btVector3 sepNormalWorldSpace; bool foundSepAxis = true; if (dispatchInfo.m_enableSatConvex) { foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), sepNormalWorldSpace,*resultOut); } else { #ifdef ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); gjkPairDetector.getClosestPoints(input,*resultOut, dispatchInfo.m_debugDraw); #else gjkPairDetector.getClosestPoints(input, withoutMargin, dispatchInfo.m_debugDraw); //gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw); #endif //ZERO_MARGIN //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); //if (l2>SIMD_EPSILON) { sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin(); #ifdef ZERO_MARGIN foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; #else foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin()); #endif } } if (foundSepAxis) { //printf("sepNormalWorldSpace=%f, %f, %f\n", sepNormalWorldSpace.getX(), sepNormalWorldSpace.getY(), sepNormalWorldSpace.getZ()); btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut); } if (m_ownManifold) { resultOut->refreshContactPoints(); } return; } else { //we can also deal with convex versus triangle (without connectivity data) if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) { btVertexArray vertices; btTriangleShape* tri = (btTriangleShape*)polyhedronB; vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]); vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]); vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]); //tri->initializePolyhedralFeatures(); btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); btVector3 sepNormalWorldSpace; btScalar minDist =-1e30f; btScalar maxDist = threshold; bool foundSepAxis = false; if (0) { polyhedronB->initializePolyhedralFeatures(); foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), sepNormalWorldSpace,*resultOut); // printf("sepNormalWorldSpace=%f, %f, %f\n", sepNormalWorldSpace.getX(), sepNormalWorldSpace.getY(), sepNormalWorldSpace.getZ()); } else { #ifdef ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); gjkPairDetector.getClosestPoints(input,*resultOut, dispatchInfo.m_debugDraw); #else gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw); #endif//ZERO_MARGIN btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if (l2>SIMD_EPSILON) { sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); //minDist = gjkPairDetector.getCachedSeparatingDistance(); //maxDist = threshold; minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); foundSepAxis = true; } } if (foundSepAxis) { btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut); } if (m_ownManifold) { resultOut->refreshContactPoints(); } return; } } } gjkPairDetector.getClosestPoints(input,*resultOut, dispatchInfo.m_debugDraw); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { int i; btVector3 v0, v1; btVector3 sepNormalWorldSpace; btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if (l2 > SIMD_EPSILON) { sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); btPlaneSpace1(sepNormalWorldSpace, v0, v1); bool perturbeA = true; const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radiusA = min0->getAngularMotionDisc(); btScalar radiusB = min1->getAngularMotionDisc(); if (radiusA < radiusB) { perturbeAngle = gContactBreakingThreshold /radiusA; perturbeA = true; } else { perturbeAngle = gContactBreakingThreshold / radiusB; perturbeA = false; } if ( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btTransform unPerturbedTransform; if (perturbeA) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for ( i=0;i<m_numPerturbationIterations;i++) { if (v0.length2()>SIMD_EPSILON) { btQuaternion perturbeRot(v0, perturbeAngle); btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); btQuaternion rotq(sepNormalWorldSpace, iterationAngle); if (perturbeA) { input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis()); input.m_transformB = body1Wrap->getWorldTransform(); #ifdef DEBUG_CONTACTS dispatchInfo.m_debugDraw->drawTransform(input.m_transformA, 5.0); #endif //DEBUG_CONTACTS } else { input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis()); #ifdef DEBUG_CONTACTS dispatchInfo.m_debugDraw->drawTransform(input.m_transformB, 5.0); #endif } btPerturbedContactResult perturbedResultOut(resultOut, input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw); gjkPairDetector.getClosestPoints(input, perturbedResultOut, dispatchInfo.m_debugDraw); } } } } #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0->getWorldTransform(), body1->getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 if (m_ownManifold) { resultOut->refreshContactPoints(); } }
// // Convex-Convex collision algorithm // void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { if (!m_manifoldPtr) { //swapped? m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape()); btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape()); btVector3 normalOnB; btVector3 pointOnBWorld; #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) { btCapsuleShape* capsuleA = (btCapsuleShape*) min0; btCapsuleShape* capsuleB = (btCapsuleShape*) min1; btVector3 localScalingA = capsuleA->getLocalScaling(); btVector3 localScalingB = capsuleB->getLocalScaling(); btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(), body0->getWorldTransform(),body1->getWorldTransform(),threshold); if (dist<threshold) { btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); } resultOut->refreshContactPoints(); return; } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); } if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) #endif //USE_SEPDISTANCE_UTIL2 { btGjkPairDetector::ClosestPointInput input; btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA(min0); gjkPairDetector.setMinkowskiB(min1); #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } else #endif //USE_SEPDISTANCE_UTIL2 { if (dispatchInfo.m_convexMaxDistanceUseCPT) { input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); } else { input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); } input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); #ifdef USE_SEPDISTANCE_UTIL2 btScalar sepDist = 0.f; if (dispatchInfo.m_useConvexConservativeDistanceUtil) { sepDist = gjkPairDetector.getCachedSeparatingDistance(); if (sepDist>SIMD_EPSILON) { sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; //now perturbe directions to get multiple contact points } } #endif //USE_SEPDISTANCE_UTIL2 //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { int i; btVector3 v0,v1; btVector3 sepNormalWorldSpace; sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); btPlaneSpace1(sepNormalWorldSpace,v0,v1); bool perturbeA = true; const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radiusA = min0->getAngularMotionDisc(); btScalar radiusB = min1->getAngularMotionDisc(); if (radiusA < radiusB) { perturbeAngle = gContactBreakingThreshold /radiusA; perturbeA = true; } else { perturbeAngle = gContactBreakingThreshold / radiusB; perturbeA = false; } if ( perturbeAngle > angleLimit ) perturbeAngle = angleLimit; btTransform unPerturbedTransform; if (perturbeA) { unPerturbedTransform = input.m_transformA; } else { unPerturbedTransform = input.m_transformB; } for ( i=0;i<m_numPerturbationIterations;i++) { if (v0.length2()>SIMD_EPSILON) { btQuaternion perturbeRot(v0,perturbeAngle); btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); btQuaternion rotq(sepNormalWorldSpace,iterationAngle); if (perturbeA) { input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis()); input.m_transformB = body1->getWorldTransform(); #ifdef DEBUG_CONTACTS dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); #endif //DEBUG_CONTACTS } else { input.m_transformA = body0->getWorldTransform(); input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis()); #ifdef DEBUG_CONTACTS dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); #endif } btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); } } } #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); } #endif //USE_SEPDISTANCE_UTIL2 } if (m_ownManifold) { resultOut->refreshContactPoints(); } }