| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" |
| #include "BulletCollision/CollisionShapes/btConvexShape.h" |
| #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" |
| #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" |
| |
| |
| |
| #if defined(DEBUG) || defined (_DEBUG) |
| //#define TEST_NON_VIRTUAL 1 |
| #include <stdio.h> //for debug printf |
| #ifdef __SPU__ |
| #include <spu_printf.h> |
| #define printf spu_printf |
| //#define DEBUG_SPU_COLLISION_DETECTION 1 |
| #endif //__SPU__ |
| #endif |
| |
| //must be above the machine epsilon |
| #define REL_ERROR2 btScalar(1.0e-6) |
| |
| //temp globals, to improve GJK/EPA/penetration calculations |
| int gNumDeepPenetrationChecks = 0; |
| int gNumGjkChecks = 0; |
| |
| |
| btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) |
| :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), |
| m_penetrationDepthSolver(penetrationDepthSolver), |
| m_simplexSolver(simplexSolver), |
| m_minkowskiA(objectA), |
| m_minkowskiB(objectB), |
| m_shapeTypeA(objectA->getShapeType()), |
| m_shapeTypeB(objectB->getShapeType()), |
| m_marginA(objectA->getMargin()), |
| m_marginB(objectB->getMargin()), |
| m_ignoreMargin(false), |
| m_lastUsedMethod(-1), |
| m_catchDegeneracies(1) |
| { |
| } |
| btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) |
| :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), |
| m_penetrationDepthSolver(penetrationDepthSolver), |
| m_simplexSolver(simplexSolver), |
| m_minkowskiA(objectA), |
| m_minkowskiB(objectB), |
| m_shapeTypeA(shapeTypeA), |
| m_shapeTypeB(shapeTypeB), |
| m_marginA(marginA), |
| m_marginB(marginB), |
| m_ignoreMargin(false), |
| m_lastUsedMethod(-1), |
| m_catchDegeneracies(1) |
| { |
| } |
| |
| void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) |
| { |
| (void)swapResults; |
| |
| getClosestPointsNonVirtual(input,output,debugDraw); |
| } |
| |
| #ifdef __SPU__ |
| void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) |
| #else |
| void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) |
| #endif |
| { |
| m_cachedSeparatingDistance = 0.f; |
| |
| btScalar distance=btScalar(0.); |
| btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); |
| btVector3 pointOnA,pointOnB; |
| btTransform localTransA = input.m_transformA; |
| btTransform localTransB = input.m_transformB; |
| btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); |
| localTransA.getOrigin() -= positionOffset; |
| localTransB.getOrigin() -= positionOffset; |
| |
| bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d(); |
| |
| btScalar marginA = m_marginA; |
| btScalar marginB = m_marginB; |
| |
| gNumGjkChecks++; |
| |
| #ifdef DEBUG_SPU_COLLISION_DETECTION |
| spu_printf("inside gjk\n"); |
| #endif |
| //for CCD we don't use margins |
| if (m_ignoreMargin) |
| { |
| marginA = btScalar(0.); |
| marginB = btScalar(0.); |
| #ifdef DEBUG_SPU_COLLISION_DETECTION |
| spu_printf("ignoring margin\n"); |
| #endif |
| } |
| |
| m_curIter = 0; |
| int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? |
| m_cachedSeparatingAxis.setValue(0,1,0); |
| |
| bool isValid = false; |
| bool checkSimplex = false; |
| bool checkPenetration = true; |
| m_degenerateSimplex = 0; |
| |
| m_lastUsedMethod = -1; |
| |
| { |
| btScalar squaredDistance = BT_LARGE_FLOAT; |
| btScalar delta = btScalar(0.); |
| |
| btScalar margin = marginA + marginB; |
| |
| |
| |
| m_simplexSolver->reset(); |
| |
| for ( ; ; ) |
| //while (true) |
| { |
| |
| btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); |
| btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); |
| |
| #if 1 |
| |
| btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); |
| btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); |
| |
| // btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA); |
| // btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB); |
| |
| #else |
| #ifdef __SPU__ |
| btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); |
| btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); |
| #else |
| btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); |
| btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); |
| #ifdef TEST_NON_VIRTUAL |
| btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); |
| btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); |
| btAssert((pInAv-pInA).length() < 0.0001); |
| btAssert((qInBv-qInB).length() < 0.0001); |
| #endif // |
| #endif //__SPU__ |
| #endif |
| |
| |
| btVector3 pWorld = localTransA(pInA); |
| btVector3 qWorld = localTransB(qInB); |
| |
| #ifdef DEBUG_SPU_COLLISION_DETECTION |
| spu_printf("got local supporting vertices\n"); |
| #endif |
| |
| if (check2d) |
| { |
| pWorld[2] = 0.f; |
| qWorld[2] = 0.f; |
| } |
| |
| btVector3 w = pWorld - qWorld; |
| delta = m_cachedSeparatingAxis.dot(w); |
| |
| // potential exit, they don't overlap |
| if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) |
| { |
| m_degenerateSimplex = 10; |
| checkSimplex=true; |
| //checkPenetration = false; |
| break; |
| } |
| |
| //exit 0: the new point is already in the simplex, or we didn't come any closer |
| if (m_simplexSolver->inSimplex(w)) |
| { |
| m_degenerateSimplex = 1; |
| checkSimplex = true; |
| break; |
| } |
| // are we getting any closer ? |
| btScalar f0 = squaredDistance - delta; |
| btScalar f1 = squaredDistance * REL_ERROR2; |
| |
| if (f0 <= f1) |
| { |
| if (f0 <= btScalar(0.)) |
| { |
| m_degenerateSimplex = 2; |
| } else |
| { |
| m_degenerateSimplex = 11; |
| } |
| checkSimplex = true; |
| break; |
| } |
| |
| #ifdef DEBUG_SPU_COLLISION_DETECTION |
| spu_printf("addVertex 1\n"); |
| #endif |
| //add current vertex to simplex |
| m_simplexSolver->addVertex(w, pWorld, qWorld); |
| #ifdef DEBUG_SPU_COLLISION_DETECTION |
| spu_printf("addVertex 2\n"); |
| #endif |
| btVector3 newCachedSeparatingAxis; |
| |
| //calculate the closest point to the origin (update vector v) |
| if (!m_simplexSolver->closest(newCachedSeparatingAxis)) |
| { |
| m_degenerateSimplex = 3; |
| checkSimplex = true; |
| break; |
| } |
| |
| if(newCachedSeparatingAxis.length2()<REL_ERROR2) |
| { |
| m_cachedSeparatingAxis = newCachedSeparatingAxis; |
| m_degenerateSimplex = 6; |
| checkSimplex = true; |
| break; |
| } |
| |
| btScalar previousSquaredDistance = squaredDistance; |
| squaredDistance = newCachedSeparatingAxis.length2(); |
| #if 0 |
| ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo |
| if (squaredDistance>previousSquaredDistance) |
| { |
| m_degenerateSimplex = 7; |
| squaredDistance = previousSquaredDistance; |
| checkSimplex = false; |
| break; |
| } |
| #endif // |
| |
| m_cachedSeparatingAxis = newCachedSeparatingAxis; |
| |
| //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); |
| |
| //are we getting any closer ? |
| if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) |
| { |
| m_simplexSolver->backup_closest(m_cachedSeparatingAxis); |
| checkSimplex = true; |
| m_degenerateSimplex = 12; |
| |
| break; |
| } |
| |
| //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject |
| if (m_curIter++ > gGjkMaxIter) |
| { |
| #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION) |
| |
| printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); |
| printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", |
| m_cachedSeparatingAxis.getX(), |
| m_cachedSeparatingAxis.getY(), |
| m_cachedSeparatingAxis.getZ(), |
| squaredDistance, |
| m_minkowskiA->getShapeType(), |
| m_minkowskiB->getShapeType()); |
| |
| #endif |
| break; |
| |
| } |
| |
| |
| bool check = (!m_simplexSolver->fullSimplex()); |
| //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); |
| |
| if (!check) |
| { |
| //do we need this backup_closest here ? |
| m_simplexSolver->backup_closest(m_cachedSeparatingAxis); |
| m_degenerateSimplex = 13; |
| break; |
| } |
| } |
| |
| if (checkSimplex) |
| { |
| m_simplexSolver->compute_points(pointOnA, pointOnB); |
| normalInB = pointOnA-pointOnB; |
| btScalar lenSqr =m_cachedSeparatingAxis.length2(); |
| |
| //valid normal |
| if (lenSqr < 0.0001) |
| { |
| m_degenerateSimplex = 5; |
| } |
| if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) |
| { |
| btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); |
| normalInB *= rlen; //normalize |
| btScalar s = btSqrt(squaredDistance); |
| |
| btAssert(s > btScalar(0.0)); |
| pointOnA -= m_cachedSeparatingAxis * (marginA / s); |
| pointOnB += m_cachedSeparatingAxis * (marginB / s); |
| distance = ((btScalar(1.)/rlen) - margin); |
| isValid = true; |
| |
| m_lastUsedMethod = 1; |
| } else |
| { |
| m_lastUsedMethod = 2; |
| } |
| } |
| |
| bool catchDegeneratePenetrationCase = |
| (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); |
| |
| //if (checkPenetration && !isValid) |
| if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) |
| { |
| //penetration case |
| |
| //if there is no way to handle penetrations, bail out |
| if (m_penetrationDepthSolver) |
| { |
| // Penetration depth case. |
| btVector3 tmpPointOnA,tmpPointOnB; |
| |
| gNumDeepPenetrationChecks++; |
| m_cachedSeparatingAxis.setZero(); |
| |
| bool isValid2 = m_penetrationDepthSolver->calcPenDepth( |
| *m_simplexSolver, |
| m_minkowskiA,m_minkowskiB, |
| localTransA,localTransB, |
| m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, |
| debugDraw,input.m_stackAlloc |
| ); |
| |
| |
| if (isValid2) |
| { |
| btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; |
| btScalar lenSqr = tmpNormalInB.length2(); |
| if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) |
| { |
| tmpNormalInB = m_cachedSeparatingAxis; |
| lenSqr = m_cachedSeparatingAxis.length2(); |
| } |
| |
| if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) |
| { |
| tmpNormalInB /= btSqrt(lenSqr); |
| btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); |
| //only replace valid penetrations when the result is deeper (check) |
| if (!isValid || (distance2 < distance)) |
| { |
| distance = distance2; |
| pointOnA = tmpPointOnA; |
| pointOnB = tmpPointOnB; |
| normalInB = tmpNormalInB; |
| isValid = true; |
| m_lastUsedMethod = 3; |
| } else |
| { |
| m_lastUsedMethod = 8; |
| } |
| } else |
| { |
| m_lastUsedMethod = 9; |
| } |
| } else |
| |
| { |
| ///this is another degenerate case, where the initial GJK calculation reports a degenerate case |
| ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) |
| ///reports a valid positive distance. Use the results of the second GJK instead of failing. |
| ///thanks to Jacob.Langford for the reproduction case |
| ///http://code.google.com/p/bullet/issues/detail?id=250 |
| |
| |
| if (m_cachedSeparatingAxis.length2() > btScalar(0.)) |
| { |
| btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; |
| //only replace valid distances when the distance is less |
| if (!isValid || (distance2 < distance)) |
| { |
| distance = distance2; |
| pointOnA = tmpPointOnA; |
| pointOnB = tmpPointOnB; |
| pointOnA -= m_cachedSeparatingAxis * marginA ; |
| pointOnB += m_cachedSeparatingAxis * marginB ; |
| normalInB = m_cachedSeparatingAxis; |
| normalInB.normalize(); |
| isValid = true; |
| m_lastUsedMethod = 6; |
| } else |
| { |
| m_lastUsedMethod = 5; |
| } |
| } |
| } |
| |
| } |
| |
| } |
| } |
| |
| |
| |
| if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) |
| { |
| #if 0 |
| ///some debugging |
| // if (check2d) |
| { |
| printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]); |
| printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex); |
| } |
| #endif |
| |
| m_cachedSeparatingAxis = normalInB; |
| m_cachedSeparatingDistance = distance; |
| |
| output.addContactPoint( |
| normalInB, |
| pointOnB+positionOffset, |
| distance); |
| |
| } |
| |
| |
| } |
| |
| |
| |
| |
| |