diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/NarrowPhaseCollision')
32 files changed, 5420 insertions, 4502 deletions
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h index 9eb880b8dfc..3c82133037e 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h @@ -16,163 +16,153 @@ subject to the following restrictions: #ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H #define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H -#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision... +#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision... #include "btGjkEpa3.h" #include "btGjkCollisionDescription.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +template <typename btConvexTemplate> +bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b, + const btGjkCollisionDescription& colDesc, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB) +{ + (void)v; + // const btScalar radialmargin(btScalar(0.)); + btVector3 guessVector(b.getWorldTransform().getOrigin() - a.getWorldTransform().getOrigin()); //?? why not use the GJK input? + btGjkEpaSolver3::sResults results; + if (btGjkEpaSolver3_Penetration(a, b, guessVector, results)) -template <typename btConvexTemplate> -bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b, - const btGjkCollisionDescription& colDesc, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB) -{ - (void)v; - - // const btScalar radialmargin(btScalar(0.)); - - btVector3 guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input? - - btGjkEpaSolver3::sResults results; - - - if(btGjkEpaSolver3_Penetration(a,b,guessVector,results)) - - { - // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return true; - } else - { - if(btGjkEpaSolver3_Distance(a,b,guessVector,results)) - { - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return false; - } - } - return false; + { + // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); + //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return true; + } + else + { + if (btGjkEpaSolver3_Distance(a, b, guessVector, results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; + } + } + return false; } template <typename btConvexTemplate, typename btGjkDistanceTemplate> -int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo) +int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo) { - - bool m_catchDegeneracies = true; - btScalar m_cachedSeparatingDistance = 0.f; - - btScalar distance=btScalar(0.); - btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); - - btVector3 pointOnA,pointOnB; - btTransform localTransA = a.getWorldTransform(); - btTransform localTransB = b.getWorldTransform(); - - btScalar marginA = a.getMargin(); - btScalar marginB = b.getMargin(); - - int m_curIter = 0; - int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN? - btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir; - - bool isValid = false; - bool checkSimplex = false; - bool checkPenetration = true; - int m_degenerateSimplex = 0; - - int m_lastUsedMethod = -1; - - { - btScalar squaredDistance = BT_LARGE_FLOAT; - btScalar delta = btScalar(0.); - - btScalar margin = marginA + marginB; - - - - simplexSolver.reset(); - - for ( ; ; ) - //while (true) - { - - btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis(); - btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis(); - - btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA); - btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - - - - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); - - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.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 (simplexSolver.inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * colDesc.m_gjkRelError2; - - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) - { - m_degenerateSimplex = 2; - } else - { - m_degenerateSimplex = 11; - } - checkSimplex = true; - break; - } - - //add current vertex to simplex - simplexSolver.addVertex(w, pWorld, qWorld); - btVector3 newCachedSeparatingAxis; - - //calculate the closest point to the origin (update vector v) - if (!simplexSolver.closest(newCachedSeparatingAxis)) - { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } - - if(newCachedSeparatingAxis.length2()<colDesc.m_gjkRelError2) - { - m_cachedSeparatingAxis = newCachedSeparatingAxis; - m_degenerateSimplex = 6; - checkSimplex = true; - break; - } - - btScalar previousSquaredDistance = squaredDistance; - squaredDistance = newCachedSeparatingAxis.length2(); + bool m_catchDegeneracies = true; + btScalar m_cachedSeparatingDistance = 0.f; + + btScalar distance = btScalar(0.); + btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.)); + + btVector3 pointOnA, pointOnB; + btTransform localTransA = a.getWorldTransform(); + btTransform localTransB = b.getWorldTransform(); + + btScalar marginA = a.getMargin(); + btScalar marginB = b.getMargin(); + + int m_curIter = 0; + int gGjkMaxIter = colDesc.m_maxGjkIterations; //this is to catch invalid input, perhaps check for #NaN? + btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir; + + bool isValid = false; + bool checkSimplex = false; + bool checkPenetration = true; + int m_degenerateSimplex = 0; + + int m_lastUsedMethod = -1; + + { + btScalar squaredDistance = BT_LARGE_FLOAT; + btScalar delta = btScalar(0.); + + btScalar margin = marginA + marginB; + + simplexSolver.reset(); + + for (;;) + //while (true) + { + btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis(); + btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis(); + + btVector3 pInA = a.getLocalSupportWithoutMargin(separatingAxisInA); + btVector3 qInB = b.getLocalSupportWithoutMargin(separatingAxisInB); + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + + btVector3 w = pWorld - qWorld; + delta = m_cachedSeparatingAxis.dot(w); + + // potential exit, they don't overlap + if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.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 (simplexSolver.inSimplex(w)) + { + m_degenerateSimplex = 1; + checkSimplex = true; + break; + } + // are we getting any closer ? + btScalar f0 = squaredDistance - delta; + btScalar f1 = squaredDistance * colDesc.m_gjkRelError2; + + if (f0 <= f1) + { + if (f0 <= btScalar(0.)) + { + m_degenerateSimplex = 2; + } + else + { + m_degenerateSimplex = 11; + } + checkSimplex = true; + break; + } + + //add current vertex to simplex + simplexSolver.addVertex(w, pWorld, qWorld); + btVector3 newCachedSeparatingAxis; + + //calculate the closest point to the origin (update vector v) + if (!simplexSolver.closest(newCachedSeparatingAxis)) + { + m_degenerateSimplex = 3; + checkSimplex = true; + break; + } + + if (newCachedSeparatingAxis.length2() < colDesc.m_gjkRelError2) + { + 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) @@ -182,188 +172,183 @@ int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate checkSimplex = false; break; } -#endif // - - - //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; - } - - m_cachedSeparatingAxis = newCachedSeparatingAxis; - - //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (m_curIter++ > gGjkMaxIter) - { -#if defined(DEBUG) || defined (_DEBUG) - - printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); - printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n", - m_cachedSeparatingAxis.getX(), - m_cachedSeparatingAxis.getY(), - m_cachedSeparatingAxis.getZ(), - squaredDistance); +#endif // + + //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; + } + + m_cachedSeparatingAxis = newCachedSeparatingAxis; + + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject + if (m_curIter++ > gGjkMaxIter) + { +#if defined(DEBUG) || defined(_DEBUG) + + printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter); + printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n", + m_cachedSeparatingAxis.getX(), + m_cachedSeparatingAxis.getY(), + m_cachedSeparatingAxis.getZ(), + squaredDistance); #endif - - break; - - } - - - bool check = (!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) - { - simplexSolver.compute_points(pointOnA, pointOnB); - normalInB = m_cachedSeparatingAxis; - - 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_degenerateSimplex && ((distance+margin) < 0.01)); - - //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) - { - //penetration case - - //if there is no way to handle penetrations, bail out - - // Penetration depth case. - btVector3 tmpPointOnA,tmpPointOnB; - - m_cachedSeparatingAxis.setZero(); - - bool isValid2 = btGjkEpaCalcPenDepth(a,b, - colDesc, - m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB); - - 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 < colDesc.m_maximumDistanceSquared))) - { - - m_cachedSeparatingAxis = normalInB; - m_cachedSeparatingDistance = distance; - distInfo->m_distance = distance; - distInfo->m_normalBtoA = normalInB; - distInfo->m_pointOnB = pointOnB; - distInfo->m_pointOnA = pointOnB+normalInB*distance; - return 0; - } - return -m_lastUsedMethod; -} + break; + } + + bool check = (!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) + { + simplexSolver.compute_points(pointOnA, pointOnB); + normalInB = m_cachedSeparatingAxis; + + 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_degenerateSimplex && ((distance + margin) < 0.01)); + + //if (checkPenetration && !isValid) + if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) + { + //penetration case + //if there is no way to handle penetrations, bail out + // Penetration depth case. + btVector3 tmpPointOnA, tmpPointOnB; + + m_cachedSeparatingAxis.setZero(); + + bool isValid2 = btGjkEpaCalcPenDepth(a, b, + colDesc, + m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB); + + 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 < colDesc.m_maximumDistanceSquared))) + { + m_cachedSeparatingAxis = normalInB; + m_cachedSeparatingDistance = distance; + distInfo->m_distance = distance; + distInfo->m_normalBtoA = normalInB; + distInfo->m_pointOnB = pointOnB; + distInfo->m_pointOnA = pointOnB + normalInB * distance; + return 0; + } + return -m_lastUsedMethod; +} -#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H +#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 940282f5762..38df8d4808e 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btContinuousConvexCollision.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" @@ -24,59 +23,60 @@ subject to the following restrictions: #include "btPointCollector.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" - - -btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_simplexSolver(simplexSolver), -m_penetrationDepthSolver(penetrationDepthSolver), -m_convexA(convexA),m_convexB1(convexB),m_planeShape(0) +btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver) + : m_simplexSolver(simplexSolver), + m_penetrationDepthSolver(penetrationDepthSolver), + m_convexA(convexA), + m_convexB1(convexB), + m_planeShape(0) { } - -btContinuousConvexCollision::btContinuousConvexCollision( const btConvexShape* convexA,const btStaticPlaneShape* plane) -:m_simplexSolver(0), -m_penetrationDepthSolver(0), -m_convexA(convexA),m_convexB1(0),m_planeShape(plane) +btContinuousConvexCollision::btContinuousConvexCollision(const btConvexShape* convexA, const btStaticPlaneShape* plane) + : m_simplexSolver(0), + m_penetrationDepthSolver(0), + m_convexA(convexA), + m_convexB1(0), + m_planeShape(plane) { } - /// This maximum should not be necessary. It allows for untested/degenerate cases in production code. /// You don't want your game ever to lock-up. #define MAX_ITERATIONS 64 -void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector) +void btContinuousConvexCollision::computeClosestPoints(const btTransform& transA, const btTransform& transB, btPointCollector& pointCollector) { if (m_convexB1) { m_simplexSolver->reset(); - btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver); + btGjkPairDetector gjk(m_convexA, m_convexB1, m_convexA->getShapeType(), m_convexB1->getShapeType(), m_convexA->getMargin(), m_convexB1->getMargin(), m_simplexSolver, m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; input.m_transformA = transA; input.m_transformB = transB; - gjk.getClosestPoints(input,pointCollector,0); - } else + gjk.getClosestPoints(input, pointCollector, 0); + } + else { //convex versus plane const btConvexShape* convexShape = m_convexA; const btStaticPlaneShape* planeShape = m_planeShape; - + const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); - + btTransform convexWorldTransform = transA; btTransform convexInPlaneTrans; - convexInPlaneTrans= transB.inverse() * convexWorldTransform; + convexInPlaneTrans = transB.inverse() * convexWorldTransform; btTransform planeInConvex; - planeInConvex= convexWorldTransform.inverse() * transB; - - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + planeInConvex = convexWorldTransform.inverse() * transB; + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected; btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal; @@ -87,41 +87,33 @@ void btContinuousConvexCollision::computeClosestPoints( const btTransform& trans } } -bool btContinuousConvexCollision::calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) +bool btContinuousConvexCollision::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) { - - /// compute linear and angular velocity for this interval, to interpolate - btVector3 linVelA,angVelA,linVelB,angVelB; - btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA); - btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB); - + btVector3 linVelA, angVelA, linVelB, angVelB; + btTransformUtil::calculateVelocity(fromA, toA, btScalar(1.), linVelA, angVelA); + btTransformUtil::calculateVelocity(fromB, toB, btScalar(1.), linVelB, angVelB); btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); - btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f; + btScalar boundingRadiusB = m_convexB1 ? m_convexB1->getAngularMotionDisc() : 0.f; btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; - btVector3 relLinVel = (linVelB-linVelA); - - btScalar relLinVelocLength = (linVelB-linVelA).length(); - - if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f) - return false; + btVector3 relLinVel = (linVelB - linVelA); + btScalar relLinVelocLength = (linVelB - linVelA).length(); + if ((relLinVelocLength + maxAngularProjectedVelocity) == 0.f) + return false; btScalar lambda = btScalar(0.); - btVector3 v(1,0,0); - - int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); bool hasResult = false; btVector3 c; @@ -131,15 +123,13 @@ bool btContinuousConvexCollision::calcTimeOfImpact( int numIter = 0; //first solution, using GJK - btScalar radius = 0.001f; -// result.drawCoordSystem(sphereTr); + // result.drawCoordSystem(sphereTr); - btPointCollector pointCollector1; + btPointCollector pointCollector1; { - - computeClosestPoints(fromA,fromB,pointCollector1); + computeClosestPoints(fromA, fromB, pointCollector1); hasResult = pointCollector1.m_hasResult; c = pointCollector1.m_pointInWorld; @@ -151,7 +141,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( dist = pointCollector1.m_distance + result.m_allowedPenetration; n = pointCollector1.m_normalOnBInWorld; btScalar projectedLinearVelocity = relLinVel.dot(n); - if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) + if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON) return false; //not close enough @@ -159,77 +149,69 @@ bool btContinuousConvexCollision::calcTimeOfImpact( { if (result.m_debugDrawer) { - result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1)); + result.m_debugDrawer->drawSphere(c, 0.2f, btVector3(1, 1, 1)); } btScalar dLambda = btScalar(0.); projectedLinearVelocity = relLinVel.dot(n); - //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) + if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON) return false; - - dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); - - - lambda = lambda + dLambda; + dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity); - if (lambda > btScalar(1.)) - return false; + lambda += dLambda; - if (lambda < btScalar(0.)) + if (lambda > btScalar(1.) || lambda < btScalar(0.)) return false; - //todo: next check with relative epsilon if (lambda <= lastLambda) { return false; //n.setValue(0,0,0); - break; + //break; } lastLambda = lambda; - - //interpolate to next lambda - btTransform interpolatedTransA,interpolatedTransB,relativeTrans; + btTransform interpolatedTransA, interpolatedTransB, relativeTrans; - btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA); - btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB); + btTransformUtil::integrateTransform(fromA, linVelA, angVelA, lambda, interpolatedTransA); + btTransformUtil::integrateTransform(fromB, linVelB, angVelB, lambda, interpolatedTransB); relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA); if (result.m_debugDrawer) { - result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0)); + result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(), 0.2f, btVector3(1, 0, 0)); } - result.DebugDraw( lambda ); + result.DebugDraw(lambda); - btPointCollector pointCollector; - computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector); + btPointCollector pointCollector; + computeClosestPoints(interpolatedTransA, interpolatedTransB, pointCollector); if (pointCollector.m_hasResult) { - dist = pointCollector.m_distance+result.m_allowedPenetration; - c = pointCollector.m_pointInWorld; + dist = pointCollector.m_distance + result.m_allowedPenetration; + c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; - } else + } + else { result.reportFailure(-1, numIter); return false; } numIter++; - if (numIter > maxIter) + if (numIter > MAX_ITERATIONS) { result.reportFailure(-2, numIter); return false; } } - + result.m_fraction = lambda; result.m_normal = n; result.m_hitPoint = c; @@ -237,6 +219,4 @@ bool btContinuousConvexCollision::calcTimeOfImpact( } return false; - } - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h index bdc0572f75a..67b2205c366 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_CONTINUOUS_COLLISION_CONVEX_CAST_H #define BT_CONTINUOUS_COLLISION_CONVEX_CAST_H @@ -25,35 +24,30 @@ class btStaticPlaneShape; /// btContinuousConvexCollision implements angular and linear time of impact for convex objects. /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). -/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent. +/// Algorithm operates in worldspace, in order to keep in between motion globally consistent. /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops class btContinuousConvexCollision : public btConvexCast { btSimplexSolverInterface* m_simplexSolver; - btConvexPenetrationDepthSolver* m_penetrationDepthSolver; - const btConvexShape* m_convexA; + btConvexPenetrationDepthSolver* m_penetrationDepthSolver; + const btConvexShape* m_convexA; //second object is either a convex or a plane (code sharing) - const btConvexShape* m_convexB1; - const btStaticPlaneShape* m_planeShape; + const btConvexShape* m_convexB1; + const btStaticPlaneShape* m_planeShape; - void computeClosestPoints( const btTransform& transA, const btTransform& transB,struct btPointCollector& pointCollector); + void computeClosestPoints(const btTransform& transA, const btTransform& transB, struct btPointCollector& pointCollector); public: + btContinuousConvexCollision(const btConvexShape* shapeA, const btConvexShape* shapeB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver); - btContinuousConvexCollision (const btConvexShape* shapeA,const btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); - - btContinuousConvexCollision(const btConvexShape* shapeA,const btStaticPlaneShape* plane ); - - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result); - + btContinuousConvexCollision(const btConvexShape* shapeA, const btStaticPlaneShape* plane); + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); }; - -#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H - +#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index bfd79d03beb..77b19be599f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_CONVEX_CAST_H #define BT_CONVEX_CAST_H @@ -23,51 +22,69 @@ subject to the following restrictions: class btMinkowskiSumShape; #include "LinearMath/btIDebugDraw.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define MAX_CONVEX_CAST_ITERATIONS 64 +#define MAX_CONVEX_CAST_EPSILON (SIMD_EPSILON * 10) +#else +#define MAX_CONVEX_CAST_ITERATIONS 32 +#define MAX_CONVEX_CAST_EPSILON btScalar(0.0001) +#endif +///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. +///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 +//will need to digg deeper to make the algorithm more robust +//since, a large epsilon can cause an early termination with false +//positive results (ray intersections that shouldn't be there) + /// btConvexCast is an interface for Casting class btConvexCast { public: - - virtual ~btConvexCast(); ///RayResult stores the closest result /// alternatively, add a callback method to decide about closest/all results - struct CastResult + struct CastResult { //virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0; - - virtual void DebugDraw(btScalar fraction) {(void)fraction;} - virtual void drawCoordSystem(const btTransform& trans) {(void)trans;} - virtual void reportFailure(int errNo, int numIterations) {(void)errNo;(void)numIterations;} + + virtual void DebugDraw(btScalar fraction) { (void)fraction; } + virtual void drawCoordSystem(const btTransform& trans) { (void)trans; } + virtual void reportFailure(int errNo, int numIterations) + { + (void)errNo; + (void)numIterations; + } CastResult() - :m_fraction(btScalar(BT_LARGE_FLOAT)), - m_debugDrawer(0), - m_allowedPenetration(btScalar(0)) + : m_fraction(btScalar(BT_LARGE_FLOAT)), + m_debugDrawer(0), + m_allowedPenetration(btScalar(0)), + m_subSimplexCastMaxIterations(MAX_CONVEX_CAST_ITERATIONS), + m_subSimplexCastEpsilon(MAX_CONVEX_CAST_EPSILON) { } + virtual ~CastResult(){}; - virtual ~CastResult() {}; - - btTransform m_hitTransformA; - btTransform m_hitTransformB; - btVector3 m_normal; - btVector3 m_hitPoint; - btScalar m_fraction; //input and output + btTransform m_hitTransformA; + btTransform m_hitTransformB; + btVector3 m_normal; + btVector3 m_hitPoint; + btScalar m_fraction; //input and output btIDebugDraw* m_debugDrawer; - btScalar m_allowedPenetration; + btScalar m_allowedPenetration; + + int m_subSimplexCastMaxIterations; + btScalar m_subSimplexCastEpsilon; }; - /// cast a convex against another convex object - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) = 0; + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) = 0; }; -#endif //BT_CONVEX_CAST_H +#endif //BT_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 29620abffb9..65c9df9340b 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_CONVEX_PENETRATION_DEPTH_H #define BT_CONVEX_PENETRATION_DEPTH_H @@ -25,16 +24,12 @@ class btTransform; ///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. class btConvexPenetrationDepthSolver { -public: - - virtual ~btConvexPenetrationDepthSolver() {}; - virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw) = 0; - - +public: + virtual ~btConvexPenetrationDepthSolver(){}; + virtual bool calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* convexA, const btConvexShape* convexB, + const btTransform& transA, const btTransform& transB, + btVector3& v, btVector3& pa, btVector3& pb, + class btIDebugDraw* debugDraw) = 0; }; -#endif //BT_CONVEX_PENETRATION_DEPTH_H - +#endif //BT_CONVEX_PENETRATION_DEPTH_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index 46ce1ab75e7..d1bbb1a46ee 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H #define BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H @@ -27,62 +26,60 @@ subject to the following restrictions: /// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB struct btDiscreteCollisionDetectorInterface { - struct Result { - - virtual ~Result(){} + virtual ~Result() {} ///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner - virtual void setShapeIdentifiersA(int partId0,int index0)=0; - virtual void setShapeIdentifiersB(int partId1,int index1)=0; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0; + virtual void setShapeIdentifiersA(int partId0, int index0) = 0; + virtual void setShapeIdentifiersB(int partId1, int index1) = 0; + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) = 0; }; struct ClosestPointInput { ClosestPointInput() - :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)) + : m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)) { } btTransform m_transformA; btTransform m_transformB; - btScalar m_maximumDistanceSquared; + btScalar m_maximumDistanceSquared; }; - virtual ~btDiscreteCollisionDetectorInterface() {}; + virtual ~btDiscreteCollisionDetectorInterface(){}; // // give either closest points (distance > 0) or penetration (distance) // the normal always points from B towards A // - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false) = 0; - + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false) = 0; }; struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result { - btVector3 m_normalOnSurfaceB; - btVector3 m_closestPointInB; - btScalar m_distance; //negative means penetration ! + btVector3 m_normalOnSurfaceB; + btVector3 m_closestPointInB; + btScalar m_distance; //negative means penetration ! - btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) - { +protected: + btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) + { + } - } - virtual ~btStorageResult() {}; +public: + virtual ~btStorageResult(){}; - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) + { + if (depth < m_distance) { - if (depth < m_distance) - { - m_normalOnSurfaceB = normalOnBInWorld; - m_closestPointInB = pointInWorld; - m_distance = depth; - } + m_normalOnSurfaceB = normalOnBInWorld; + m_closestPointInB = pointInWorld; + m_distance = depth; } + } }; -#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H - +#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h index 0b49b0ecc65..c9fd84bebfa 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GJK_COLLISION_DESCRIPTION_H #define GJK_COLLISION_DESCRIPTION_H @@ -21,21 +20,20 @@ subject to the following restrictions: struct btGjkCollisionDescription { - btVector3 m_firstDir; - int m_maxGjkIterations; - btScalar m_maximumDistanceSquared; - btScalar m_gjkRelError2; - btGjkCollisionDescription() - :m_firstDir(0,1,0), - m_maxGjkIterations(1000), - m_maximumDistanceSquared(1e30f), - m_gjkRelError2(1.0e-6) - { - } - virtual ~btGjkCollisionDescription() - { - } + btVector3 m_firstDir; + int m_maxGjkIterations; + btScalar m_maximumDistanceSquared; + btScalar m_gjkRelError2; + btGjkCollisionDescription() + : m_firstDir(0, 1, 0), + m_maxGjkIterations(1000), + m_maximumDistanceSquared(1e30f), + m_gjkRelError2(1.0e-6) + { + } + virtual ~btGjkCollisionDescription() + { + } }; -#endif //GJK_COLLISION_DESCRIPTION_H - +#endif //GJK_COLLISION_DESCRIPTION_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp index bef697a0a11..9d61e75dac8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btGjkConvexCast.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" #include "btGjkPairDetector.h" @@ -27,41 +25,39 @@ subject to the following restrictions: #define MAX_ITERATIONS 32 #endif -btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) -:m_simplexSolver(simplexSolver), -m_convexA(convexA), -m_convexB(convexB) +btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver) + : m_simplexSolver(simplexSolver), + m_convexA(convexA), + m_convexB(convexB) { } -bool btGjkConvexCast::calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) +bool btGjkConvexCast::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) { - - m_simplexSolver->reset(); /// compute linear velocity for this interval, to interpolate //assume no rotation/angular velocity, assert here? - btVector3 linVelA,linVelB; - linVelA = toA.getOrigin()-fromA.getOrigin(); - linVelB = toB.getOrigin()-fromB.getOrigin(); + btVector3 linVelA, linVelB; + linVelA = toA.getOrigin() - fromA.getOrigin(); + linVelB = toB.getOrigin() - fromB.getOrigin(); btScalar radius = btScalar(0.001); btScalar lambda = btScalar(0.); - btVector3 v(1,0,0); + btVector3 v(1, 0, 0); int maxIter = MAX_ITERATIONS; btVector3 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); bool hasResult = false; btVector3 c; - btVector3 r = (linVelA-linVelB); + btVector3 r = (linVelA - linVelB); btScalar lastLambda = lambda; //btScalar epsilon = btScalar(0.001); @@ -69,17 +65,14 @@ bool btGjkConvexCast::calcTimeOfImpact( int numIter = 0; //first solution, using GJK - btTransform identityTrans; identityTrans.setIdentity(); + // result.drawCoordSystem(sphereTr); -// result.drawCoordSystem(sphereTr); - - btPointCollector pointCollector; + btPointCollector pointCollector; - - btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver); + btGjkPairDetector gjk(m_convexA, m_convexB, m_simplexSolver, 0); //m_penetrationDepthSolver); btGjkPairDetector::ClosestPointInput input; //we don't use margins during CCD @@ -87,7 +80,7 @@ bool btGjkConvexCast::calcTimeOfImpact( input.m_transformA = fromA; input.m_transformB = fromB; - gjk.getClosestPoints(input,pointCollector,0); + gjk.getClosestPoints(input, pointCollector, 0); hasResult = pointCollector.m_hasResult; c = pointCollector.m_pointInWorld; @@ -98,20 +91,18 @@ bool btGjkConvexCast::calcTimeOfImpact( dist = pointCollector.m_distance; n = pointCollector.m_normalOnBInWorld; - - //not close enough while (dist > radius) { numIter++; if (numIter > maxIter) { - return false; //todo: report a failure + return false; //todo: report a failure } btScalar dLambda = btScalar(0.); btScalar projectedLinearVelocity = r.dot(n); - + dLambda = dist / (projectedLinearVelocity); lambda = lambda - dLambda; @@ -132,35 +123,35 @@ bool btGjkConvexCast::calcTimeOfImpact( lastLambda = lambda; //interpolate to next lambda - result.DebugDraw( lambda ); - input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); - input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); - - gjk.getClosestPoints(input,pointCollector,0); + result.DebugDraw(lambda); + input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(), toA.getOrigin(), lambda); + input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda); + + gjk.getClosestPoints(input, pointCollector, 0); if (pointCollector.m_hasResult) { if (pointCollector.m_distance < btScalar(0.)) { result.m_fraction = lastLambda; n = pointCollector.m_normalOnBInWorld; - result.m_normal=n; + result.m_normal = n; result.m_hitPoint = pointCollector.m_pointInWorld; return true; } - c = pointCollector.m_pointInWorld; + c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; dist = pointCollector.m_distance; - } else + } + else { //?? return false; } - } //is n normalized? //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (n.dot(r)>=-result.m_allowedPenetration) + if (n.dot(r) >= -result.m_allowedPenetration) return false; result.m_fraction = lambda; @@ -170,7 +161,4 @@ bool btGjkConvexCast::calcTimeOfImpact( } return false; - - } - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h index 6a42ee63b03..ef5979173e9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_GJK_CONVEX_CAST_H #define BT_GJK_CONVEX_CAST_H @@ -29,22 +27,20 @@ class btMinkowskiSumShape; ///GjkConvexCast performs a raycast on a convex object using support mapping. class btGjkConvexCast : public btConvexCast { - btSimplexSolverInterface* m_simplexSolver; - const btConvexShape* m_convexA; - const btConvexShape* m_convexB; + btSimplexSolverInterface* m_simplexSolver; + const btConvexShape* m_convexA; + const btConvexShape* m_convexB; public: - - btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver); + btGjkConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver); /// cast a convex against another convex object - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result); - + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); }; -#endif //BT_GJK_CONVEX_CAST_H +#endif //BT_GJK_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index 3268f06c2f9..7d53f8624a8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -26,996 +26,1069 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #include "BulletCollision/CollisionShapes/btSphereShape.h" #include "btGjkEpa2.h" -#if defined(DEBUG) || defined (_DEBUG) -#include <stdio.h> //for debug printf +#if defined(DEBUG) || defined(_DEBUG) +#include <stdio.h> //for debug printf #ifdef __SPU__ #include <spu_printf.h> #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #endif namespace gjkepa2_impl { +// Config - // Config +/* GJK */ +#define GJK_MAX_ITERATIONS 128 - /* GJK */ -#define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((btScalar)0.0001) -#define GJK_MIN_DISTANCE ((btScalar)0.0001) -#define GJK_DUPLICATED_EPS ((btScalar)0.0001) -#define GJK_SIMPLEX2_EPS ((btScalar)0.0) -#define GJK_SIMPLEX3_EPS ((btScalar)0.0) -#define GJK_SIMPLEX4_EPS ((btScalar)0.0) - - /* EPA */ -#define EPA_MAX_VERTICES 64 -#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) -#define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((btScalar)0.0001) -#define EPA_FALLBACK (10*EPA_ACCURACY) -#define EPA_PLANE_EPS ((btScalar)0.00001) -#define EPA_INSIDE_EPS ((btScalar)0.01) +#ifdef BT_USE_DOUBLE_PRECISION +#define GJK_ACCURACY ((btScalar)1e-12) +#define GJK_MIN_DISTANCE ((btScalar)1e-12) +#define GJK_DUPLICATED_EPS ((btScalar)1e-12) +#else +#define GJK_ACCURACY ((btScalar)0.0001) +#define GJK_MIN_DISTANCE ((btScalar)0.0001) +#define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#endif //BT_USE_DOUBLE_PRECISION +#define GJK_SIMPLEX2_EPS ((btScalar)0.0) +#define GJK_SIMPLEX3_EPS ((btScalar)0.0) +#define GJK_SIMPLEX4_EPS ((btScalar)0.0) - // Shorthands - typedef unsigned int U; - typedef unsigned char U1; +/* EPA */ +#define EPA_MAX_VERTICES 128 +#define EPA_MAX_ITERATIONS 255 - // MinkowskiDiff - struct MinkowskiDiff - { - const btConvexShape* m_shapes[2]; - btMatrix3x3 m_toshape1; - btTransform m_toshape0; -#ifdef __SPU__ - bool m_enableMargin; +#ifdef BT_USE_DOUBLE_PRECISION +#define EPA_ACCURACY ((btScalar)1e-12) +#define EPA_PLANE_EPS ((btScalar)1e-14) +#define EPA_INSIDE_EPS ((btScalar)1e-9) #else - btVector3 (btConvexShape::*Ls)(const btVector3&) const; -#endif//__SPU__ - +#define EPA_ACCURACY ((btScalar)0.0001) +#define EPA_PLANE_EPS ((btScalar)0.00001) +#define EPA_INSIDE_EPS ((btScalar)0.01) +#endif - MinkowskiDiff() - { +#define EPA_FALLBACK (10 * EPA_ACCURACY) +#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2) - } +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; + +// MinkowskiDiff +struct MinkowskiDiff +{ + const btConvexShape* m_shapes[2]; + btMatrix3x3 m_toshape1; + btTransform m_toshape0; #ifdef __SPU__ - void EnableMargin(bool enable) - { - m_enableMargin = enable; - } - inline btVector3 Support0(const btVector3& d) const - { - if (m_enableMargin) - { - return m_shapes[0]->localGetSupportVertexNonVirtual(d); - } else - { - return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); - } - } - inline btVector3 Support1(const btVector3& d) const - { - if (m_enableMargin) - { - return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d)); - } else - { - return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d)); - } - } + bool m_enableMargin; #else - void EnableMargin(bool enable) - { - if(enable) - Ls=&btConvexShape::localGetSupportVertexNonVirtual; - else - Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; - } - inline btVector3 Support0(const btVector3& d) const + btVector3 (btConvexShape::*Ls)(const btVector3&) const; +#endif //__SPU__ + + MinkowskiDiff() + { + } +#ifdef __SPU__ + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + if (m_enableMargin) { - return(((m_shapes[0])->*(Ls))(d)); + return m_shapes[0]->localGetSupportVertexNonVirtual(d); } - inline btVector3 Support1(const btVector3& d) const + else { - return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); + return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); } -#endif //__SPU__ - - inline btVector3 Support(const btVector3& d) const + } + inline btVector3 Support1(const btVector3& d) const + { + if (m_enableMargin) { - return(Support0(d)-Support1(-d)); + return m_toshape0 * (m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1 * d)); } - btVector3 Support(const btVector3& d,U index) const + else { - if(index) - return(Support1(d)); - else - return(Support0(d)); + return m_toshape0 * (m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1 * d)); } - }; + } +#else + void EnableMargin(bool enable) + { + if (enable) + Ls = &btConvexShape::localGetSupportVertexNonVirtual; + else + Ls = &btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; + } + inline btVector3 Support0(const btVector3& d) const + { + return (((m_shapes[0])->*(Ls))(d)); + } + inline btVector3 Support1(const btVector3& d) const + { + return (m_toshape0 * ((m_shapes[1])->*(Ls))(m_toshape1 * d)); + } +#endif //__SPU__ - typedef MinkowskiDiff tShape; + inline btVector3 Support(const btVector3& d) const + { + return (Support0(d) - Support1(-d)); + } + btVector3 Support(const btVector3& d, U index) const + { + if (index) + return (Support1(d)); + else + return (Support0(d)); + } +}; +typedef MinkowskiDiff tShape; - // GJK - struct GJK +// GJK +struct GJK +{ + /* Types */ + struct sSV { - /* Types */ - struct sSV - { - btVector3 d,w; - }; - struct sSimplex + btVector3 d, w; + }; + struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; + struct eStatus + { + enum _ { - sSV* c[4]; - btScalar p[4]; - U rank; - }; - struct eStatus { enum _ { Valid, Inside, - Failed };}; - /* Fields */ - tShape m_shape; - btVector3 m_ray; - btScalar m_distance; - sSimplex m_simplices[2]; - sSV m_store[4]; - sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; - eStatus::_ m_status; - /* Methods */ - GJK() - { - Initialize(); + Failed + }; + }; + /* Fields */ + tShape m_shape; + btVector3 m_ray; + btScalar m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + U m_nfree; + U m_current; + sSimplex* m_simplex; + eStatus::_ m_status; + /* Methods */ + GJK() + { + Initialize(); + } + void Initialize() + { + m_ray = btVector3(0, 0, 0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } + eStatus::_ Evaluate(const tShape& shapearg, const btVector3& guess) + { + U iterations = 0; + btScalar sqdist = 0; + btScalar alpha = 0; + btVector3 lastw[4]; + U clastw = 0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const btScalar sqrl = m_ray.length2(); + appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : btVector3(1, 0, 0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do + { + const U next = 1 - m_current; + sSimplex& cs = m_simplices[m_current]; + sSimplex& ns = m_simplices[next]; + /* Check zero */ + const btScalar rl = m_ray.length(); + if (rl < GJK_MIN_DISTANCE) + { /* Touching or inside */ + m_status = eStatus::Inside; + break; } - void Initialize() + /* Append new vertice in -'v' direction */ + appendvertice(cs, -m_ray); + const btVector3& w = cs.c[cs.rank - 1]->w; + bool found = false; + for (U i = 0; i < 4; ++i) { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; + if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS) + { + found = true; + break; + } } - eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) + if (found) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + { /* Update lastw */ + lastw[clastw = (clastw + 1) & 3] = w; + } + /* Check for termination */ + const btScalar omega = btDot(m_ray, w) / rl; + alpha = btMax(omega, alpha); + if (((rl - alpha) - (GJK_ACCURACY * rl)) <= 0) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + btScalar weights[4]; + U mask = 0; + switch (cs.rank) { - U iterations=0; - btScalar sqdist=0; - btScalar alpha=0; - btVector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eStatus::Valid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar rl=m_ray.length(); - if(rl<GJK_MIN_DISTANCE) - {/* Touching or inside */ - m_status=eStatus::Inside; - break; - } - /* Append new vertice in -'v' direction */ - appendvertice(cs,-m_ray); - const btVector3& w=cs.c[cs.rank-1]->w; - bool found=false; - for(U i=0;i<4;++i) + case 2: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + weights, mask); + break; + case 3: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights, mask); + break; + case 4: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights, mask); + break; + } + if (sqdist >= 0) + { /* Valid */ + ns.rank = 0; + m_ray = btVector3(0, 0, 0); + m_current = next; + for (U i = 0, ni = cs.rank; i < ni; ++i) + { + if (mask & (1 << i)) { - if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) - { found=true;break; } - } - if(found) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w * weights[i]; } else - {/* Update lastw */ - lastw[clastw=(clastw+1)&3]=w; - } - /* Check for termination */ - const btScalar omega=btDot(m_ray,w)/rl; - alpha=btMax(omega,alpha); - if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - /* Reduce simplex */ - btScalar weights[4]; - U mask=0; - switch(cs.rank) { - case 2: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - weights,mask);break; - case 3: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - weights,mask);break; - case 4: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - cs.c[3]->w, - weights,mask);break; - } - if(sqdist>=0) - {/* Valid */ - ns.rank = 0; - m_ray = btVector3(0,0,0); - m_current = next; - for(U i=0,ni=cs.rank;i<ni;++i) - { - if(mask&(1<<i)) - { - ns.c[ns.rank] = cs.c[i]; - ns.p[ns.rank++] = weights[i]; - m_ray += cs.c[i]->w*weights[i]; - } - else - { - m_free[m_nfree++] = cs.c[i]; - } - } - if(mask==15) m_status=eStatus::Inside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; + m_free[m_nfree++] = cs.c[i]; } - m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; - } while(m_status==eStatus::Valid); - m_simplex=&m_simplices[m_current]; - switch(m_status) + } + if (mask == 15) m_status = eStatus::Inside; + } + else + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus::Failed; + } while (m_status == eStatus::Valid); + m_simplex = &m_simplices[m_current]; + switch (m_status) + { + case eStatus::Valid: + m_distance = m_ray.length(); + break; + case eStatus::Inside: + m_distance = 0; + break; + default: + { + } + } + return (m_status); + } + bool EncloseOrigin() + { + switch (m_simplex->rank) + { + case 1: + { + for (U i = 0; i < 3; ++i) { - case eStatus::Valid: m_distance=m_ray.length();break; - case eStatus::Inside: m_distance=0;break; - default: - { - } - } - return(m_status); + btVector3 axis = btVector3(0, 0, 0); + axis[i] = 1; + appendvertice(*m_simplex, axis); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -axis); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } } - bool EncloseOrigin() + break; + case 2: { - switch(m_simplex->rank) + const btVector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w; + for (U i = 0; i < 3; ++i) { - case 1: + btVector3 axis = btVector3(0, 0, 0); + axis[i] = 1; + const btVector3 p = btCross(d, axis); + if (p.length2() > 0) { - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - appendvertice(*m_simplex, axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 2: - { - const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - const btVector3 p=btCross(d,axis); - if(p.length2()>0) - { - appendvertice(*m_simplex, p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } + appendvertice(*m_simplex, p); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -p); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); } - break; - case 3: - { - const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w, - m_simplex->c[2]->w-m_simplex->c[0]->w); - if(n.length2()>0) - { - appendvertice(*m_simplex,n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 4: - { - if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, - m_simplex->c[1]->w-m_simplex->c[3]->w, - m_simplex->c[2]->w-m_simplex->c[3]->w))>0) - return(true); - } - break; } - return(false); } - /* Internals */ - void getsupport(const btVector3& d,sSV& sv) const + break; + case 3: { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); + const btVector3 n = btCross(m_simplex->c[1]->w - m_simplex->c[0]->w, + m_simplex->c[2]->w - m_simplex->c[0]->w); + if (n.length2() > 0) + { + appendvertice(*m_simplex, n); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -n); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } } - void removevertice(sSimplex& simplex) + break; + case 4: { - m_free[m_nfree++]=simplex.c[--simplex.rank]; + if (btFabs(det(m_simplex->c[0]->w - m_simplex->c[3]->w, + m_simplex->c[1]->w - m_simplex->c[3]->w, + m_simplex->c[2]->w - m_simplex->c[3]->w)) > 0) + return (true); } - void appendvertice(sSimplex& simplex,const btVector3& v) + break; + } + return (false); + } + /* Internals */ + void getsupport(const btVector3& d, sSV& sv) const + { + sv.d = d / d.length(); + sv.w = m_shape.Support(sv.d); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++] = simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex, const btVector3& v) + { + simplex.p[simplex.rank] = 0; + simplex.c[simplex.rank] = m_free[--m_nfree]; + getsupport(v, *simplex.c[simplex.rank++]); + } + static btScalar det(const btVector3& a, const btVector3& b, const btVector3& c) + { + return (a.y() * b.z() * c.x() + a.z() * b.x() * c.y() - + a.x() * b.z() * c.y() - a.y() * b.x() * c.z() + + a.x() * b.y() * c.z() - a.z() * b.y() * c.x()); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + btScalar* w, U& m) + { + const btVector3 d = b - a; + const btScalar l = d.length2(); + if (l > GJK_SIMPLEX2_EPS) + { + const btScalar t(l > 0 ? -btDot(a, d) / l : 0); + if (t >= 1) { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); + w[0] = 0; + w[1] = 1; + m = 2; + return (b.length2()); } - static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) + else if (t <= 0) { - return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- - a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ - a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); + w[0] = 1; + w[1] = 0; + m = 1; + return (a.length2()); } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - btScalar* w,U& m) + else { - const btVector3 d=b-a; - const btScalar l=d.length2(); - if(l>GJK_SIMPLEX2_EPS) - { - const btScalar t(l>0?-btDot(a,d)/l:0); - if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } - else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } - else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } - } - return(-1); + w[0] = 1 - (w[1] = t); + m = 3; + return ((a + d * t).length2()); } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar* w,U& m) + } + return (-1); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w, U& m) + { + static const U imd3[] = {1, 2, 0}; + const btVector3* vt[] = {&a, &b, &c}; + const btVector3 dl[] = {a - b, b - c, c - a}; + const btVector3 n = btCross(dl[0], dl[1]); + const btScalar l = n.length2(); + if (l > GJK_SIMPLEX3_EPS) + { + btScalar mindist = -1; + btScalar subw[2] = {0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c}; - const btVector3 dl[]={a-b,b-c,c-a}; - const btVector3 n=btCross(dl[0],dl[1]); - const btScalar l=n.length2(); - if(l>GJK_SIMPLEX3_EPS) + if (btDot(*vt[i], btCross(dl[i], n)) > 0) { - btScalar mindist=-1; - btScalar subw[2]={0.f,0.f}; - U subm(0); - for(U i=0;i<3;++i) + const U j = imd3[i]; + const btScalar subd(projectorigin(*vt[i], *vt[j], subw, subm)); + if ((mindist < 0) || (subd < mindist)) { - if(btDot(*vt[i],btCross(dl[i],n))>0) - { - const U j=imd3[i]; - const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); - if((mindist<0)||(subd<mindist)) - { - mindist = subd; - m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); - w[i] = subw[0]; - w[j] = subw[1]; - w[imd3[j]] = 0; - } - } + mindist = subd; + m = static_cast<U>(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; } - if(mindist<0) - { - const btScalar d=btDot(a,n); - const btScalar s=btSqrt(l); - const btVector3 p=n*(d/l); - mindist = p.length2(); - m = 7; - w[0] = (btCross(dl[1],b-p)).length()/s; - w[1] = (btCross(dl[2],c-p)).length()/s; - w[2] = 1-(w[0]+w[1]); - } - return(mindist); } - return(-1); } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& d, - btScalar* w,U& m) + if (mindist < 0) { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c,&d}; - const btVector3 dl[]={a-d,b-d,c-d}; - const btScalar vl=det(dl[0],dl[1],dl[2]); - const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0; - if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) + const btScalar d = btDot(a, n); + const btScalar s = btSqrt(l); + const btVector3 p = n * (d / l); + mindist = p.length2(); + m = 7; + w[0] = (btCross(dl[1], b - p)).length() / s; + w[1] = (btCross(dl[2], c - p)).length() / s; + w[2] = 1 - (w[0] + w[1]); + } + return (mindist); + } + return (-1); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& d, + btScalar* w, U& m) + { + static const U imd3[] = {1, 2, 0}; + const btVector3* vt[] = {&a, &b, &c, &d}; + const btVector3 dl[] = {a - d, b - d, c - d}; + const btScalar vl = det(dl[0], dl[1], dl[2]); + const bool ng = (vl * btDot(a, btCross(b - c, a - b))) <= 0; + if (ng && (btFabs(vl) > GJK_SIMPLEX4_EPS)) + { + btScalar mindist = -1; + btScalar subw[3] = {0.f, 0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + const U j = imd3[i]; + const btScalar s = vl * btDot(d, btCross(dl[i], dl[j])); + if (s > 0) { - btScalar mindist=-1; - btScalar subw[3]={0.f,0.f,0.f}; - U subm(0); - for(U i=0;i<3;++i) - { - const U j=imd3[i]; - const btScalar s=vl*btDot(d,btCross(dl[i],dl[j])); - if(s>0) - { - const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); - if((mindist<0)||(subd<mindist)) - { - mindist = subd; - m = static_cast<U>((subm&1?1<<i:0)+ - (subm&2?1<<j:0)+ - (subm&4?8:0)); - w[i] = subw[0]; - w[j] = subw[1]; - w[imd3[j]] = 0; - w[3] = subw[2]; - } - } - } - if(mindist<0) + const btScalar subd = projectorigin(*vt[i], *vt[j], d, subw, subm); + if ((mindist < 0) || (subd < mindist)) { - mindist = 0; - m = 15; - w[0] = det(c,b,d)/vl; - w[1] = det(a,c,d)/vl; - w[2] = det(b,a,d)/vl; - w[3] = 1-(w[0]+w[1]+w[2]); + mindist = subd; + m = static_cast<U>((subm & 1 ? 1 << i : 0) + + (subm & 2 ? 1 << j : 0) + + (subm & 4 ? 8 : 0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + w[3] = subw[2]; } - return(mindist); } - return(-1); } - }; + if (mindist < 0) + { + mindist = 0; + m = 15; + w[0] = det(c, b, d) / vl; + w[1] = det(a, c, d) / vl; + w[2] = det(b, a, d) / vl; + w[3] = 1 - (w[0] + w[1] + w[2]); + } + return (mindist); + } + return (-1); + } +}; - // EPA - struct EPA +// EPA +struct EPA +{ + /* Types */ + typedef GJK::sSV sSV; + struct sFace { - /* Types */ - typedef GJK::sSV sSV; - struct sFace - { - btVector3 n; - btScalar d; - sSV* c[3]; - sFace* f[3]; - sFace* l[2]; - U1 e[3]; - U1 pass; - }; - struct sList - { - sFace* root; - U count; - sList() : root(0),count(0) {} - }; - struct sHorizon + btVector3 n; + btScalar d; + sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + U1 e[3]; + U1 pass; + }; + struct sList + { + sFace* root; + U count; + sList() : root(0), count(0) {} + }; + struct sHorizon + { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(0), ff(0), nf(0) {} + }; + struct eStatus + { + enum _ { - sFace* cf; - sFace* ff; - U nf; - sHorizon() : cf(0),ff(0),nf(0) {} - }; - struct eStatus { enum _ { Valid, Touching, Degenerated, NonConvex, - InvalidHull, + InvalidHull, OutOfFaces, OutOfVertices, AccuraryReached, FallBack, - Failed };}; - /* Fields */ - eStatus::_ m_status; - GJK::sSimplex m_result; - btVector3 m_normal; - btScalar m_depth; - sSV m_sv_store[EPA_MAX_VERTICES]; - sFace m_fc_store[EPA_MAX_FACES]; - U m_nextsv; - sList m_hull; - sList m_stock; - /* Methods */ - EPA() - { - Initialize(); - } + Failed + }; + }; + /* Fields */ + eStatus::_ m_status; + GJK::sSimplex m_result; + btVector3 m_normal; + btScalar m_depth; + sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + U m_nextsv; + sList m_hull; + sList m_stock; + /* Methods */ + EPA() + { + Initialize(); + } + static inline void bind(sFace* fa, U ea, sFace* fb, U eb) + { + fa->e[ea] = (U1)eb; + fa->f[ea] = fb; + fb->e[eb] = (U1)ea; + fb->f[eb] = fa; + } + static inline void append(sList& list, sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if (list.root) list.root->l[0] = face; + list.root = face; + ++list.count; + } + static inline void remove(sList& list, sFace* face) + { + if (face->l[1]) face->l[1]->l[0] = face->l[0]; + if (face->l[0]) face->l[0]->l[1] = face->l[1]; + if (face == list.root) list.root = face->l[1]; + --list.count; + } - static inline void bind(sFace* fa,U ea,sFace* fb,U eb) - { - fa->e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } - static inline void append(sList& list,sFace* face) + void Initialize() + { + m_status = eStatus::Failed; + m_normal = btVector3(0, 0, 0); + m_depth = 0; + m_nextsv = 0; + for (U i = 0; i < EPA_MAX_FACES; ++i) + { + append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]); + } + } + eStatus::_ Evaluate(GJK& gjk, const btVector3& guess) + { + GJK::sSimplex& simplex = *gjk.m_simplex; + if ((simplex.rank > 1) && gjk.EncloseOrigin()) + { + /* Clean up */ + while (m_hull.root) { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; + sFace* f = m_hull.root; + remove(m_hull, f); + append(m_stock, f); } - static inline void remove(sList& list,sFace* face) + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if (gjk.det(simplex.c[0]->w - simplex.c[3]->w, + simplex.c[1]->w - simplex.c[3]->w, + simplex.c[2]->w - simplex.c[3]->w) < 0) { - if(face->l[1]) face->l[1]->l[0]=face->l[0]; - if(face->l[0]) face->l[0]->l[1]=face->l[1]; - if(face==list.root) list.root=face->l[1]; - --list.count; + btSwap(simplex.c[0], simplex.c[1]); + btSwap(simplex.p[0], simplex.p[1]); } - - - void Initialize() + /* Build initial hull */ + sFace* tetra[] = {newface(simplex.c[0], simplex.c[1], simplex.c[2], true), + newface(simplex.c[1], simplex.c[0], simplex.c[3], true), + newface(simplex.c[2], simplex.c[1], simplex.c[3], true), + newface(simplex.c[0], simplex.c[2], simplex.c[3], true)}; + if (m_hull.count == 4) { - m_status = eStatus::Failed; - m_normal = btVector3(0,0,0); - m_depth = 0; - m_nextsv = 0; - for(U i=0;i<EPA_MAX_FACES;++i) + sFace* best = findbest(); + sFace outer = *best; + U pass = 0; + U iterations = 0; + bind(tetra[0], 0, tetra[1], 0); + bind(tetra[0], 1, tetra[2], 0); + bind(tetra[0], 2, tetra[3], 0); + bind(tetra[1], 1, tetra[3], 2); + bind(tetra[1], 2, tetra[2], 1); + bind(tetra[2], 2, tetra[3], 1); + m_status = eStatus::Valid; + for (; iterations < EPA_MAX_ITERATIONS; ++iterations) { - append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); - } - } - eStatus::_ Evaluate(GJK& gjk,const btVector3& guess) - { - GJK::sSimplex& simplex=*gjk.m_simplex; - if((simplex.rank>1)&&gjk.EncloseOrigin()) - { - - /* Clean up */ - while(m_hull.root) - { - sFace* f = m_hull.root; - remove(m_hull,f); - append(m_stock,f); - } - m_status = eStatus::Valid; - m_nextsv = 0; - /* Orient simplex */ - if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, - simplex.c[1]->w-simplex.c[3]->w, - simplex.c[2]->w-simplex.c[3]->w)<0) - { - btSwap(simplex.c[0],simplex.c[1]); - btSwap(simplex.p[0],simplex.p[1]); - } - /* Build initial hull */ - sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), - newface(simplex.c[1],simplex.c[0],simplex.c[3],true), - newface(simplex.c[2],simplex.c[1],simplex.c[3],true), - newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; - if(m_hull.count==4) + if (m_nextsv < EPA_MAX_VERTICES) { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U iterations=0; - bind(tetra[0],0,tetra[1],0); - bind(tetra[0],1,tetra[2],0); - bind(tetra[0],2,tetra[3],0); - bind(tetra[1],1,tetra[3],2); - bind(tetra[1],2,tetra[2],1); - bind(tetra[2],2,tetra[3],1); - m_status=eStatus::Valid; - for(;iterations<EPA_MAX_ITERATIONS;++iterations) + sHorizon horizon; + sSV* w = &m_sv_store[m_nextsv++]; + bool valid = true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n, *w); + const btScalar wdist = btDot(best->n, w->w) - best->d; + if (wdist > EPA_ACCURACY) { - if(m_nextsv<EPA_MAX_VERTICES) - { - sHorizon horizon; - sSV* w=&m_sv_store[m_nextsv++]; - bool valid=true; - best->pass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const btScalar wdist=btDot(best->n,w->w)-best->d; - if(wdist>EPA_ACCURACY) - { - for(U j=0;(j<3)&&valid;++j) - { - valid&=expand( pass,w, - best->f[j],best->e[j], - horizon); - } - if(valid&&(horizon.nf>=3)) - { - bind(horizon.cf,1,horizon.ff,2); - remove(m_hull,best); - append(m_stock,best); - best=findbest(); - outer=*best; - } else { m_status=eStatus::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } + for (U j = 0; (j < 3) && valid; ++j) + { + valid &= expand(pass, w, + best->f[j], best->e[j], + horizon); + } + if (valid && (horizon.nf >= 3)) + { + bind(horizon.cf, 1, horizon.ff, 2); + remove(m_hull, best); + append(m_stock, best); + best = findbest(); + outer = *best; + } + else + { + m_status = eStatus::InvalidHull; + break; + } + } + else + { + m_status = eStatus::AccuraryReached; + break; } - const btVector3 projection=outer.n*outer.d; - m_normal = outer.n; - m_depth = outer.d; - m_result.rank = 3; - m_result.c[0] = outer.c[0]; - m_result.c[1] = outer.c[1]; - m_result.c[2] = outer.c[2]; - m_result.p[0] = btCross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = btCross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = btCross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; - m_result.p[0] /= sum; - m_result.p[1] /= sum; - m_result.p[2] /= sum; - return(m_status); - } - } - /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const btScalar nl=m_normal.length(); - if(nl>0) - m_normal = m_normal/nl; - else - m_normal = btVector3(1,0,0); - m_depth = 0; - m_result.rank=1; - m_result.c[0]=simplex.c[0]; - m_result.p[0]=1; - return(m_status); - } - bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist) - { - const btVector3 ba = b->w - a->w; - const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane - const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required - - if(a_dot_nab < 0) - { - // Outside of edge a->b - - const btScalar ba_l2 = ba.length2(); - const btScalar a_dot_ba = btDot(a->w, ba); - const btScalar b_dot_ba = btDot(b->w, ba); - - if(a_dot_ba > 0) - { - // Pick distance vertex a - dist = a->w.length(); - } - else if(b_dot_ba < 0) - { - // Pick distance vertex b - dist = b->w.length(); } else { - // Pick distance to edge a->b - const btScalar a_dot_b = btDot(a->w, b->w); - dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + m_status = eStatus::OutOfVertices; + break; } - - return true; } + const btVector3 projection = outer.n * outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = btCross(outer.c[1]->w - projection, + outer.c[2]->w - projection) + .length(); + m_result.p[1] = btCross(outer.c[2]->w - projection, + outer.c[0]->w - projection) + .length(); + m_result.p[2] = btCross(outer.c[0]->w - projection, + outer.c[1]->w - projection) + .length(); + const btScalar sum = m_result.p[0] + m_result.p[1] + m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return (m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const btScalar nl = m_normal.length(); + if (nl > 0) + m_normal = m_normal / nl; + else + m_normal = btVector3(1, 0, 0); + m_depth = 0; + m_result.rank = 1; + m_result.c[0] = simplex.c[0]; + m_result.p[0] = 1; + return (m_status); + } + bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist) + { + const btVector3 ba = b->w - a->w; + const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required - return false; + if (a_dot_nab < 0) + { + // Outside of edge a->b + + const btScalar ba_l2 = ba.length2(); + const btScalar a_dot_ba = btDot(a->w, ba); + const btScalar b_dot_ba = btDot(b->w, ba); + + if (a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); } - sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + else if (b_dot_ba < 0) { - if(m_stock.root) - { - sFace* face=m_stock.root; - remove(m_stock,face); - append(m_hull,face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = btCross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const btScalar a_dot_b = btDot(a->w, b->w); + dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + } - if(v) - { - if(!(getedgedist(face, a, b, face->d) || - getedgedist(face, b, c, face->d) || - getedgedist(face, c, a, face->d))) - { - // Origin projects to the interior of the triangle - // Use distance to triangle plane - face->d = btDot(a->w, face->n) / l; - } + return true; + } - face->n /= l; - if(forced || (face->d >= -EPA_PLANE_EPS)) - { - return face; - } - else - m_status=eStatus::NonConvex; - } - else - m_status=eStatus::Degenerated; + return false; + } + sFace* newface(sSV* a, sSV* b, sSV* c, bool forced) + { + if (m_stock.root) + { + sFace* face = m_stock.root; + remove(m_stock, face); + append(m_hull, face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = btCross(b->w - a->w, c->w - a->w); + const btScalar l = face->n.length(); + const bool v = l > EPA_ACCURACY; - remove(m_hull, face); - append(m_stock, face); - return 0; + if (v) + { + if (!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) + { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = btDot(a->w, face->n) / l; + } + face->n /= l; + if (forced || (face->d >= -EPA_PLANE_EPS)) + { + return face; } - m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces; - return 0; + else + m_status = eStatus::NonConvex; + } + else + m_status = eStatus::Degenerated; + + remove(m_hull, face); + append(m_stock, face); + return 0; + } + m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces; + return 0; + } + sFace* findbest() + { + sFace* minf = m_hull.root; + btScalar mind = minf->d * minf->d; + for (sFace* f = minf->l[1]; f; f = f->l[1]) + { + const btScalar sqd = f->d * f->d; + if (sqd < mind) + { + minf = f; + mind = sqd; } - sFace* findbest() + } + return (minf); + } + bool expand(U pass, sSV* w, sFace* f, U e, sHorizon& horizon) + { + static const U i1m3[] = {1, 2, 0}; + static const U i2m3[] = {2, 0, 1}; + if (f->pass != pass) + { + const U e1 = i1m3[e]; + if ((btDot(f->n, w->w) - f->d) < -EPA_PLANE_EPS) { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - for(sFace* f=minf->l[1];f;f=f->l[1]) + sFace* nf = newface(f->c[e1], f->c[e], w, false); + if (nf) { - const btScalar sqd=f->d*f->d; - if(sqd<mind) - { - minf=f; - mind=sqd; - } + bind(nf, 0, f, e); + if (horizon.cf) + bind(horizon.cf, 1, nf, 2); + else + horizon.ff = nf; + horizon.cf = nf; + ++horizon.nf; + return (true); } - return(minf); } - bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) + else { - static const U i1m3[]={1,2,0}; - static const U i2m3[]={2,0,1}; - if(f->pass!=pass) + const U e2 = i2m3[e]; + f->pass = (U1)pass; + if (expand(pass, w, f->f[e1], f->e[e1], horizon) && + expand(pass, w, f->f[e2], f->e[e2], horizon)) { - const U e1=i1m3[e]; - if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) - { - sFace* nf=newface(f->c[e1],f->c[e],w,false); - if(nf) - { - bind(nf,0,f,e); - if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; - horizon.cf=nf; - ++horizon.nf; - return(true); - } - } - else - { - const U e2=i2m3[e]; - f->pass = (U1)pass; - if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& - expand(pass,w,f->f[e2],f->e[e2],horizon)) - { - remove(m_hull,f); - append(m_stock,f); - return(true); - } - } + remove(m_hull, f); + append(m_stock, f); + return (true); } - return(false); } - - }; - - // - static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - btGjkEpaSolver2::sResults& results, - tShape& shape, - bool withmargins) - { - /* Results */ - results.witnesses[0] = - results.witnesses[1] = btVector3(0,0,0); - results.status = btGjkEpaSolver2::sResults::Separated; - /* Shape */ - shape.m_shapes[0] = shape0; - shape.m_shapes[1] = shape1; - shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); - shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); - shape.EnableMargin(withmargins); + } + return (false); } +}; +// +static void Initialize(const btConvexShape* shape0, const btTransform& wtrs0, + const btConvexShape* shape1, const btTransform& wtrs1, + btGjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) +{ + /* Results */ + results.witnesses[0] = + results.witnesses[1] = btVector3(0, 0, 0); + results.status = btGjkEpaSolver2::sResults::Separated; + /* Shape */ + shape.m_shapes[0] = shape0; + shape.m_shapes[1] = shape1; + shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); + shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); + shape.EnableMargin(withmargins); } +} // namespace gjkepa2_impl + // // Api // -using namespace gjkepa2_impl; +using namespace gjkepa2_impl; // -int btGjkEpaSolver2::StackSizeRequirement() +int btGjkEpaSolver2::StackSizeRequirement() { - return(sizeof(GJK)+sizeof(EPA)); + return (sizeof(GJK) + sizeof(EPA)); } // -bool btGjkEpaSolver2::Distance( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) +bool btGjkEpaSolver2::Distance(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) { - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); - if(gjk_status==GJK::eStatus::Valid) + tShape shape; + Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, false); + GJK gjk; + GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, guess); + if (gjk_status == GJK::eStatus::Valid) { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;i<gjk.m_simplex->rank;++i) + btVector3 w0 = btVector3(0, 0, 0); + btVector3 w1 = btVector3(0, 0, 0); + for (U i = 0; i < gjk.m_simplex->rank; ++i) { - const btScalar p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + const btScalar p = gjk.m_simplex->p[i]; + w0 += shape.Support(gjk.m_simplex->c[i]->d, 0) * p; + w1 += shape.Support(-gjk.m_simplex->c[i]->d, 1) * p; } - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*w1; - results.normal = w0-w1; - results.distance = results.normal.length(); - results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; - return(true); + results.witnesses[0] = wtrs0 * w0; + results.witnesses[1] = wtrs0 * w1; + results.normal = w0 - w1; + results.distance = results.normal.length(); + results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1; + return (true); } else { - results.status = gjk_status==GJK::eStatus::Inside? - sResults::Penetrating : - sResults::GJK_Failed ; - return(false); + results.status = gjk_status == GJK::eStatus::Inside ? sResults::Penetrating : sResults::GJK_Failed; + return (false); } } // -bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results, - bool usemargins) +bool btGjkEpaSolver2::Penetration(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + bool usemargins) { - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); - switch(gjk_status) + tShape shape; + Initialize(shape0, wtrs0, shape1, wtrs1, results, shape, usemargins); + GJK gjk; + GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, -guess); + switch (gjk_status) { - case GJK::eStatus::Inside: + case GJK::eStatus::Inside: { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) + EPA epa; + EPA::eStatus::_ epa_status = epa.Evaluate(gjk, -guess); + if (epa_status != EPA::eStatus::Failed) { - btVector3 w0=btVector3(0,0,0); - for(U i=0;i<epa.m_result.rank;++i) + btVector3 w0 = btVector3(0, 0, 0); + for (U i = 0; i < epa.m_result.rank; ++i) { - w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; + w0 += shape.Support(epa.m_result.c[i]->d, 0) * epa.m_result.p[i]; } - results.status = sResults::Penetrating; - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); - results.normal = -epa.m_normal; - results.distance = -epa.m_depth; - return(true); - } else results.status=sResults::EPA_Failed; + results.status = sResults::Penetrating; + results.witnesses[0] = wtrs0 * w0; + results.witnesses[1] = wtrs0 * (w0 - epa.m_normal * epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return (true); + } + else + results.status = sResults::EPA_Failed; } break; - case GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; + case GJK::eStatus::Failed: + results.status = sResults::GJK_Failed; + break; default: - { - } + { + } } - return(false); + return (false); } #ifndef __SPU__ // -btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, - btScalar margin, - const btConvexShape* shape0, - const btTransform& wtrs0, - sResults& results) +btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, + btScalar margin, + const btConvexShape* shape0, + const btTransform& wtrs0, + sResults& results) { - tShape shape; - btSphereShape shape1(margin); - btTransform wtrs1(btQuaternion(0,0,0,1),position); - Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); - if(gjk_status==GJK::eStatus::Valid) + tShape shape; + btSphereShape shape1(margin); + btTransform wtrs1(btQuaternion(0, 0, 0, 1), position); + Initialize(shape0, wtrs0, &shape1, wtrs1, results, shape, false); + GJK gjk; + GJK::eStatus::_ gjk_status = gjk.Evaluate(shape, btVector3(1, 1, 1)); + if (gjk_status == GJK::eStatus::Valid) { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;i<gjk.m_simplex->rank;++i) + btVector3 w0 = btVector3(0, 0, 0); + btVector3 w1 = btVector3(0, 0, 0); + for (U i = 0; i < gjk.m_simplex->rank; ++i) { - const btScalar p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + const btScalar p = gjk.m_simplex->p[i]; + w0 += shape.Support(gjk.m_simplex->c[i]->d, 0) * p; + w1 += shape.Support(-gjk.m_simplex->c[i]->d, 1) * p; } - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*w1; - const btVector3 delta= results.witnesses[1]- - results.witnesses[0]; - const btScalar margin= shape0->getMarginNonVirtual()+ - shape1.getMarginNonVirtual(); - const btScalar length= delta.length(); - results.normal = delta/length; - results.witnesses[0] += results.normal*margin; - return(length-margin); + results.witnesses[0] = wtrs0 * w0; + results.witnesses[1] = wtrs0 * w1; + const btVector3 delta = results.witnesses[1] - + results.witnesses[0]; + const btScalar margin = shape0->getMarginNonVirtual() + + shape1.getMarginNonVirtual(); + const btScalar length = delta.length(); + results.normal = delta / length; + results.witnesses[0] += results.normal * margin; + results.distance = length - margin; + return results.distance; } else { - if(gjk_status==GJK::eStatus::Inside) + if (gjk_status == GJK::eStatus::Inside) { - if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + if (Penetration(shape0, wtrs0, &shape1, wtrs1, gjk.m_ray, results)) { - const btVector3 delta= results.witnesses[0]- - results.witnesses[1]; - const btScalar length= delta.length(); + const btVector3 delta = results.witnesses[0] - + results.witnesses[1]; + const btScalar length = delta.length(); if (length >= SIMD_EPSILON) - results.normal = delta/length; - return(-length); + results.normal = delta / length; + return (-length); } - } + } } - return(SIMD_INFINITY); + return (SIMD_INFINITY); } // -bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) +bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) { - if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) - return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); + if (!Distance(shape0, wtrs0, shape1, wtrs1, guess, results)) + return (Penetration(shape0, wtrs0, shape1, wtrs1, guess, results, false)); else - return(true); + return (true); } -#endif //__SPU__ +#endif //__SPU__ -/* Symbols cleanup */ +/* Symbols cleanup */ #undef GJK_MAX_ITERATIONS -#undef GJK_ACCURARY +#undef GJK_ACCURACY #undef GJK_MIN_DISTANCE #undef GJK_DUPLICATED_EPS #undef GJK_SIMPLEX2_EPS diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h index ac501d5ecfe..893daea3f54 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -28,48 +28,46 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #include "BulletCollision/CollisionShapes/btConvexShape.h" ///btGjkEpaSolver contributed under zlib by Nathanael Presson -struct btGjkEpaSolver2 +struct btGjkEpaSolver2 { -struct sResults + struct sResults { - enum eStatus + enum eStatus { - Separated, /* Shapes doesnt penetrate */ - Penetrating, /* Shapes are penetrating */ - GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ - } status; - btVector3 witnesses[2]; - btVector3 normal; - btScalar distance; + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + btVector3 witnesses[2]; + btVector3 normal; + btScalar distance; }; -static int StackSizeRequirement(); + static int StackSizeRequirement(); -static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - const btVector3& guess, - sResults& results); + static bool Distance(const btConvexShape* shape0, const btTransform& wtrs0, + const btConvexShape* shape1, const btTransform& wtrs1, + const btVector3& guess, + sResults& results); -static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, + static bool Penetration(const btConvexShape* shape0, const btTransform& wtrs0, + const btConvexShape* shape1, const btTransform& wtrs1, const btVector3& guess, sResults& results, - bool usemargins=true); + bool usemargins = true); #ifndef __SPU__ -static btScalar SignedDistance( const btVector3& position, - btScalar margin, - const btConvexShape* shape, - const btTransform& wtrs, - sResults& results); - -static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - const btVector3& guess, - sResults& results); -#endif //__SPU__ + static btScalar SignedDistance(const btVector3& position, + btScalar margin, + const btConvexShape* shape, + const btTransform& wtrs, + sResults& results); + static bool SignedDistance(const btConvexShape* shape0, const btTransform& wtrs0, + const btConvexShape* shape1, const btTransform& wtrs1, + const btVector3& guess, + sResults& results); +#endif //__SPU__ }; -#endif //BT_GJK_EPA2_H - +#endif //BT_GJK_EPA2_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h index ce1f24bc50d..6fedbbb3e5f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h @@ -29,915 +29,946 @@ Improvements and refactoring by Erwin Coumans, 2008-2014 #include "LinearMath/btTransform.h" #include "btGjkCollisionDescription.h" - - -struct btGjkEpaSolver3 +struct btGjkEpaSolver3 { -struct sResults + struct sResults { - enum eStatus + enum eStatus { - Separated, /* Shapes doesnt penetrate */ - Penetrating, /* Shapes are penetrating */ - GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ - } status; - btVector3 witnesses[2]; - btVector3 normal; - btScalar distance; + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + btVector3 witnesses[2]; + btVector3 normal; + btScalar distance; }; - - }; - - -#if defined(DEBUG) || defined (_DEBUG) -#include <stdio.h> //for debug printf +#if defined(DEBUG) || defined(_DEBUG) +#include <stdio.h> //for debug printf #ifdef __SPU__ #include <spu_printf.h> #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #endif +// Config - - // Config - - /* GJK */ -#define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((btScalar)0.0001) -#define GJK_MIN_DISTANCE ((btScalar)0.0001) -#define GJK_DUPLICATED_EPS ((btScalar)0.0001) -#define GJK_SIMPLEX2_EPS ((btScalar)0.0) -#define GJK_SIMPLEX3_EPS ((btScalar)0.0) -#define GJK_SIMPLEX4_EPS ((btScalar)0.0) - - /* EPA */ -#define EPA_MAX_VERTICES 64 -#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) -#define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((btScalar)0.0001) -#define EPA_FALLBACK (10*EPA_ACCURACY) -#define EPA_PLANE_EPS ((btScalar)0.00001) -#define EPA_INSIDE_EPS ((btScalar)0.01) - - - // Shorthands - typedef unsigned int U; - typedef unsigned char U1; - - // MinkowskiDiff - template <typename btConvexTemplate> - struct MinkowskiDiff - { - const btConvexTemplate* m_convexAPtr; - const btConvexTemplate* m_convexBPtr; - - btMatrix3x3 m_toshape1; - btTransform m_toshape0; - - bool m_enableMargin; - - - MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b) - :m_convexAPtr(&a), - m_convexBPtr(&b) - { - } - - void EnableMargin(bool enable) - { - m_enableMargin = enable; - } - inline btVector3 Support0(const btVector3& d) const - { - return m_convexAPtr->getLocalSupportWithMargin(d); - } - inline btVector3 Support1(const btVector3& d) const - { - return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d); - } - - - inline btVector3 Support(const btVector3& d) const - { - return(Support0(d)-Support1(-d)); - } - btVector3 Support(const btVector3& d,U index) const - { - if(index) - return(Support1(d)); - else - return(Support0(d)); - } - }; - -enum eGjkStatus +/* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURARY ((btScalar)0.0001) +#define GJK_MIN_DISTANCE ((btScalar)0.0001) +#define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#define GJK_SIMPLEX2_EPS ((btScalar)0.0) +#define GJK_SIMPLEX3_EPS ((btScalar)0.0) +#define GJK_SIMPLEX4_EPS ((btScalar)0.0) + +/* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES * 2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((btScalar)0.0001) +#define EPA_FALLBACK (10 * EPA_ACCURACY) +#define EPA_PLANE_EPS ((btScalar)0.00001) +#define EPA_INSIDE_EPS ((btScalar)0.01) + +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; + +// MinkowskiDiff +template <typename btConvexTemplate> +struct MinkowskiDiff { - eGjkValid, - eGjkInside, - eGjkFailed + const btConvexTemplate* m_convexAPtr; + const btConvexTemplate* m_convexBPtr; + + btMatrix3x3 m_toshape1; + btTransform m_toshape0; + + bool m_enableMargin; + + MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b) + : m_convexAPtr(&a), + m_convexBPtr(&b) + { + } + + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + return m_convexAPtr->getLocalSupportWithMargin(d); + } + inline btVector3 Support1(const btVector3& d) const + { + return m_toshape0 * m_convexBPtr->getLocalSupportWithMargin(m_toshape1 * d); + } + + inline btVector3 Support(const btVector3& d) const + { + return (Support0(d) - Support1(-d)); + } + btVector3 Support(const btVector3& d, U index) const + { + if (index) + return (Support1(d)); + else + return (Support0(d)); + } }; - // GJK - template <typename btConvexTemplate> - struct GJK - { - /* Types */ - struct sSV - { - btVector3 d,w; - }; - struct sSimplex - { - sSV* c[4]; - btScalar p[4]; - U rank; - }; - - /* Fields */ - - MinkowskiDiff<btConvexTemplate> m_shape; - btVector3 m_ray; - btScalar m_distance; - sSimplex m_simplices[2]; - sSV m_store[4]; - sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; - eGjkStatus m_status; - /* Methods */ - - GJK(const btConvexTemplate& a, const btConvexTemplate& b) - :m_shape(a,b) - { - Initialize(); - } - void Initialize() - { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eGjkFailed; - m_current = 0; - m_distance = 0; - } - eGjkStatus Evaluate(const MinkowskiDiff<btConvexTemplate>& shapearg,const btVector3& guess) - { - U iterations=0; - btScalar sqdist=0; - btScalar alpha=0; - btVector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eGjkValid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar rl=m_ray.length(); - if(rl<GJK_MIN_DISTANCE) - {/* Touching or inside */ - m_status=eGjkInside; - break; - } - /* Append new vertice in -'v' direction */ - appendvertice(cs,-m_ray); - const btVector3& w=cs.c[cs.rank-1]->w; - bool found=false; - for(U i=0;i<4;++i) - { - if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS) - { found=true;break; } - } - if(found) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - else - {/* Update lastw */ - lastw[clastw=(clastw+1)&3]=w; - } - /* Check for termination */ - const btScalar omega=btDot(m_ray,w)/rl; - alpha=btMax(omega,alpha); - if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - /* Reduce simplex */ - btScalar weights[4]; - U mask=0; - switch(cs.rank) - { - case 2: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - weights,mask);break; - case 3: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - weights,mask);break; - case 4: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - cs.c[3]->w, - weights,mask);break; - } - if(sqdist>=0) - {/* Valid */ - ns.rank = 0; - m_ray = btVector3(0,0,0); - m_current = next; - for(U i=0,ni=cs.rank;i<ni;++i) - { - if(mask&(1<<i)) - { - ns.c[ns.rank] = cs.c[i]; - ns.p[ns.rank++] = weights[i]; - m_ray += cs.c[i]->w*weights[i]; - } - else - { - m_free[m_nfree++] = cs.c[i]; - } - } - if(mask==15) m_status=eGjkInside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eGjkFailed; - } while(m_status==eGjkValid); - m_simplex=&m_simplices[m_current]; - switch(m_status) - { - case eGjkValid: m_distance=m_ray.length();break; - case eGjkInside: m_distance=0;break; - default: - { - } - } - return(m_status); - } - bool EncloseOrigin() - { - switch(m_simplex->rank) - { - case 1: - { - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - appendvertice(*m_simplex, axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 2: - { - const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - const btVector3 p=btCross(d,axis); - if(p.length2()>0) - { - appendvertice(*m_simplex, p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - } - break; - case 3: - { - const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w, - m_simplex->c[2]->w-m_simplex->c[0]->w); - if(n.length2()>0) - { - appendvertice(*m_simplex,n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 4: - { - if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, - m_simplex->c[1]->w-m_simplex->c[3]->w, - m_simplex->c[2]->w-m_simplex->c[3]->w))>0) - return(true); - } - break; - } - return(false); - } - /* Internals */ - void getsupport(const btVector3& d,sSV& sv) const - { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); - } - void removevertice(sSimplex& simplex) - { - m_free[m_nfree++]=simplex.c[--simplex.rank]; - } - void appendvertice(sSimplex& simplex,const btVector3& v) - { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); - } - static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) - { - return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- - a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ - a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); - } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - btScalar* w,U& m) - { - const btVector3 d=b-a; - const btScalar l=d.length2(); - if(l>GJK_SIMPLEX2_EPS) - { - const btScalar t(l>0?-btDot(a,d)/l:0); - if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } - else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } - else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } - } - return(-1); - } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar* w,U& m) - { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c}; - const btVector3 dl[]={a-b,b-c,c-a}; - const btVector3 n=btCross(dl[0],dl[1]); - const btScalar l=n.length2(); - if(l>GJK_SIMPLEX3_EPS) - { - btScalar mindist=-1; - btScalar subw[2]={0.f,0.f}; - U subm(0); - for(U i=0;i<3;++i) - { - if(btDot(*vt[i],btCross(dl[i],n))>0) - { - const U j=imd3[i]; - const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); - if((mindist<0)||(subd<mindist)) - { - mindist = subd; - m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); - w[i] = subw[0]; - w[j] = subw[1]; - w[imd3[j]] = 0; - } - } - } - if(mindist<0) - { - const btScalar d=btDot(a,n); - const btScalar s=btSqrt(l); - const btVector3 p=n*(d/l); - mindist = p.length2(); - m = 7; - w[0] = (btCross(dl[1],b-p)).length()/s; - w[1] = (btCross(dl[2],c-p)).length()/s; - w[2] = 1-(w[0]+w[1]); - } - return(mindist); - } - return(-1); - } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& d, - btScalar* w,U& m) - { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c,&d}; - const btVector3 dl[]={a-d,b-d,c-d}; - const btScalar vl=det(dl[0],dl[1],dl[2]); - const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0; - if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS)) - { - btScalar mindist=-1; - btScalar subw[3]={0.f,0.f,0.f}; - U subm(0); - for(U i=0;i<3;++i) - { - const U j=imd3[i]; - const btScalar s=vl*btDot(d,btCross(dl[i],dl[j])); - if(s>0) - { - const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); - if((mindist<0)||(subd<mindist)) - { - mindist = subd; - m = static_cast<U>((subm&1?1<<i:0)+ - (subm&2?1<<j:0)+ - (subm&4?8:0)); - w[i] = subw[0]; - w[j] = subw[1]; - w[imd3[j]] = 0; - w[3] = subw[2]; - } - } - } - if(mindist<0) - { - mindist = 0; - m = 15; - w[0] = det(c,b,d)/vl; - w[1] = det(a,c,d)/vl; - w[2] = det(b,a,d)/vl; - w[3] = 1-(w[0]+w[1]+w[2]); - } - return(mindist); - } - return(-1); - } - }; +enum eGjkStatus +{ + eGjkValid, + eGjkInside, + eGjkFailed +}; + +// GJK +template <typename btConvexTemplate> +struct GJK +{ + /* Types */ + struct sSV + { + btVector3 d, w; + }; + struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; + + /* Fields */ + MinkowskiDiff<btConvexTemplate> m_shape; + btVector3 m_ray; + btScalar m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + U m_nfree; + U m_current; + sSimplex* m_simplex; + eGjkStatus m_status; + /* Methods */ + + GJK(const btConvexTemplate& a, const btConvexTemplate& b) + : m_shape(a, b) + { + Initialize(); + } + void Initialize() + { + m_ray = btVector3(0, 0, 0); + m_nfree = 0; + m_status = eGjkFailed; + m_current = 0; + m_distance = 0; + } + eGjkStatus Evaluate(const MinkowskiDiff<btConvexTemplate>& shapearg, const btVector3& guess) + { + U iterations = 0; + btScalar sqdist = 0; + btScalar alpha = 0; + btVector3 lastw[4]; + U clastw = 0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eGjkValid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const btScalar sqrl = m_ray.length2(); + appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : btVector3(1, 0, 0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do + { + const U next = 1 - m_current; + sSimplex& cs = m_simplices[m_current]; + sSimplex& ns = m_simplices[next]; + /* Check zero */ + const btScalar rl = m_ray.length(); + if (rl < GJK_MIN_DISTANCE) + { /* Touching or inside */ + m_status = eGjkInside; + break; + } + /* Append new vertice in -'v' direction */ + appendvertice(cs, -m_ray); + const btVector3& w = cs.c[cs.rank - 1]->w; + bool found = false; + for (U i = 0; i < 4; ++i) + { + if ((w - lastw[i]).length2() < GJK_DUPLICATED_EPS) + { + found = true; + break; + } + } + if (found) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + { /* Update lastw */ + lastw[clastw = (clastw + 1) & 3] = w; + } + /* Check for termination */ + const btScalar omega = btDot(m_ray, w) / rl; + alpha = btMax(omega, alpha); + if (((rl - alpha) - (GJK_ACCURARY * rl)) <= 0) + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + btScalar weights[4]; + U mask = 0; + switch (cs.rank) + { + case 2: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + weights, mask); + break; + case 3: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights, mask); + break; + case 4: + sqdist = projectorigin(cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights, mask); + break; + } + if (sqdist >= 0) + { /* Valid */ + ns.rank = 0; + m_ray = btVector3(0, 0, 0); + m_current = next; + for (U i = 0, ni = cs.rank; i < ni; ++i) + { + if (mask & (1 << i)) + { + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w * weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if (mask == 15) m_status = eGjkInside; + } + else + { /* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eGjkFailed; + } while (m_status == eGjkValid); + m_simplex = &m_simplices[m_current]; + switch (m_status) + { + case eGjkValid: + m_distance = m_ray.length(); + break; + case eGjkInside: + m_distance = 0; + break; + default: + { + } + } + return (m_status); + } + bool EncloseOrigin() + { + switch (m_simplex->rank) + { + case 1: + { + for (U i = 0; i < 3; ++i) + { + btVector3 axis = btVector3(0, 0, 0); + axis[i] = 1; + appendvertice(*m_simplex, axis); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -axis); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d = m_simplex->c[1]->w - m_simplex->c[0]->w; + for (U i = 0; i < 3; ++i) + { + btVector3 axis = btVector3(0, 0, 0); + axis[i] = 1; + const btVector3 p = btCross(d, axis); + if (p.length2() > 0) + { + appendvertice(*m_simplex, p); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -p); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n = btCross(m_simplex->c[1]->w - m_simplex->c[0]->w, + m_simplex->c[2]->w - m_simplex->c[0]->w); + if (n.length2() > 0) + { + appendvertice(*m_simplex, n); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + appendvertice(*m_simplex, -n); + if (EncloseOrigin()) return (true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if (btFabs(det(m_simplex->c[0]->w - m_simplex->c[3]->w, + m_simplex->c[1]->w - m_simplex->c[3]->w, + m_simplex->c[2]->w - m_simplex->c[3]->w)) > 0) + return (true); + } + break; + } + return (false); + } + /* Internals */ + void getsupport(const btVector3& d, sSV& sv) const + { + sv.d = d / d.length(); + sv.w = m_shape.Support(sv.d); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++] = simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex, const btVector3& v) + { + simplex.p[simplex.rank] = 0; + simplex.c[simplex.rank] = m_free[--m_nfree]; + getsupport(v, *simplex.c[simplex.rank++]); + } + static btScalar det(const btVector3& a, const btVector3& b, const btVector3& c) + { + return (a.y() * b.z() * c.x() + a.z() * b.x() * c.y() - + a.x() * b.z() * c.y() - a.y() * b.x() * c.z() + + a.x() * b.y() * c.z() - a.z() * b.y() * c.x()); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + btScalar* w, U& m) + { + const btVector3 d = b - a; + const btScalar l = d.length2(); + if (l > GJK_SIMPLEX2_EPS) + { + const btScalar t(l > 0 ? -btDot(a, d) / l : 0); + if (t >= 1) + { + w[0] = 0; + w[1] = 1; + m = 2; + return (b.length2()); + } + else if (t <= 0) + { + w[0] = 1; + w[1] = 0; + m = 1; + return (a.length2()); + } + else + { + w[0] = 1 - (w[1] = t); + m = 3; + return ((a + d * t).length2()); + } + } + return (-1); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w, U& m) + { + static const U imd3[] = {1, 2, 0}; + const btVector3* vt[] = {&a, &b, &c}; + const btVector3 dl[] = {a - b, b - c, c - a}; + const btVector3 n = btCross(dl[0], dl[1]); + const btScalar l = n.length2(); + if (l > GJK_SIMPLEX3_EPS) + { + btScalar mindist = -1; + btScalar subw[2] = {0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + if (btDot(*vt[i], btCross(dl[i], n)) > 0) + { + const U j = imd3[i]; + const btScalar subd(projectorigin(*vt[i], *vt[j], subw, subm)); + if ((mindist < 0) || (subd < mindist)) + { + mindist = subd; + m = static_cast<U>(((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + } + } + } + if (mindist < 0) + { + const btScalar d = btDot(a, n); + const btScalar s = btSqrt(l); + const btVector3 p = n * (d / l); + mindist = p.length2(); + m = 7; + w[0] = (btCross(dl[1], b - p)).length() / s; + w[1] = (btCross(dl[2], c - p)).length() / s; + w[2] = 1 - (w[0] + w[1]); + } + return (mindist); + } + return (-1); + } + static btScalar projectorigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& d, + btScalar* w, U& m) + { + static const U imd3[] = {1, 2, 0}; + const btVector3* vt[] = {&a, &b, &c, &d}; + const btVector3 dl[] = {a - d, b - d, c - d}; + const btScalar vl = det(dl[0], dl[1], dl[2]); + const bool ng = (vl * btDot(a, btCross(b - c, a - b))) <= 0; + if (ng && (btFabs(vl) > GJK_SIMPLEX4_EPS)) + { + btScalar mindist = -1; + btScalar subw[3] = {0.f, 0.f, 0.f}; + U subm(0); + for (U i = 0; i < 3; ++i) + { + const U j = imd3[i]; + const btScalar s = vl * btDot(d, btCross(dl[i], dl[j])); + if (s > 0) + { + const btScalar subd = projectorigin(*vt[i], *vt[j], d, subw, subm); + if ((mindist < 0) || (subd < mindist)) + { + mindist = subd; + m = static_cast<U>((subm & 1 ? 1 << i : 0) + + (subm & 2 ? 1 << j : 0) + + (subm & 4 ? 8 : 0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + w[3] = subw[2]; + } + } + } + if (mindist < 0) + { + mindist = 0; + m = 15; + w[0] = det(c, b, d) / vl; + w[1] = det(a, c, d) / vl; + w[2] = det(b, a, d) / vl; + w[3] = 1 - (w[0] + w[1] + w[2]); + } + return (mindist); + } + return (-1); + } +}; -enum eEpaStatus +enum eEpaStatus { - eEpaValid, - eEpaTouching, - eEpaDegenerated, - eEpaNonConvex, - eEpaInvalidHull, - eEpaOutOfFaces, - eEpaOutOfVertices, - eEpaAccuraryReached, - eEpaFallBack, - eEpaFailed + eEpaValid, + eEpaTouching, + eEpaDegenerated, + eEpaNonConvex, + eEpaInvalidHull, + eEpaOutOfFaces, + eEpaOutOfVertices, + eEpaAccuraryReached, + eEpaFallBack, + eEpaFailed }; +// EPA +template <typename btConvexTemplate> +struct EPA +{ + /* Types */ + + struct sFace + { + btVector3 n; + btScalar d; + typename GJK<btConvexTemplate>::sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + U1 e[3]; + U1 pass; + }; + struct sList + { + sFace* root; + U count; + sList() : root(0), count(0) {} + }; + struct sHorizon + { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(0), ff(0), nf(0) {} + }; + + /* Fields */ + eEpaStatus m_status; + typename GJK<btConvexTemplate>::sSimplex m_result; + btVector3 m_normal; + btScalar m_depth; + typename GJK<btConvexTemplate>::sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + U m_nextsv; + sList m_hull; + sList m_stock; + /* Methods */ + EPA() + { + Initialize(); + } + + static inline void bind(sFace* fa, U ea, sFace* fb, U eb) + { + fa->e[ea] = (U1)eb; + fa->f[ea] = fb; + fb->e[eb] = (U1)ea; + fb->f[eb] = fa; + } + static inline void append(sList& list, sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if (list.root) list.root->l[0] = face; + list.root = face; + ++list.count; + } + static inline void remove(sList& list, sFace* face) + { + if (face->l[1]) face->l[1]->l[0] = face->l[0]; + if (face->l[0]) face->l[0]->l[1] = face->l[1]; + if (face == list.root) list.root = face->l[1]; + --list.count; + } + + void Initialize() + { + m_status = eEpaFailed; + m_normal = btVector3(0, 0, 0); + m_depth = 0; + m_nextsv = 0; + for (U i = 0; i < EPA_MAX_FACES; ++i) + { + append(m_stock, &m_fc_store[EPA_MAX_FACES - i - 1]); + } + } + eEpaStatus Evaluate(GJK<btConvexTemplate>& gjk, const btVector3& guess) + { + typename GJK<btConvexTemplate>::sSimplex& simplex = *gjk.m_simplex; + if ((simplex.rank > 1) && gjk.EncloseOrigin()) + { + /* Clean up */ + while (m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull, f); + append(m_stock, f); + } + m_status = eEpaValid; + m_nextsv = 0; + /* Orient simplex */ + if (gjk.det(simplex.c[0]->w - simplex.c[3]->w, + simplex.c[1]->w - simplex.c[3]->w, + simplex.c[2]->w - simplex.c[3]->w) < 0) + { + btSwap(simplex.c[0], simplex.c[1]); + btSwap(simplex.p[0], simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[] = {newface(simplex.c[0], simplex.c[1], simplex.c[2], true), + newface(simplex.c[1], simplex.c[0], simplex.c[3], true), + newface(simplex.c[2], simplex.c[1], simplex.c[3], true), + newface(simplex.c[0], simplex.c[2], simplex.c[3], true)}; + if (m_hull.count == 4) + { + sFace* best = findbest(); + sFace outer = *best; + U pass = 0; + U iterations = 0; + bind(tetra[0], 0, tetra[1], 0); + bind(tetra[0], 1, tetra[2], 0); + bind(tetra[0], 2, tetra[3], 0); + bind(tetra[1], 1, tetra[3], 2); + bind(tetra[1], 2, tetra[2], 1); + bind(tetra[2], 2, tetra[3], 1); + m_status = eEpaValid; + for (; iterations < EPA_MAX_ITERATIONS; ++iterations) + { + if (m_nextsv < EPA_MAX_VERTICES) + { + sHorizon horizon; + typename GJK<btConvexTemplate>::sSV* w = &m_sv_store[m_nextsv++]; + bool valid = true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n, *w); + const btScalar wdist = btDot(best->n, w->w) - best->d; + if (wdist > EPA_ACCURACY) + { + for (U j = 0; (j < 3) && valid; ++j) + { + valid &= expand(pass, w, + best->f[j], best->e[j], + horizon); + } + if (valid && (horizon.nf >= 3)) + { + bind(horizon.cf, 1, horizon.ff, 2); + remove(m_hull, best); + append(m_stock, best); + best = findbest(); + outer = *best; + } + else + { + m_status = eEpaInvalidHull; + break; + } + } + else + { + m_status = eEpaAccuraryReached; + break; + } + } + else + { + m_status = eEpaOutOfVertices; + break; + } + } + const btVector3 projection = outer.n * outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = btCross(outer.c[1]->w - projection, + outer.c[2]->w - projection) + .length(); + m_result.p[1] = btCross(outer.c[2]->w - projection, + outer.c[0]->w - projection) + .length(); + m_result.p[2] = btCross(outer.c[0]->w - projection, + outer.c[1]->w - projection) + .length(); + const btScalar sum = m_result.p[0] + m_result.p[1] + m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return (m_status); + } + } + /* Fallback */ + m_status = eEpaFallBack; + m_normal = -guess; + const btScalar nl = m_normal.length(); + if (nl > 0) + m_normal = m_normal / nl; + else + m_normal = btVector3(1, 0, 0); + m_depth = 0; + m_result.rank = 1; + m_result.c[0] = simplex.c[0]; + m_result.p[0] = 1; + return (m_status); + } + bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist) + { + const btVector3 ba = b->w - a->w; + const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if (a_dot_nab < 0) + { + // Outside of edge a->b + + const btScalar ba_l2 = ba.length2(); + const btScalar a_dot_ba = btDot(a->w, ba); + const btScalar b_dot_ba = btDot(b->w, ba); + + if (a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); + } + else if (b_dot_ba < 0) + { + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const btScalar a_dot_b = btDot(a->w, b->w); + dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + } + + return true; + } + + return false; + } + sFace* newface(typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, typename GJK<btConvexTemplate>::sSV* c, bool forced) + { + if (m_stock.root) + { + sFace* face = m_stock.root; + remove(m_stock, face); + append(m_hull, face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = btCross(b->w - a->w, c->w - a->w); + const btScalar l = face->n.length(); + const bool v = l > EPA_ACCURACY; + + if (v) + { + if (!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) + { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = btDot(a->w, face->n) / l; + } + + face->n /= l; + if (forced || (face->d >= -EPA_PLANE_EPS)) + { + return face; + } + else + m_status = eEpaNonConvex; + } + else + m_status = eEpaDegenerated; + + remove(m_hull, face); + append(m_stock, face); + return 0; + } + m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces; + return 0; + } + sFace* findbest() + { + sFace* minf = m_hull.root; + btScalar mind = minf->d * minf->d; + for (sFace* f = minf->l[1]; f; f = f->l[1]) + { + const btScalar sqd = f->d * f->d; + if (sqd < mind) + { + minf = f; + mind = sqd; + } + } + return (minf); + } + bool expand(U pass, typename GJK<btConvexTemplate>::sSV* w, sFace* f, U e, sHorizon& horizon) + { + static const U i1m3[] = {1, 2, 0}; + static const U i2m3[] = {2, 0, 1}; + if (f->pass != pass) + { + const U e1 = i1m3[e]; + if ((btDot(f->n, w->w) - f->d) < -EPA_PLANE_EPS) + { + sFace* nf = newface(f->c[e1], f->c[e], w, false); + if (nf) + { + bind(nf, 0, f, e); + if (horizon.cf) + bind(horizon.cf, 1, nf, 2); + else + horizon.ff = nf; + horizon.cf = nf; + ++horizon.nf; + return (true); + } + } + else + { + const U e2 = i2m3[e]; + f->pass = (U1)pass; + if (expand(pass, w, f->f[e1], f->e[e1], horizon) && + expand(pass, w, f->f[e2], f->e[e2], horizon)) + { + remove(m_hull, f); + append(m_stock, f); + return (true); + } + } + } + return (false); + } +}; - // EPA template <typename btConvexTemplate> - struct EPA - { - /* Types */ - - struct sFace - { - btVector3 n; - btScalar d; - typename GJK<btConvexTemplate>::sSV* c[3]; - sFace* f[3]; - sFace* l[2]; - U1 e[3]; - U1 pass; - }; - struct sList - { - sFace* root; - U count; - sList() : root(0),count(0) {} - }; - struct sHorizon - { - sFace* cf; - sFace* ff; - U nf; - sHorizon() : cf(0),ff(0),nf(0) {} - }; - - /* Fields */ - eEpaStatus m_status; - typename GJK<btConvexTemplate>::sSimplex m_result; - btVector3 m_normal; - btScalar m_depth; - typename GJK<btConvexTemplate>::sSV m_sv_store[EPA_MAX_VERTICES]; - sFace m_fc_store[EPA_MAX_FACES]; - U m_nextsv; - sList m_hull; - sList m_stock; - /* Methods */ - EPA() - { - Initialize(); - } - - - static inline void bind(sFace* fa,U ea,sFace* fb,U eb) - { - fa->e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } - static inline void append(sList& list,sFace* face) - { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; - } - static inline void remove(sList& list,sFace* face) - { - if(face->l[1]) face->l[1]->l[0]=face->l[0]; - if(face->l[0]) face->l[0]->l[1]=face->l[1]; - if(face==list.root) list.root=face->l[1]; - --list.count; - } - - - void Initialize() - { - m_status = eEpaFailed; - m_normal = btVector3(0,0,0); - m_depth = 0; - m_nextsv = 0; - for(U i=0;i<EPA_MAX_FACES;++i) - { - append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); - } - } - eEpaStatus Evaluate(GJK<btConvexTemplate>& gjk,const btVector3& guess) - { - typename GJK<btConvexTemplate>::sSimplex& simplex=*gjk.m_simplex; - if((simplex.rank>1)&&gjk.EncloseOrigin()) - { - - /* Clean up */ - while(m_hull.root) - { - sFace* f = m_hull.root; - remove(m_hull,f); - append(m_stock,f); - } - m_status = eEpaValid; - m_nextsv = 0; - /* Orient simplex */ - if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, - simplex.c[1]->w-simplex.c[3]->w, - simplex.c[2]->w-simplex.c[3]->w)<0) - { - btSwap(simplex.c[0],simplex.c[1]); - btSwap(simplex.p[0],simplex.p[1]); - } - /* Build initial hull */ - sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), - newface(simplex.c[1],simplex.c[0],simplex.c[3],true), - newface(simplex.c[2],simplex.c[1],simplex.c[3],true), - newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; - if(m_hull.count==4) - { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U iterations=0; - bind(tetra[0],0,tetra[1],0); - bind(tetra[0],1,tetra[2],0); - bind(tetra[0],2,tetra[3],0); - bind(tetra[1],1,tetra[3],2); - bind(tetra[1],2,tetra[2],1); - bind(tetra[2],2,tetra[3],1); - m_status=eEpaValid; - for(;iterations<EPA_MAX_ITERATIONS;++iterations) - { - if(m_nextsv<EPA_MAX_VERTICES) - { - sHorizon horizon; - typename GJK<btConvexTemplate>::sSV* w=&m_sv_store[m_nextsv++]; - bool valid=true; - best->pass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const btScalar wdist=btDot(best->n,w->w)-best->d; - if(wdist>EPA_ACCURACY) - { - for(U j=0;(j<3)&&valid;++j) - { - valid&=expand( pass,w, - best->f[j],best->e[j], - horizon); - } - if(valid&&(horizon.nf>=3)) - { - bind(horizon.cf,1,horizon.ff,2); - remove(m_hull,best); - append(m_stock,best); - best=findbest(); - outer=*best; - } else { m_status=eEpaInvalidHull;break; } - } else { m_status=eEpaAccuraryReached;break; } - } else { m_status=eEpaOutOfVertices;break; } - } - const btVector3 projection=outer.n*outer.d; - m_normal = outer.n; - m_depth = outer.d; - m_result.rank = 3; - m_result.c[0] = outer.c[0]; - m_result.c[1] = outer.c[1]; - m_result.c[2] = outer.c[2]; - m_result.p[0] = btCross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = btCross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = btCross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; - m_result.p[0] /= sum; - m_result.p[1] /= sum; - m_result.p[2] /= sum; - return(m_status); - } - } - /* Fallback */ - m_status = eEpaFallBack; - m_normal = -guess; - const btScalar nl=m_normal.length(); - if(nl>0) - m_normal = m_normal/nl; - else - m_normal = btVector3(1,0,0); - m_depth = 0; - m_result.rank=1; - m_result.c[0]=simplex.c[0]; - m_result.p[0]=1; - return(m_status); - } - bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist) - { - const btVector3 ba = b->w - a->w; - const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane - const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required - - if(a_dot_nab < 0) - { - // Outside of edge a->b - - const btScalar ba_l2 = ba.length2(); - const btScalar a_dot_ba = btDot(a->w, ba); - const btScalar b_dot_ba = btDot(b->w, ba); - - if(a_dot_ba > 0) - { - // Pick distance vertex a - dist = a->w.length(); - } - else if(b_dot_ba < 0) - { - // Pick distance vertex b - dist = b->w.length(); - } - else - { - // Pick distance to edge a->b - const btScalar a_dot_b = btDot(a->w, b->w); - dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); - } - - return true; - } - - return false; - } - sFace* newface(typename GJK<btConvexTemplate>::sSV* a,typename GJK<btConvexTemplate>::sSV* b,typename GJK<btConvexTemplate>::sSV* c,bool forced) - { - if(m_stock.root) - { - sFace* face=m_stock.root; - remove(m_stock,face); - append(m_hull,face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = btCross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; - - if(v) - { - if(!(getedgedist(face, a, b, face->d) || - getedgedist(face, b, c, face->d) || - getedgedist(face, c, a, face->d))) - { - // Origin projects to the interior of the triangle - // Use distance to triangle plane - face->d = btDot(a->w, face->n) / l; - } - - face->n /= l; - if(forced || (face->d >= -EPA_PLANE_EPS)) - { - return face; - } - else - m_status=eEpaNonConvex; - } - else - m_status=eEpaDegenerated; - - remove(m_hull, face); - append(m_stock, face); - return 0; - - } - m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces; - return 0; - } - sFace* findbest() - { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const btScalar sqd=f->d*f->d; - if(sqd<mind) - { - minf=f; - mind=sqd; - } - } - return(minf); - } - bool expand(U pass,typename GJK<btConvexTemplate>::sSV* w,sFace* f,U e,sHorizon& horizon) - { - static const U i1m3[]={1,2,0}; - static const U i2m3[]={2,0,1}; - if(f->pass!=pass) - { - const U e1=i1m3[e]; - if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) - { - sFace* nf=newface(f->c[e1],f->c[e],w,false); - if(nf) - { - bind(nf,0,f,e); - if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; - horizon.cf=nf; - ++horizon.nf; - return(true); - } - } - else - { - const U e2=i2m3[e]; - f->pass = (U1)pass; - if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& - expand(pass,w,f->f[e2],f->e[e2],horizon)) - { - remove(m_hull,f); - append(m_stock,f); - return(true); - } - } - } - return(false); - } - - }; - - template <typename btConvexTemplate> - static void Initialize( const btConvexTemplate& a, const btConvexTemplate& b, - btGjkEpaSolver3::sResults& results, - MinkowskiDiff<btConvexTemplate>& shape) - { - /* Results */ - results.witnesses[0] = - results.witnesses[1] = btVector3(0,0,0); - results.status = btGjkEpaSolver3::sResults::Separated; - /* Shape */ - - shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis()); - shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform()); - - } - +static void Initialize(const btConvexTemplate& a, const btConvexTemplate& b, + btGjkEpaSolver3::sResults& results, + MinkowskiDiff<btConvexTemplate>& shape) +{ + /* Results */ + results.witnesses[0] = + results.witnesses[1] = btVector3(0, 0, 0); + results.status = btGjkEpaSolver3::sResults::Separated; + /* Shape */ + + shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis()); + shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform()); +} // // Api // - - // template <typename btConvexTemplate> -bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b, - const btVector3& guess, - btGjkEpaSolver3::sResults& results) +bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b, + const btVector3& guess, + btGjkEpaSolver3::sResults& results) { - MinkowskiDiff<btConvexTemplate> shape(a,b); - Initialize(a,b,results,shape); - GJK<btConvexTemplate> gjk(a,b); - eGjkStatus gjk_status=gjk.Evaluate(shape,guess); - if(gjk_status==eGjkValid) - { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;i<gjk.m_simplex->rank;++i) - { - const btScalar p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; - } - results.witnesses[0] = a.getWorldTransform()*w0; - results.witnesses[1] = a.getWorldTransform()*w1; - results.normal = w0-w1; - results.distance = results.normal.length(); - results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; - return(true); - } - else - { - results.status = gjk_status==eGjkInside? - btGjkEpaSolver3::sResults::Penetrating : - btGjkEpaSolver3::sResults::GJK_Failed ; - return(false); - } + MinkowskiDiff<btConvexTemplate> shape(a, b); + Initialize(a, b, results, shape); + GJK<btConvexTemplate> gjk(a, b); + eGjkStatus gjk_status = gjk.Evaluate(shape, guess); + if (gjk_status == eGjkValid) + { + btVector3 w0 = btVector3(0, 0, 0); + btVector3 w1 = btVector3(0, 0, 0); + for (U i = 0; i < gjk.m_simplex->rank; ++i) + { + const btScalar p = gjk.m_simplex->p[i]; + w0 += shape.Support(gjk.m_simplex->c[i]->d, 0) * p; + w1 += shape.Support(-gjk.m_simplex->c[i]->d, 1) * p; + } + results.witnesses[0] = a.getWorldTransform() * w0; + results.witnesses[1] = a.getWorldTransform() * w1; + results.normal = w0 - w1; + results.distance = results.normal.length(); + results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1; + return (true); + } + else + { + results.status = gjk_status == eGjkInside ? btGjkEpaSolver3::sResults::Penetrating : btGjkEpaSolver3::sResults::GJK_Failed; + return (false); + } } - template <typename btConvexTemplate> -bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a, - const btConvexTemplate& b, - const btVector3& guess, - btGjkEpaSolver3::sResults& results) +bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a, + const btConvexTemplate& b, + const btVector3& guess, + btGjkEpaSolver3::sResults& results) { - MinkowskiDiff<btConvexTemplate> shape(a,b); - Initialize(a,b,results,shape); - GJK<btConvexTemplate> gjk(a,b); - eGjkStatus gjk_status=gjk.Evaluate(shape,-guess); - switch(gjk_status) - { - case eGjkInside: - { - EPA<btConvexTemplate> epa; - eEpaStatus epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=eEpaFailed) - { - btVector3 w0=btVector3(0,0,0); - for(U i=0;i<epa.m_result.rank;++i) - { - w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; - } - results.status = btGjkEpaSolver3::sResults::Penetrating; - results.witnesses[0] = a.getWorldTransform()*w0; - results.witnesses[1] = a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth); - results.normal = -epa.m_normal; - results.distance = -epa.m_depth; - return(true); - } else results.status=btGjkEpaSolver3::sResults::EPA_Failed; - } - break; - case eGjkFailed: - results.status=btGjkEpaSolver3::sResults::GJK_Failed; - break; - default: - { - } - } - return(false); + MinkowskiDiff<btConvexTemplate> shape(a, b); + Initialize(a, b, results, shape); + GJK<btConvexTemplate> gjk(a, b); + eGjkStatus gjk_status = gjk.Evaluate(shape, -guess); + switch (gjk_status) + { + case eGjkInside: + { + EPA<btConvexTemplate> epa; + eEpaStatus epa_status = epa.Evaluate(gjk, -guess); + if (epa_status != eEpaFailed) + { + btVector3 w0 = btVector3(0, 0, 0); + for (U i = 0; i < epa.m_result.rank; ++i) + { + w0 += shape.Support(epa.m_result.c[i]->d, 0) * epa.m_result.p[i]; + } + results.status = btGjkEpaSolver3::sResults::Penetrating; + results.witnesses[0] = a.getWorldTransform() * w0; + results.witnesses[1] = a.getWorldTransform() * (w0 - epa.m_normal * epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return (true); + } + else + results.status = btGjkEpaSolver3::sResults::EPA_Failed; + } + break; + case eGjkFailed: + results.status = btGjkEpaSolver3::sResults::GJK_Failed; + break; + default: + { + } + } + return (false); } #if 0 @@ -990,28 +1021,28 @@ int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanc #endif template <typename btConvexTemplate, typename btDistanceInfoTemplate> -int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, - const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo) +int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, + const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo) { - btGjkEpaSolver3::sResults results; - btVector3 guess = colDesc.m_firstDir; - - bool isSeparated = btGjkEpaSolver3_Distance( a,b, - guess, - results); - if (isSeparated) - { - distInfo->m_distance = results.distance; - distInfo->m_pointOnA= results.witnesses[0]; - distInfo->m_pointOnB= results.witnesses[1]; - distInfo->m_normalBtoA= results.normal; - return 0; - } - - return -1; + btGjkEpaSolver3::sResults results; + btVector3 guess = colDesc.m_firstDir; + + bool isSeparated = btGjkEpaSolver3_Distance(a, b, + guess, + results); + if (isSeparated) + { + distInfo->m_distance = results.distance; + distInfo->m_pointOnA = results.witnesses[0]; + distInfo->m_pointOnB = results.witnesses[1]; + distInfo->m_normalBtoA = results.normal; + return 0; + } + + return -1; } -/* Symbols cleanup */ +/* Symbols cleanup */ #undef GJK_MAX_ITERATIONS #undef GJK_ACCURARY @@ -1029,7 +1060,4 @@ int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, #undef EPA_PLANE_EPS #undef EPA_INSIDE_EPS - - -#endif //BT_GJK_EPA3_H - +#endif //BT_GJK_EPA3_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 572ec36f563..07629229abc 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -18,49 +18,64 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "btGjkEpaPenetrationDepthSolver.h" - #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* pConvexA, const btConvexShape* pConvexB, - const btTransform& transformA, const btTransform& transformB, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw) +bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* pConvexA, const btConvexShape* pConvexB, + const btTransform& transformA, const btTransform& transformB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, + class btIDebugDraw* debugDraw) { - (void)debugDraw; (void)v; (void)simplexSolver; -// const btScalar radialmargin(btScalar(0.)); - - btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin()); - btGjkEpaSolver2::sResults results; - + btVector3 guessVectors[] = { + btVector3(transformB.getOrigin() - transformA.getOrigin()).safeNormalize(), + btVector3(transformA.getOrigin() - transformB.getOrigin()).safeNormalize(), + btVector3(0, 0, 1), + btVector3(0, 1, 0), + btVector3(1, 0, 0), + btVector3(1, 1, 0), + btVector3(1, 1, 1), + btVector3(0, 1, 1), + btVector3(1, 0, 1), + }; - if(btGjkEpaSolver2::Penetration(pConvexA,transformA, - pConvexB,transformB, - guessVector,results)) - - { - // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return true; - } else + int numVectors = sizeof(guessVectors) / sizeof(btVector3); + + for (int i = 0; i < numVectors; i++) { - if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results)) + simplexSolver.reset(); + btVector3 guessVector = guessVectors[i]; + + btGjkEpaSolver2::sResults results; + + if (btGjkEpaSolver2::Penetration(pConvexA, transformA, + pConvexB, transformB, + guessVector, results)) + { wWitnessOnA = results.witnesses[0]; wWitnessOnB = results.witnesses[1]; v = results.normal; - return false; + return true; + } + else + { + if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; + } } } + //failed to find a distance/penetration + wWitnessOnA.setValue(0, 0, 0); + wWitnessOnB.setValue(0, 0, 0); + v.setValue(0, 0, 0); return false; } - - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h index 1ed6340af3b..92d6df1729f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -23,21 +23,18 @@ subject to the following restrictions: ///calculate the penetration depth between two convex shapes. class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver { - public : - - btGjkEpaPenetrationDepthSolver() - { - } - - bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* pConvexA, const btConvexShape* pConvexB, - const btTransform& transformA, const btTransform& transformB, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw); - - private : - +public: + btGjkEpaPenetrationDepthSolver() + { + } + + bool calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* pConvexA, const btConvexShape* pConvexB, + const btTransform& transformA, const btTransform& transformB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, + class btIDebugDraw* debugDraw); + +private: }; -#endif // BT_GJP_EPA_PENETRATION_DEPTH_H - +#endif // BT_GJP_EPA_PENETRATION_DEPTH_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 759443a9613..5af93cb2fb3 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -18,80 +18,685 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" - - -#if defined(DEBUG) || defined (_DEBUG) +#if defined(DEBUG) || defined(_DEBUG) //#define TEST_NON_VIRTUAL 1 -#include <stdio.h> //for debug printf +#include <stdio.h> //for debug printf #ifdef __SPU__ #include <spu_printf.h> #define printf spu_printf -#endif //__SPU__ +#endif //__SPU__ #endif //must be above the machine epsilon +#ifdef BT_USE_DOUBLE_PRECISION +#define REL_ERROR2 btScalar(1.0e-12) +btScalar gGjkEpaPenetrationTolerance = 1.0e-12; +#else #define REL_ERROR2 btScalar(1.0e-6) +btScalar gGjkEpaPenetrationTolerance = 0.001; +#endif + -//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), -m_fixContactNormalDirection(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), -m_fixContactNormalDirection(1) -{ -} - -void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +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), + m_fixContactNormalDirection(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), + m_fixContactNormalDirection(1) +{ +} + +void btGjkPairDetector::getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults) { (void)swapResults; - getClosestPointsNonVirtual(input,output,debugDraw); + getClosestPointsNonVirtual(input, output, debugDraw); +} + +static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb) +{ + btVector3 separatingAxisInA = (dir)*localTransA.getBasis(); + btVector3 separatingAxisInB = (-dir) * localTransB.getBasis(); + + btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + + btVector3 pInA = pInANoMargin; + btVector3 qInB = qInBNoMargin; + + supAworld = localTransA(pInA); + supBworld = localTransB(qInB); + + if (check2d) + { + supAworld[2] = 0.f; + supBworld[2] = 0.f; + } + + aMinb = supAworld - supBworld; +} + +struct btSupportVector +{ + btVector3 v; //!< Support point in minkowski sum + btVector3 v1; //!< Support point in obj1 + btVector3 v2; //!< Support point in obj2 +}; + +struct btSimplex +{ + btSupportVector ps[4]; + int last; //!< index of last added point +}; + +static btVector3 ccd_vec3_origin(0, 0, 0); + +inline void btSimplexInit(btSimplex *s) +{ + s->last = -1; +} + +inline int btSimplexSize(const btSimplex *s) +{ + return s->last + 1; +} + +inline const btSupportVector *btSimplexPoint(const btSimplex *s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} +inline void btSupportCopy(btSupportVector *d, const btSupportVector *s) +{ + *d = *s; +} + +inline void btVec3Copy(btVector3 *v, const btVector3 *w) +{ + *v = *w; +} + +inline void ccdVec3Add(btVector3 *v, const btVector3 *w) +{ + v->m_floats[0] += w->m_floats[0]; + v->m_floats[1] += w->m_floats[1]; + v->m_floats[2] += w->m_floats[2]; +} + +inline void ccdVec3Sub(btVector3 *v, const btVector3 *w) +{ + *v -= *w; +} +inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) +{ + *d = (*v) - (*w); +} +inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b) +{ + btScalar dot; + dot = a->dot(*b); + + return dot; +} + +inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b) +{ + btVector3 ab; + btVec3Sub2(&ab, a, b); + return btVec3Dot(&ab, &ab); +} + +inline void btVec3Scale(btVector3 *d, btScalar k) +{ + d->m_floats[0] *= k; + d->m_floats[1] *= k; + d->m_floats[2] *= k; +} + +inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b) +{ + d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]); + d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]); + d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]); +} + +inline void btTripleCross(const btVector3 *a, const btVector3 *b, + const btVector3 *c, btVector3 *d) +{ + btVector3 e; + btVec3Cross(&e, a, b); + btVec3Cross(d, &e, c); +} + +inline int ccdEq(btScalar _a, btScalar _b) +{ + btScalar ab; + btScalar a, b; + + ab = btFabs(_a - _b); + if (btFabs(ab) < SIMD_EPSILON) + return 1; + + a = btFabs(_a); + b = btFabs(_b); + if (b > a) + { + return ab < SIMD_EPSILON * b; + } + else + { + return ab < SIMD_EPSILON * a; + } +} + +btScalar ccdVec3X(const btVector3 *v) +{ + return v->x(); +} + +btScalar ccdVec3Y(const btVector3 *v) +{ + return v->y(); +} + +btScalar ccdVec3Z(const btVector3 *v) +{ + return v->z(); +} +inline int btVec3Eq(const btVector3 *a, const btVector3 *b) +{ + return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) && ccdEq(ccdVec3Z(a), ccdVec3Z(b)); +} + +inline void btSimplexAdd(btSimplex *s, const btSupportVector *v) +{ + // here is no check on boundaries in sake of speed + ++s->last; + btSupportCopy(s->ps + s->last, v); +} + +inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a) +{ + btSupportCopy(s->ps + pos, a); +} + +inline void btSimplexSetSize(btSimplex *s, int size) +{ + s->last = size - 1; +} + +inline const btSupportVector *ccdSimplexLast(const btSimplex *s) +{ + return btSimplexPoint(s, s->last); +} + +inline int ccdSign(btScalar val) +{ + if (btFuzzyZero(val)) + { + return 0; + } + else if (val < btScalar(0)) + { + return -1; + } + return 1; +} + +inline btScalar btVec3PointSegmentDist2(const btVector3 *P, + const btVector3 *x0, + const btVector3 *b, + btVector3 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + btScalar dist, t; + btVector3 d, a; + + // direction of segment + btVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + btVec3Sub2(&a, x0, P); + + t = -btScalar(1.) * btVec3Dot(&a, &d); + t /= btVec3Dot(&d, &d); + + if (t < btScalar(0) || btFuzzyZero(t)) + { + dist = ccdVec3Dist2(x0, P); + if (witness) + btVec3Copy(witness, x0); + } + else if (t > btScalar(1) || ccdEq(t, btScalar(1))) + { + dist = ccdVec3Dist2(b, P); + if (witness) + btVec3Copy(witness, b); + } + else + { + if (witness) + { + btVec3Copy(witness, &d); + btVec3Scale(witness, t); + ccdVec3Add(witness, x0); + dist = ccdVec3Dist2(witness, P); + } + else + { + // recycling variables + btVec3Scale(&d, t); + ccdVec3Add(&d, &a); + dist = btVec3Dot(&d, &d); + } + } + + return dist; +} + +btScalar btVec3PointTriDist2(const btVector3 *P, + const btVector3 *x0, const btVector3 *B, + const btVector3 *C, + btVector3 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + btVector3 d1, d2, a; + double u, v, w, p, q, r; + double s, t, dist, dist2; + btVector3 witness2; + + btVec3Sub2(&d1, B, x0); + btVec3Sub2(&d2, C, x0); + btVec3Sub2(&a, x0, P); + + u = btVec3Dot(&a, &a); + v = btVec3Dot(&d1, &d1); + w = btVec3Dot(&d2, &d2); + p = btVec3Dot(&a, &d1); + q = btVec3Dot(&a, &d2); + r = btVec3Dot(&d1, &d2); + + s = (q * r - w * p) / (w * v - r * r); + t = (-s * r - q) / w; + + if ((btFuzzyZero(s) || s > btScalar(0)) && (ccdEq(s, btScalar(1)) || s < btScalar(1)) && (btFuzzyZero(t) || t > btScalar(0)) && (ccdEq(t, btScalar(1)) || t < btScalar(1)) && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1))) + { + if (witness) + { + btVec3Scale(&d1, s); + btVec3Scale(&d2, t); + btVec3Copy(witness, x0); + ccdVec3Add(witness, &d1); + ccdVec3Add(witness, &d2); + + dist = ccdVec3Dist2(witness, P); + } + else + { + dist = s * s * v; + dist += t * t * w; + dist += btScalar(2.) * s * t * r; + dist += btScalar(2.) * s * p; + dist += btScalar(2.) * t * q; + dist += u; + } + } + else + { + dist = btVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist) + { + dist = dist2; + if (witness) + btVec3Copy(witness, &witness2); + } + + dist2 = btVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist) + { + dist = dist2; + if (witness) + btVec3Copy(witness, &witness2); + } + } + + return dist; +} + +static int btDoSimplex2(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B; + btVector3 AB, AO, tmp; + btScalar dot; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other point + B = btSimplexPoint(simplex, 0); + // compute AB oriented segment + btVec3Sub2(&AB, &B->v, &A->v); + // compute AO vector + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + + // dot product AB . AO + dot = btVec3Dot(&AB, &AO); + + // check if origin doesn't lie on AB segment + btVec3Cross(&tmp, &AB, &AO); + if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0)) + { + return 1; + } + + // check if origin is in area where AB segment is + if (btFuzzyZero(dot) || dot < btScalar(0)) + { + // origin is in outside are of A + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + else + { + // origin is in area where AB segment is + + // keep simplex untouched and set direction to + // AB x AO x AB + btTripleCross(&AB, &AO, &AB, dir); + } + + return 0; +} + +static int btDoSimplex3(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B, *C; + btVector3 AO, AB, AC, ABC, tmp; + btScalar dot, dist; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other points + B = btSimplexPoint(simplex, 1); + C = btSimplexPoint(simplex, 0); + + // check touching contact + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0); + if (btFuzzyZero(dist)) + { + return 1; + } + + // check if triangle is really triangle (has area > 0) + // if not simplex can't be expanded and thus no itersection is found + if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v)) + { + return -1; + } + + // compute AO vector + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + + // compute AB and AC segments and ABC vector (perpendircular to triangle) + btVec3Sub2(&AB, &B->v, &A->v); + btVec3Sub2(&AC, &C->v, &A->v); + btVec3Cross(&ABC, &AB, &AC); + + btVec3Cross(&tmp, &ABC, &AC); + dot = btVec3Dot(&tmp, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + dot = btVec3Dot(&AC, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + // C is already in place + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AC, &AO, &AC, dir); + } + else + { + dot = btVec3Dot(&AB, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AB, &AO, &AB, dir); + } + else + { + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + } + } + else + { + btVec3Cross(&tmp, &AB, &ABC); + dot = btVec3Dot(&tmp, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + dot = btVec3Dot(&AB, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AB, &AO, &AB, dir); + } + else + { + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + } + else + { + dot = btVec3Dot(&ABC, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + btVec3Copy(dir, &ABC); + } + else + { + btSupportVector tmp; + btSupportCopy(&tmp, C); + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, &tmp); + + btVec3Copy(dir, &ABC); + btVec3Scale(dir, -btScalar(1)); + } + } + } + + return 0; +} + +static int btDoSimplex4(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B, *C, *D; + btVector3 AO, AB, AC, AD, ABC, ACD, ADB; + int B_on_ACD, C_on_ADB, D_on_ABC; + int AB_O, AC_O, AD_O; + btScalar dist; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other points + B = btSimplexPoint(simplex, 2); + C = btSimplexPoint(simplex, 1); + D = btSimplexPoint(simplex, 0); + + // check if tetrahedron is really tetrahedron (has volume > 0) + // if it is not simplex can't be expanded and thus no intersection is + // found + dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + { + return -1; + } + + // check if origin lies on some of tetrahedron's face - if so objects + // intersect + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + + // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + btVec3Sub2(&AB, &B->v, &A->v); + btVec3Sub2(&AC, &C->v, &A->v); + btVec3Sub2(&AD, &D->v, &A->v); + btVec3Cross(&ABC, &AB, &AC); + btVec3Cross(&ACD, &AC, &AD); + btVec3Cross(&ADB, &AD, &AB); + + // side (positive or negative) of B, C, D relative to planes ACD, ADB + // and ABC respectively + B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB)); + C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC)); + D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD)); + + // whether origin is on same side of ACD, ADB, ABC as B, C, D + // respectively + AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD; + AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB; + AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC; + + if (AB_O && AC_O && AD_O) + { + // origin is in tetrahedron + return 1; + // rearrange simplex to triangle and call btDoSimplex3() + } + else if (!AB_O) + { + // B is farthest from the origin among all of the tetrahedron's + // points, so remove it from the list and go on with the triangle + // case + + // D and C are in place + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + else if (!AC_O) + { + // C is farthest + btSimplexSet(simplex, 1, D); + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + else + { // (!AD_O) + btSimplexSet(simplex, 0, C); + btSimplexSet(simplex, 1, B); + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + + return btDoSimplex3(simplex, dir); +} + +static int btDoSimplex(btSimplex *simplex, btVector3 *dir) +{ + if (btSimplexSize(simplex) == 2) + { + // simplex contains segment only one segment + return btDoSimplex2(simplex, dir); + } + else if (btSimplexSize(simplex) == 3) + { + // simplex contains triangle + return btDoSimplex3(simplex, dir); + } + else + { // btSimplexSize(simplex) == 4 + // tetrahedron - this is the only shape which can encapsule origin + // so btDoSimplex4() also contains test on it + return btDoSimplex4(simplex, dir); + } } #ifdef __SPU__ -void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw) #else -void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw) +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.)); + btScalar distance = btScalar(0.); + btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.)); - btVector3 pointOnA,pointOnB; - btTransform localTransA = input.m_transformA; + btVector3 pointOnA, pointOnB; + btTransform localTransA = input.m_transformA; btTransform localTransB = input.m_transformB; - btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); + btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; @@ -100,7 +705,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu btScalar marginA = m_marginA; btScalar marginB = m_marginB; - gNumGjkChecks++; //for CCD we don't use margins if (m_ignoreMargin) @@ -110,8 +714,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu } m_curIter = 0; - int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? - m_cachedSeparatingAxis.setValue(0,1,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; @@ -119,191 +723,291 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu m_degenerateSimplex = 0; m_lastUsedMethod = -1; - + int status = -2; + btVector3 orgNormalInB(0, 0, 0); + btScalar margin = marginA + marginB; + + //we add a separate implementation to check if the convex shapes intersect + //See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang + //Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver + //and remove this temporary code from libCCD + //this fixes issue https://github.com/bulletphysics/bullet3/issues/1703 + //note, for large differences in shapes, use double precision build! { btScalar squaredDistance = BT_LARGE_FLOAT; btScalar delta = btScalar(0.); - - btScalar margin = marginA + marginB; - - - m_simplexSolver->reset(); - - for ( ; ; ) - //while (true) - { + btSimplex simplex1; + btSimplex *simplex = &simplex1; + btSimplexInit(simplex); - btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); + btVector3 dir(1, 0, 0); + { + btVector3 lastSupV; + btVector3 supAworld; + btVector3 supBworld; + btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV); - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + btSupportVector last; + last.v = lastSupV; + last.v1 = supAworld; + last.v2 = supBworld; - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); + btSimplexAdd(simplex, &last); + dir = -lastSupV; - if (check2d) + // start iterations + for (int iterations = 0; iterations < gGjkMaxIter; iterations++) { - pWorld[2] = 0.f; - qWorld[2] = 0.f; - } + // obtain support point + btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV); + + // check if farthest point in Minkowski difference in direction dir + // isn't somewhere before origin (the test on negative dot product) + // - because if it is, objects are not intersecting at all. + btScalar delta = lastSupV.dot(dir); + if (delta < 0) + { + //no intersection, besides margin + status = -1; + break; + } - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); + // add last support vector to simplex + last.v = lastSupV; + last.v1 = supAworld; + last.v2 = supBworld; - // 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; - } + btSimplexAdd(simplex, &last); - //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 btDoSimplex returns 1 if objects intersect, -1 if objects don't + // intersect and 0 if algorithm should continue - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) + btVector3 newDir; + int do_simplex_res = btDoSimplex(simplex, &dir); + + if (do_simplex_res == 1) + { + status = 0; // intersection found + break; + } + else if (do_simplex_res == -1) + { + // intersection not found + status = -1; + break; + } + + if (btFuzzyZero(btVec3Dot(&dir, &dir))) + { + // intersection not found + status = -1; + } + + if (dir.length2() < SIMD_EPSILON) { - m_degenerateSimplex = 2; - } else + //no intersection, besides margin + status = -1; + break; + } + + if (dir.fuzzyZero()) { - m_degenerateSimplex = 11; + // intersection not found + status = -1; + break; } - checkSimplex = true; - break; } + } - //add current vertex to simplex - m_simplexSolver->addVertex(w, pWorld, qWorld); - btVector3 newCachedSeparatingAxis; + m_simplexSolver->reset(); + if (status == 0) + { + //status = 0; + //printf("Intersect!\n"); + } - //calculate the closest point to the origin (update vector v) - if (!m_simplexSolver->closest(newCachedSeparatingAxis)) + if (status == -1) + { + //printf("not intersect\n"); + } + //printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]); + if (1) + { + for (;;) + //while (true) { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } + btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis(); + btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis(); - if(newCachedSeparatingAxis.length2()<REL_ERROR2) - { - m_cachedSeparatingAxis = newCachedSeparatingAxis; - m_degenerateSimplex = 6; - checkSimplex = true; - break; - } + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + + 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; + } + + //add current vertex to simplex + m_simplexSolver->addVertex(w, pWorld, qWorld); + 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(); + 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 // - - - //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; - } + ///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; - //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (m_curIter++ > gGjkMaxIter) - { - #if defined(DEBUG) || defined (_DEBUG) + break; + } - 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()); + m_cachedSeparatingAxis = newCachedSeparatingAxis; - #endif - break; + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject + if (m_curIter++ > gGjkMaxIter) + { +#if defined(DEBUG) || defined(_DEBUG) - } + 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()); + 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 (!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 = m_cachedSeparatingAxis; - btScalar lenSqr =m_cachedSeparatingAxis.length2(); - - //valid normal - if (lenSqr < 0.0001) + if (checkSimplex) { - 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; + m_simplexSolver->compute_points(pointOnA, pointOnB); + normalInB = m_cachedSeparatingAxis; + + btScalar lenSqr = m_cachedSeparatingAxis.length2(); + + //valid normal + if (lenSqr < REL_ERROR2) + { + 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; + orgNormalInB = normalInB; + + m_lastUsedMethod = 1; + } + else + { + m_lastUsedMethod = 2; + } } } - bool catchDegeneratePenetrationCase = - (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); + bool catchDegeneratePenetrationCase = + (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance + margin) < gGjkEpaPenetrationTolerance)); //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) + if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0)) { //penetration case @@ -311,149 +1015,169 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu if (m_penetrationDepthSolver) { // Penetration depth case. - btVector3 tmpPointOnA,tmpPointOnB; - - gNumDeepPenetrationChecks++; + btVector3 tmpPointOnA, tmpPointOnB; + m_cachedSeparatingAxis.setZero(); - bool isValid2 = m_penetrationDepthSolver->calcPenDepth( - *m_simplexSolver, - m_minkowskiA,m_minkowskiB, - localTransA,localTransB, + bool isValid2 = m_penetrationDepthSolver->calcPenDepth( + *m_simplexSolver, + m_minkowskiA, m_minkowskiB, + localTransA, localTransB, m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, - debugDraw - ); + debugDraw); - - if (isValid2) + if (m_cachedSeparatingAxis.length2()) { - btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - btScalar lenSqr = tmpNormalInB.length2(); - if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) + if (isValid2) { - tmpNormalInB = m_cachedSeparatingAxis; - lenSqr = m_cachedSeparatingAxis.length2(); - } + 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(); - m_lastUsedMethod = 3; - //only replace valid penetrations when the result is deeper (check) - if (!isValid || (distance2 < distance)) + if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON)) { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - normalInB = tmpNormalInB; - ///todo: need to track down this EPA penetration solver degeneracy - ///the penetration solver reports penetration but the contact normal - ///connecting the contact points is pointing in the opposite direction - ///until then, detect the issue and revert the normal + tmpNormalInB /= btSqrt(lenSqr); + btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length(); + m_lastUsedMethod = 3; + //only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) { - btScalar d1=0; - { - btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis(); - - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - btVector3 w = pWorld - qWorld; - d1 = (-normalInB).dot(w); - } - btScalar d0 = 0.f; - { - btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis(); - - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - btVector3 w = pWorld - qWorld; - d0 = normalInB.dot(w); - } - if (d1>d0) - { - m_lastUsedMethod = 10; - normalInB*=-1; - } - + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + isValid = true; + } + else + { + m_lastUsedMethod = 8; } - isValid = true; - - } else + } + else { - m_lastUsedMethod = 8; + m_lastUsedMethod = 9; } - } else - { - m_lastUsedMethod = 9; } - } else + 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 + ///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.)) { - m_lastUsedMethod = 5; + 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; + } } } } - + else + { + //printf("EPA didn't return a valid value\n"); + } } - } } - - - if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) + if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared))) { - m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; + if (1) + { + ///todo: need to track down this EPA penetration solver degeneracy + ///the penetration solver reports penetration but the contact normal + ///connecting the contact points is pointing in the opposite direction + ///until then, detect the issue and revert the normal - output.addContactPoint( - normalInB, - pointOnB+positionOffset, - distance); + btScalar d2 = 0.f; + { + btVector3 separatingAxisInA = (-orgNormalInB) * localTransA.getBasis(); + btVector3 separatingAxisInB = orgNormalInB * localTransB.getBasis(); - } + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d2 = orgNormalInB.dot(w) - margin; + } -} + btScalar d1 = 0; + { + btVector3 separatingAxisInA = (normalInB)*localTransA.getBasis(); + btVector3 separatingAxisInB = -normalInB * localTransB.getBasis(); + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d1 = (-normalInB).dot(w) - margin; + } + btScalar d0 = 0.f; + { + btVector3 separatingAxisInA = (-normalInB) * input.m_transformA.getBasis(); + btVector3 separatingAxisInB = normalInB * input.m_transformB.getBasis(); + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d0 = normalInB.dot(w) - margin; + } + if (d1 > d0) + { + m_lastUsedMethod = 10; + normalInB *= -1; + } + + if (orgNormalInB.length2()) + { + if (d2 > d0 && d2 > d1 && d2 > distance) + { + normalInB = orgNormalInB; + distance = d2; + } + } + } + + output.addContactPoint( + normalInB, + pointOnB + positionOffset, + distance); + } + else + { + //printf("invalid gjk query\n"); + } +} diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index feeae686213..faa02287cae 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -13,9 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - - #ifndef BT_GJK_PAIR_DETECTOR_H #define BT_GJK_PAIR_DETECTOR_H @@ -29,39 +26,34 @@ class btConvexPenetrationDepthSolver; /// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface class btGjkPairDetector : public btDiscreteCollisionDetectorInterface { - - - btVector3 m_cachedSeparatingAxis; - btConvexPenetrationDepthSolver* m_penetrationDepthSolver; + btVector3 m_cachedSeparatingAxis; + btConvexPenetrationDepthSolver* m_penetrationDepthSolver; btSimplexSolverInterface* m_simplexSolver; const btConvexShape* m_minkowskiA; const btConvexShape* m_minkowskiB; - int m_shapeTypeA; + int m_shapeTypeA; int m_shapeTypeB; - btScalar m_marginA; - btScalar m_marginB; + btScalar m_marginA; + btScalar m_marginB; - bool m_ignoreMargin; - btScalar m_cachedSeparatingDistance; - + bool m_ignoreMargin; + btScalar m_cachedSeparatingDistance; public: - //some debugging to fix degeneracy problems - int m_lastUsedMethod; - int m_curIter; - int m_degenerateSimplex; - int m_catchDegeneracies; - int m_fixContactNormalDirection; + int m_lastUsedMethod; + int m_curIter; + int m_degenerateSimplex; + int m_catchDegeneracies; + int m_fixContactNormalDirection; - btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); - btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); - virtual ~btGjkPairDetector() {}; + btGjkPairDetector(const btConvexShape* objectA, const btConvexShape* objectB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver); + btGjkPairDetector(const btConvexShape* objectA, const btConvexShape* objectB, int shapeTypeA, int shapeTypeB, btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver); + virtual ~btGjkPairDetector(){}; - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false); - void getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); - + void getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw); void setMinkowskiA(const btConvexShape* minkA) { @@ -72,32 +64,30 @@ public: { m_minkowskiB = minkB; } - void setCachedSeperatingAxis(const btVector3& seperatingAxis) + void setCachedSeparatingAxis(const btVector3& separatingAxis) { - m_cachedSeparatingAxis = seperatingAxis; + m_cachedSeparatingAxis = separatingAxis; } const btVector3& getCachedSeparatingAxis() const { return m_cachedSeparatingAxis; } - btScalar getCachedSeparatingDistance() const + btScalar getCachedSeparatingDistance() const { return m_cachedSeparatingDistance; } - void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) + void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) { m_penetrationDepthSolver = penetrationDepthSolver; } ///don't use setIgnoreMargin, it's for Bullet's internal use - void setIgnoreMargin(bool ignoreMargin) + void setIgnoreMargin(bool ignoreMargin) { m_ignoreMargin = ignoreMargin; } - - }; -#endif //BT_GJK_PAIR_DETECTOR_H +#endif //BT_GJK_PAIR_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index e40fb1d3db6..573fc86bf97 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -20,137 +20,155 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" #ifdef PFX_USE_FREE_VECTORMATH - #include "physics_effects/base_level/solver/pfx_constraint_row.h" +#include "physics_effects/base_level/solver/pfx_constraint_row.h" typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow; #else - // Don't change following order of parameters - ATTRIBUTE_ALIGNED16(struct) btConstraintRow { - btScalar m_normal[3]; - btScalar m_rhs; - btScalar m_jacDiagInv; - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_accumImpulse; - }; - typedef btConstraintRow PfxConstraintRow; -#endif //PFX_USE_FREE_VECTORMATH - - +// Don't change following order of parameters +ATTRIBUTE_ALIGNED16(struct) +btConstraintRow +{ + btScalar m_normal[3]; + btScalar m_rhs; + btScalar m_jacDiagInv; + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_accumImpulse; +}; +typedef btConstraintRow PfxConstraintRow; +#endif //PFX_USE_FREE_VECTORMATH + +enum btContactPointFlags +{ + BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED = 1, + BT_CONTACT_FLAG_HAS_CONTACT_CFM = 2, + BT_CONTACT_FLAG_HAS_CONTACT_ERP = 4, + BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8, + BT_CONTACT_FLAG_FRICTION_ANCHOR = 16, +}; /// ManifoldContactPoint collects and maintains persistent contactpoints. /// used to improve stability and performance of rigidbody dynamics response. class btManifoldPoint +{ +public: + btManifoldPoint() + : m_userPersistentData(0), + m_contactPointFlags(0), + m_appliedImpulse(0.f), + m_prevRHS(0.f), + m_appliedImpulseLateral1(0.f), + m_appliedImpulseLateral2(0.f), + m_contactMotion1(0.f), + m_contactMotion2(0.f), + m_contactCFM(0.f), + m_contactERP(0.f), + m_frictionCFM(0.f), + m_lifeTime(0) { - public: - btManifoldPoint() - :m_userPersistentData(0), - m_lateralFrictionInitialized(false), - m_appliedImpulse(0.f), - m_appliedImpulseLateral1(0.f), - m_appliedImpulseLateral2(0.f), - m_contactMotion1(0.f), - m_contactMotion2(0.f), - m_contactCFM1(0.f), - m_contactCFM2(0.f), - m_lifeTime(0) - { - } - - btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB, - const btVector3 &normal, - btScalar distance ) : - m_localPointA( pointA ), - m_localPointB( pointB ), - m_normalWorldOnB( normal ), - m_distance1( distance ), - m_combinedFriction(btScalar(0.)), - m_combinedRollingFriction(btScalar(0.)), - m_combinedRestitution(btScalar(0.)), - m_userPersistentData(0), - m_lateralFrictionInitialized(false), - m_appliedImpulse(0.f), - m_appliedImpulseLateral1(0.f), - m_appliedImpulseLateral2(0.f), - m_contactMotion1(0.f), - m_contactMotion2(0.f), - m_contactCFM1(0.f), - m_contactCFM2(0.f), - m_lifeTime(0) - { - - } - - - - btVector3 m_localPointA; - btVector3 m_localPointB; - btVector3 m_positionWorldOnB; - ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity - btVector3 m_positionWorldOnA; - btVector3 m_normalWorldOnB; - - btScalar m_distance1; - btScalar m_combinedFriction; - btScalar m_combinedRollingFriction; - btScalar m_combinedRestitution; - - //BP mod, store contact triangles. - int m_partId0; - int m_partId1; - int m_index0; - int m_index1; - - mutable void* m_userPersistentData; - bool m_lateralFrictionInitialized; - - btScalar m_appliedImpulse; - btScalar m_appliedImpulseLateral1; - btScalar m_appliedImpulseLateral2; - btScalar m_contactMotion1; - btScalar m_contactMotion2; - btScalar m_contactCFM1; - btScalar m_contactCFM2; - - int m_lifeTime;//lifetime of the contactpoint in frames - - btVector3 m_lateralFrictionDir1; - btVector3 m_lateralFrictionDir2; - - - - - btScalar getDistance() const - { - return m_distance1; - } - int getLifeTime() const - { - return m_lifeTime; - } - - const btVector3& getPositionWorldOnA() const { - return m_positionWorldOnA; -// return m_positionWorldOnB + m_normalWorldOnB * m_distance1; - } - - const btVector3& getPositionWorldOnB() const - { - return m_positionWorldOnB; - } - - void setDistance(btScalar dist) - { - m_distance1 = dist; - } - - ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver - btScalar getAppliedImpulse() const - { - return m_appliedImpulse; - } - - + } + + btManifoldPoint(const btVector3& pointA, const btVector3& pointB, + const btVector3& normal, + btScalar distance) : m_localPointA(pointA), + m_localPointB(pointB), + m_normalWorldOnB(normal), + m_distance1(distance), + m_combinedFriction(btScalar(0.)), + m_combinedRollingFriction(btScalar(0.)), + m_combinedSpinningFriction(btScalar(0.)), + m_combinedRestitution(btScalar(0.)), + m_userPersistentData(0), + m_contactPointFlags(0), + m_appliedImpulse(0.f), + m_prevRHS(0.f), + m_appliedImpulseLateral1(0.f), + m_appliedImpulseLateral2(0.f), + m_contactMotion1(0.f), + m_contactMotion2(0.f), + m_contactCFM(0.f), + m_contactERP(0.f), + m_frictionCFM(0.f), + m_lifeTime(0) + { + } + + btVector3 m_localPointA; + btVector3 m_localPointB; + btVector3 m_positionWorldOnB; + ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity + btVector3 m_positionWorldOnA; + btVector3 m_normalWorldOnB; + + btScalar m_distance1; + btScalar m_combinedFriction; + btScalar m_combinedRollingFriction; //torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever + btScalar m_combinedSpinningFriction; //torsional friction around contact normal, useful for grasping objects + btScalar m_combinedRestitution; + + //BP mod, store contact triangles. + int m_partId0; + int m_partId1; + int m_index0; + int m_index1; + + mutable void* m_userPersistentData; + //bool m_lateralFrictionInitialized; + int m_contactPointFlags; + + btScalar m_appliedImpulse; + btScalar m_prevRHS; + btScalar m_appliedImpulseLateral1; + btScalar m_appliedImpulseLateral2; + btScalar m_contactMotion1; + btScalar m_contactMotion2; + + union { + btScalar m_contactCFM; + btScalar m_combinedContactStiffness1; + }; + union { + btScalar m_contactERP; + btScalar m_combinedContactDamping1; }; -#endif //BT_MANIFOLD_CONTACT_POINT_H + btScalar m_frictionCFM; + + int m_lifeTime; //lifetime of the contactpoint in frames + + btVector3 m_lateralFrictionDir1; + btVector3 m_lateralFrictionDir2; + + btScalar getDistance() const + { + return m_distance1; + } + int getLifeTime() const + { + return m_lifeTime; + } + + const btVector3& getPositionWorldOnA() const + { + return m_positionWorldOnA; + // return m_positionWorldOnB + m_normalWorldOnB * m_distance1; + } + + const btVector3& getPositionWorldOnB() const + { + return m_positionWorldOnB; + } + + void setDistance(btScalar dist) + { + m_distance1 = dist; + } + + ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver + btScalar getAppliedImpulse() const + { + return m_appliedImpulse; + } +}; + +#endif //BT_MANIFOLD_CONTACT_POINT_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index fa45f49037e..c042c242080 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -21,42 +21,38 @@ subject to the following restrictions: #define NUM_UNITSPHERE_POINTS 42 - bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw - ) + const btConvexShape* convexA, const btConvexShape* convexB, + const btTransform& transA, const btTransform& transB, + btVector3& v, btVector3& pa, btVector3& pb, + class btIDebugDraw* debugDraw) { - (void)v; - - bool check2d= convexA->isConvex2d() && convexB->isConvex2d(); + + bool check2d = convexA->isConvex2d() && convexB->isConvex2d(); struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result { - - btIntermediateResult():m_hasResult(false) + btIntermediateResult() : m_hasResult(false) { } - + btVector3 m_normalOnBInWorld; btVector3 m_pointInWorld; btScalar m_depth; - bool m_hasResult; + bool m_hasResult; - virtual void setShapeIdentifiersA(int partId0,int index0) + virtual void setShapeIdentifiersA(int partId0, int index0) { (void)partId0; (void)index0; } - virtual void setShapeIdentifiersB(int partId1,int index1) + virtual void setShapeIdentifiersB(int partId1, int index1) { (void)partId1; (void)index1; } - void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { m_normalOnBInWorld = normalOnBInWorld; m_pointInWorld = pointInWorld; @@ -68,42 +64,42 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s //just take fixed number of orientation, and sample the penetration depth in that direction btScalar minProj = btScalar(BT_LARGE_FLOAT); btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.)); - btVector3 minA,minB; - btVector3 seperatingAxisInA,seperatingAxisInB; - btVector3 pInA,qInB,pWorld,qWorld,w; + btVector3 minA, minB; + btVector3 separatingAxisInA, separatingAxisInB; + btVector3 pInA, qInB, pWorld, qWorld, w; #ifndef __SPU__ #define USE_BATCHED_SUPPORT 1 #endif #ifdef USE_BATCHED_SUPPORT - btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; + btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; + btVector3 separatingAxisInABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; + btVector3 separatingAxisInBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int i; int numSampleDirections = NUM_UNITSPHERE_POINTS; - for (i=0;i<numSampleDirections;i++) + for (i = 0; i < numSampleDirections; i++) { btVector3 norm = getPenetrationDirections()[i]; - seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ; - seperatingAxisInBBatch[i] = norm * transB.getBasis() ; + separatingAxisInABatch[i] = (-norm) * transA.getBasis(); + separatingAxisInBBatch[i] = norm * transB.getBasis(); } { int numPDA = convexA->getNumPreferredPenetrationDirections(); if (numPDA) { - for (int i=0;i<numPDA;i++) + for (int i = 0; i < numPDA; i++) { btVector3 norm; - convexA->getPreferredPenetrationDirection(i,norm); - norm = transA.getBasis() * norm; + convexA->getPreferredPenetrationDirection(i, norm); + norm = transA.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; - seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); - seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); + separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); + separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; } } @@ -113,42 +109,38 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numPDB = convexB->getNumPreferredPenetrationDirections(); if (numPDB) { - for (int i=0;i<numPDB;i++) + for (int i = 0; i < numPDB; i++) { btVector3 norm; - convexB->getPreferredPenetrationDirection(i,norm); - norm = transB.getBasis() * norm; + convexB->getPreferredPenetrationDirection(i, norm); + norm = transB.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; - seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); - seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); + separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); + separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; } } } + convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInABatch, supportVerticesABatch, numSampleDirections); + convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInBBatch, supportVerticesBBatch, numSampleDirections); - - - convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); - convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); - - for (i=0;i<numSampleDirections;i++) + for (i = 0; i < numSampleDirections; i++) { btVector3 norm = getPenetrationDirections()[i]; if (check2d) { norm[2] = 0.f; } - if (norm.length2()>0.01) + if (norm.length2() > 0.01) { - - seperatingAxisInA = seperatingAxisInABatch[i]; - seperatingAxisInB = seperatingAxisInBBatch[i]; + separatingAxisInA = separatingAxisInABatch[i]; + separatingAxisInB = separatingAxisInBBatch[i]; pInA = supportVerticesABatch[i]; qInB = supportVerticesBBatch[i]; - pWorld = transA(pInA); + pWorld = transA(pInA); qWorld = transB(qInB); if (check2d) { @@ -156,7 +148,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s qWorld[2] = 0.f; } - w = qWorld - pWorld; + w = qWorld - pWorld; btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) @@ -167,7 +159,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minB = qWorld; } } - } + } #else int numSampleDirections = NUM_UNITSPHERE_POINTS; @@ -177,11 +169,11 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numPDA = convexA->getNumPreferredPenetrationDirections(); if (numPDA) { - for (int i=0;i<numPDA;i++) + for (int i = 0; i < numPDA; i++) { btVector3 norm; - convexA->getPreferredPenetrationDirection(i,norm); - norm = transA.getBasis() * norm; + convexA->getPreferredPenetrationDirection(i, norm); + norm = transA.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } @@ -192,28 +184,28 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s int numPDB = convexB->getNumPreferredPenetrationDirections(); if (numPDB) { - for (int i=0;i<numPDB;i++) + for (int i = 0; i < numPDB; i++) { btVector3 norm; - convexB->getPreferredPenetrationDirection(i,norm); - norm = transB.getBasis() * norm; + convexB->getPreferredPenetrationDirection(i, norm); + norm = transB.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } } -#endif // __SPU__ +#endif // __SPU__ - for (int i=0;i<numSampleDirections;i++) + for (int i = 0; i < numSampleDirections; i++) { const btVector3& norm = getPenetrationDirections()[i]; - seperatingAxisInA = (-norm)* transA.getBasis(); - seperatingAxisInB = norm* transB.getBasis(); - pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - pWorld = transA(pInA); + separatingAxisInA = (-norm) * transA.getBasis(); + separatingAxisInB = norm * transB.getBasis(); + pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); + qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); + pWorld = transA(pInA); qWorld = transB(qInB); - w = qWorld - pWorld; + w = qWorld - pWorld; btScalar delta = norm.dot(w); //find smallest delta if (delta < minProj) @@ -224,48 +216,39 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s minB = qWorld; } } -#endif //USE_BATCHED_SUPPORT +#endif //USE_BATCHED_SUPPORT //add the margins - minA += minNorm*convexA->getMarginNonVirtual(); - minB -= minNorm*convexB->getMarginNonVirtual(); + minA += minNorm * convexA->getMarginNonVirtual(); + minB -= minNorm * convexB->getMarginNonVirtual(); //no penetration if (minProj < btScalar(0.)) return false; - btScalar extraSeparation = 0.5f;///scale dependent - minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); - - - - + btScalar extraSeparation = 0.5f; ///scale dependent + minProj += extraSeparation + (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); //#define DEBUG_DRAW 1 #ifdef DEBUG_DRAW if (debugDraw) { - btVector3 color(0,1,0); - debugDraw->drawLine(minA,minB,color); - color = btVector3 (1,1,1); - btVector3 vec = minB-minA; + btVector3 color(0, 1, 0); + debugDraw->drawLine(minA, minB, color); + color = btVector3(1, 1, 1); + btVector3 vec = minB - minA; btScalar prj2 = minNorm.dot(vec); - debugDraw->drawLine(minA,minA+(minNorm*minProj),color); - + debugDraw->drawLine(minA, minA + (minNorm * minProj), color); } -#endif //DEBUG_DRAW +#endif //DEBUG_DRAW - - - btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); + btGjkPairDetector gjkdet(convexA, convexB, &simplexSolver, 0); btScalar offsetDist = minProj; btVector3 offset = minNorm * offsetDist; - - btGjkPairDetector::ClosestPointInput input; - + btVector3 newOrg = transA.getOrigin() + offset; btTransform displacedTrans = transA; @@ -273,89 +256,81 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_transformA = displacedTrans; input.m_transformB = transB; - input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; - + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); //minProj; + btIntermediateResult res; - gjkdet.setCachedSeperatingAxis(-minNorm); - gjkdet.getClosestPoints(input,res,debugDraw); + gjkdet.setCachedSeparatingAxis(-minNorm); + gjkdet.getClosestPoints(input, res, debugDraw); btScalar correctedMinNorm = minProj - res.m_depth; - //the penetration depth is over-estimated, relax it - btScalar penetration_relaxation= btScalar(1.); - minNorm*=penetration_relaxation; - + btScalar penetration_relaxation = btScalar(1.); + minNorm *= penetration_relaxation; if (res.m_hasResult) { - pa = res.m_pointInWorld - minNorm * correctedMinNorm; pb = res.m_pointInWorld; v = minNorm; - + #ifdef DEBUG_DRAW if (debugDraw) { - btVector3 color(1,0,0); - debugDraw->drawLine(pa,pb,color); + btVector3 color(1, 0, 0); + debugDraw->drawLine(pa, pb, color); } -#endif//DEBUG_DRAW - - +#endif //DEBUG_DRAW } return res.m_hasResult; } -btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections() +btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections() { - static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = - { - btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), - btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), - btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), - btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), - btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), - btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), - btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), - btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), - btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), - btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), - btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), - btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), - btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), - btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), - btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), - btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), - btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), - btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), - btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), - btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), - btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), - btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), - btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), - btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), - btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), - btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), - btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), - btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), - btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), - btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) - }; + static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] = + { + btVector3(btScalar(0.000000), btScalar(-0.000000), btScalar(-1.000000)), + btVector3(btScalar(0.723608), btScalar(-0.525725), btScalar(-0.447219)), + btVector3(btScalar(-0.276388), btScalar(-0.850649), btScalar(-0.447219)), + btVector3(btScalar(-0.894426), btScalar(-0.000000), btScalar(-0.447216)), + btVector3(btScalar(-0.276388), btScalar(0.850649), btScalar(-0.447220)), + btVector3(btScalar(0.723608), btScalar(0.525725), btScalar(-0.447219)), + btVector3(btScalar(0.276388), btScalar(-0.850649), btScalar(0.447220)), + btVector3(btScalar(-0.723608), btScalar(-0.525725), btScalar(0.447219)), + btVector3(btScalar(-0.723608), btScalar(0.525725), btScalar(0.447219)), + btVector3(btScalar(0.276388), btScalar(0.850649), btScalar(0.447219)), + btVector3(btScalar(0.894426), btScalar(0.000000), btScalar(0.447216)), + btVector3(btScalar(-0.000000), btScalar(0.000000), btScalar(1.000000)), + btVector3(btScalar(0.425323), btScalar(-0.309011), btScalar(-0.850654)), + btVector3(btScalar(-0.162456), btScalar(-0.499995), btScalar(-0.850654)), + btVector3(btScalar(0.262869), btScalar(-0.809012), btScalar(-0.525738)), + btVector3(btScalar(0.425323), btScalar(0.309011), btScalar(-0.850654)), + btVector3(btScalar(0.850648), btScalar(-0.000000), btScalar(-0.525736)), + btVector3(btScalar(-0.525730), btScalar(-0.000000), btScalar(-0.850652)), + btVector3(btScalar(-0.688190), btScalar(-0.499997), btScalar(-0.525736)), + btVector3(btScalar(-0.162456), btScalar(0.499995), btScalar(-0.850654)), + btVector3(btScalar(-0.688190), btScalar(0.499997), btScalar(-0.525736)), + btVector3(btScalar(0.262869), btScalar(0.809012), btScalar(-0.525738)), + btVector3(btScalar(0.951058), btScalar(0.309013), btScalar(0.000000)), + btVector3(btScalar(0.951058), btScalar(-0.309013), btScalar(0.000000)), + btVector3(btScalar(0.587786), btScalar(-0.809017), btScalar(0.000000)), + btVector3(btScalar(0.000000), btScalar(-1.000000), btScalar(0.000000)), + btVector3(btScalar(-0.587786), btScalar(-0.809017), btScalar(0.000000)), + btVector3(btScalar(-0.951058), btScalar(-0.309013), btScalar(-0.000000)), + btVector3(btScalar(-0.951058), btScalar(0.309013), btScalar(-0.000000)), + btVector3(btScalar(-0.587786), btScalar(0.809017), btScalar(-0.000000)), + btVector3(btScalar(-0.000000), btScalar(1.000000), btScalar(-0.000000)), + btVector3(btScalar(0.587786), btScalar(0.809017), btScalar(-0.000000)), + btVector3(btScalar(0.688190), btScalar(-0.499997), btScalar(0.525736)), + btVector3(btScalar(-0.262869), btScalar(-0.809012), btScalar(0.525738)), + btVector3(btScalar(-0.850648), btScalar(0.000000), btScalar(0.525736)), + btVector3(btScalar(-0.262869), btScalar(0.809012), btScalar(0.525738)), + btVector3(btScalar(0.688190), btScalar(0.499997), btScalar(0.525736)), + btVector3(btScalar(0.525730), btScalar(0.000000), btScalar(0.850652)), + btVector3(btScalar(0.162456), btScalar(-0.499995), btScalar(0.850654)), + btVector3(btScalar(-0.425323), btScalar(-0.309011), btScalar(0.850654)), + btVector3(btScalar(-0.425323), btScalar(0.309011), btScalar(0.850654)), + btVector3(btScalar(0.162456), btScalar(0.499995), btScalar(0.850654))}; return sPenetrationDirections; } - - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h index fd533b4fc33..8e3e3932591 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -23,18 +23,14 @@ subject to the following restrictions: class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver { protected: - - static btVector3* getPenetrationDirections(); + static btVector3* getPenetrationDirections(); public: - - virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* convexA,const btConvexShape* convexB, - const btTransform& transA,const btTransform& transB, - btVector3& v, btVector3& pa, btVector3& pb, - class btIDebugDraw* debugDraw - ); + virtual bool calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* convexA, const btConvexShape* convexB, + const btTransform& transA, const btTransform& transB, + btVector3& v, btVector3& pa, btVector3& pb, + class btIDebugDraw* debugDraw); }; -#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H - +#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h index a22a0bae66b..534a66d3fa3 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h @@ -28,32 +28,31 @@ //#define MPR_AVERAGE_CONTACT_POSITIONS - struct btMprCollisionDescription { - btVector3 m_firstDir; - int m_maxGjkIterations; - btScalar m_maximumDistanceSquared; - btScalar m_gjkRelError2; - - btMprCollisionDescription() - : m_firstDir(0,1,0), - m_maxGjkIterations(1000), - m_maximumDistanceSquared(1e30f), - m_gjkRelError2(1.0e-6) - { - } - virtual ~btMprCollisionDescription() - { - } + btVector3 m_firstDir; + int m_maxGjkIterations; + btScalar m_maximumDistanceSquared; + btScalar m_gjkRelError2; + + btMprCollisionDescription() + : m_firstDir(0, 1, 0), + m_maxGjkIterations(1000), + m_maximumDistanceSquared(1e30f), + m_gjkRelError2(1.0e-6) + { + } + virtual ~btMprCollisionDescription() + { + } }; struct btMprDistanceInfo { - btVector3 m_pointOnA; - btVector3 m_pointOnB; - btVector3 m_normalBtoA; - btScalar m_distance; + btVector3 m_pointOnA; + btVector3 m_pointOnB; + btVector3 m_normalBtoA; + btScalar m_distance; }; #ifdef __cplusplus @@ -67,131 +66,112 @@ struct btMprDistanceInfo #define BT_MPR_TOLERANCE 1E-6f #define BT_MPR_MAX_ITERATIONS 1000 -struct _btMprSupport_t +struct _btMprSupport_t { - btVector3 v; //!< Support point in minkowski sum - btVector3 v1; //!< Support point in obj1 - btVector3 v2; //!< Support point in obj2 + btVector3 v; //!< Support point in minkowski sum + btVector3 v1; //!< Support point in obj1 + btVector3 v2; //!< Support point in obj2 }; typedef struct _btMprSupport_t btMprSupport_t; -struct _btMprSimplex_t +struct _btMprSimplex_t { - btMprSupport_t ps[4]; - int last; //!< index of last added point + btMprSupport_t ps[4]; + int last; //!< index of last added point }; typedef struct _btMprSimplex_t btMprSimplex_t; -inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx) +inline btMprSupport_t *btMprSimplexPointW(btMprSimplex_t *s, int idx) { - return &s->ps[idx]; + return &s->ps[idx]; } inline void btMprSimplexSetSize(btMprSimplex_t *s, int size) { - s->last = size - 1; + s->last = size - 1; } #ifdef DEBUG_MPR -inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index) +inline void btPrintPortalVertex(_btMprSimplex_t *portal, int index) { - printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(),portal->ps[index].v.y(),portal->ps[index].v.z(), - portal->ps[index].v1.x(),portal->ps[index].v1.y(),portal->ps[index].v1.z(), - portal->ps[index].v2.x(),portal->ps[index].v2.y(),portal->ps[index].v2.z()); + printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(), portal->ps[index].v.y(), portal->ps[index].v.z(), + portal->ps[index].v1.x(), portal->ps[index].v1.y(), portal->ps[index].v1.z(), + portal->ps[index].v2.x(), portal->ps[index].v2.y(), portal->ps[index].v2.z()); } -#endif //DEBUG_MPR - - - +#endif //DEBUG_MPR inline int btMprSimplexSize(const btMprSimplex_t *s) { - return s->last + 1; + return s->last + 1; } - -inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx) +inline const btMprSupport_t *btMprSimplexPoint(const btMprSimplex_t *s, int idx) { - // here is no check on boundaries - return &s->ps[idx]; + // here is no check on boundaries + return &s->ps[idx]; } inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s) { - *d = *s; + *d = *s; } inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a) { - btMprSupportCopy(s->ps + pos, a); + btMprSupportCopy(s->ps + pos, a); } - inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2) { - btMprSupport_t supp; + btMprSupport_t supp; - btMprSupportCopy(&supp, &s->ps[pos1]); - btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]); - btMprSupportCopy(&s->ps[pos2], &supp); + btMprSupportCopy(&supp, &s->ps[pos1]); + btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]); + btMprSupportCopy(&s->ps[pos2], &supp); } - inline int btMprIsZero(float val) { - return BT_MPR_FABS(val) < FLT_EPSILON; + return BT_MPR_FABS(val) < FLT_EPSILON; } - - inline int btMprEq(float _a, float _b) { - float ab; - float a, b; + float ab; + float a, b; - ab = BT_MPR_FABS(_a - _b); - if (BT_MPR_FABS(ab) < FLT_EPSILON) - return 1; + ab = BT_MPR_FABS(_a - _b); + if (BT_MPR_FABS(ab) < FLT_EPSILON) + return 1; - a = BT_MPR_FABS(_a); - b = BT_MPR_FABS(_b); - if (b > a){ - return ab < FLT_EPSILON * b; - }else{ - return ab < FLT_EPSILON * a; - } + a = BT_MPR_FABS(_a); + b = BT_MPR_FABS(_b); + if (b > a) + { + return ab < FLT_EPSILON * b; + } + else + { + return ab < FLT_EPSILON * a; + } } - -inline int btMprVec3Eq(const btVector3* a, const btVector3 *b) +inline int btMprVec3Eq(const btVector3 *a, const btVector3 *b) { - return btMprEq((*a).x(), (*b).x()) - && btMprEq((*a).y(), (*b).y()) - && btMprEq((*a).z(), (*b).z()); + return btMprEq((*a).x(), (*b).x()) && btMprEq((*a).y(), (*b).y()) && btMprEq((*a).z(), (*b).z()); } - - - - - - - - - - template <typename btConvexTemplate> -inline void btFindOrigin(const btConvexTemplate& a, const btConvexTemplate& b, const btMprCollisionDescription& colDesc,btMprSupport_t *center) +inline void btFindOrigin(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSupport_t *center) { - center->v1 = a.getObjectCenterInWorld(); - center->v2 = b.getObjectCenterInWorld(); - center->v = center->v1 - center->v2; + center->v2 = b.getObjectCenterInWorld(); + center->v = center->v1 - center->v2; } inline void btMprVec3Set(btVector3 *v, float x, float y, float z) { - v->setValue(x,y,z); + v->setValue(x, y, z); } inline void btMprVec3Add(btVector3 *v, const btVector3 *w) @@ -201,41 +181,38 @@ inline void btMprVec3Add(btVector3 *v, const btVector3 *w) inline void btMprVec3Copy(btVector3 *v, const btVector3 *w) { - *v = *w; + *v = *w; } inline void btMprVec3Scale(btVector3 *d, float k) { - *d *= k; + *d *= k; } inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b) { - float dot; + float dot; - dot = btDot(*a,*b); - return dot; + dot = btDot(*a, *b); + return dot; } - inline float btMprVec3Len2(const btVector3 *v) { - return btMprVec3Dot(v, v); + return btMprVec3Dot(v, v); } inline void btMprVec3Normalize(btVector3 *d) { - float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d)); - btMprVec3Scale(d, k); + float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d)); + btMprVec3Scale(d, k); } inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b) { - *d = btCross(*a,*b); - + *d = btCross(*a, *b); } - inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) { *d = *v - *w; @@ -243,666 +220,665 @@ inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir) { - btVector3 v2v1, v3v1; + btVector3 v2v1, v3v1; - btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 1)->v); - btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 1)->v); - btMprVec3Cross(dir, &v2v1, &v3v1); - btMprVec3Normalize(dir); + btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 1)->v); + btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 1)->v); + btMprVec3Cross(dir, &v2v1, &v3v1); + btMprVec3Normalize(dir); } - inline int portalEncapsulesOrigin(const btMprSimplex_t *portal, - const btVector3 *dir) + const btVector3 *dir) { - float dot; - dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v); - return btMprIsZero(dot) || dot > 0.f; + float dot; + dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v); + return btMprIsZero(dot) || dot > 0.f; } inline int portalReachTolerance(const btMprSimplex_t *portal, - const btMprSupport_t *v4, - const btVector3 *dir) + const btMprSupport_t *v4, + const btVector3 *dir) { - float dv1, dv2, dv3, dv4; - float dot1, dot2, dot3; + float dv1, dv2, dv3, dv4; + float dot1, dot2, dot3; - // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} + // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} - dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir); - dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir); - dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir); - dv4 = btMprVec3Dot(&v4->v, dir); + dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir); + dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir); + dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir); + dv4 = btMprVec3Dot(&v4->v, dir); - dot1 = dv4 - dv1; - dot2 = dv4 - dv2; - dot3 = dv4 - dv3; + dot1 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; - dot1 = BT_MPR_FMIN(dot1, dot2); - dot1 = BT_MPR_FMIN(dot1, dot3); + dot1 = BT_MPR_FMIN(dot1, dot2); + dot1 = BT_MPR_FMIN(dot1, dot3); - return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE; + return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE; } inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, - const btMprSupport_t *v4, - const btVector3 *dir) + const btMprSupport_t *v4, + const btVector3 *dir) { - float dot; - dot = btMprVec3Dot(&v4->v, dir); - return btMprIsZero(dot) || dot > 0.f; + float dot; + dot = btMprVec3Dot(&v4->v, dir); + return btMprIsZero(dot) || dot > 0.f; } inline void btExpandPortal(btMprSimplex_t *portal, - const btMprSupport_t *v4) -{ - float dot; - btVector3 v4v0; - - btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v); - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0); - if (dot > 0.f){ - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0); - if (dot > 0.f){ - btMprSimplexSet(portal, 1, v4); - }else{ - btMprSimplexSet(portal, 3, v4); - } - }else{ - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0); - if (dot > 0.f){ - btMprSimplexSet(portal, 2, v4); - }else{ - btMprSimplexSet(portal, 1, v4); - } - } + const btMprSupport_t *v4) +{ + float dot; + btVector3 v4v0; + + btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v); + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0); + if (dot > 0.f) + { + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0); + if (dot > 0.f) + { + btMprSimplexSet(portal, 1, v4); + } + else + { + btMprSimplexSet(portal, 3, v4); + } + } + else + { + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0); + if (dot > 0.f) + { + btMprSimplexSet(portal, 2, v4); + } + else + { + btMprSimplexSet(portal, 1, v4); + } + } } template <typename btConvexTemplate> -inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - const btVector3& dir, btMprSupport_t *supp) +inline void btMprSupport(const btConvexTemplate &a, const btConvexTemplate &b, + const btMprCollisionDescription &colDesc, + const btVector3 &dir, btMprSupport_t *supp) { - btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis(); - btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis(); + btVector3 separatingAxisInA = dir * a.getWorldTransform().getBasis(); + btVector3 separatingAxisInB = -dir * b.getWorldTransform().getBasis(); - btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA); - btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB); + btVector3 pInA = a.getLocalSupportWithMargin(separatingAxisInA); + btVector3 qInB = b.getLocalSupportWithMargin(separatingAxisInB); supp->v1 = a.getWorldTransform()(pInA); supp->v2 = b.getWorldTransform()(qInB); supp->v = supp->v1 - supp->v2; } - template <typename btConvexTemplate> -static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal) -{ - btVector3 dir, va, vb; - float dot; - int cont; - - - - // vertex 0 is center of portal - btFindOrigin(a,b,colDesc, btMprSimplexPointW(portal, 0)); - - - // vertex 0 is center of portal - btMprSimplexSetSize(portal, 1); - - - - btVector3 zero = btVector3(0,0,0); - btVector3* org = &zero; - - if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)){ - // Portal's center lies on origin (0,0,0) => we know that objects - // intersect but we would need to know penetration info. - // So move center little bit... - btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); - btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va); - } - - - // vertex 1 = support in direction of origin - btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v); - btMprVec3Scale(&dir, -1.f); - btMprVec3Normalize(&dir); - - - btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 1)); - - btMprSimplexSetSize(portal, 2); - - // test if origin isn't outside of v1 - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir); - - - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - - // vertex 2 - btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v, - &btMprSimplexPoint(portal, 1)->v); - if (btMprIsZero(btMprVec3Len2(&dir))){ - if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)){ - // origin lies on v1 - return 1; - }else{ - // origin lies on v0-v1 segment - return 2; - } - } - - btMprVec3Normalize(&dir); - btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 2)); - - - - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir); - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - btMprSimplexSetSize(portal, 3); - - // vertex 3 direction - btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Cross(&dir, &va, &vb); - btMprVec3Normalize(&dir); - - // it is better to form portal faces to be oriented "outside" origin - dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v); - if (dot > 0.f){ - btMprSimplexSwap(portal, 1, 2); - btMprVec3Scale(&dir, -1.f); - } - - while (btMprSimplexSize(portal) < 4){ - btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 3)); - - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir); - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - cont = 0; - - // test if origin is outside (v1, v0, v3) - set v2 as v3 and - // continue - btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 3)->v); - dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); - if (dot < 0.f && !btMprIsZero(dot)){ - btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3)); - cont = 1; - } - - if (!cont){ - // test if origin is outside (v3, v0, v2) - set v1 as v3 and - // continue - btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 2)->v); - dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); - if (dot < 0.f && !btMprIsZero(dot)){ - btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3)); - cont = 1; - } - } - - if (cont){ - btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Cross(&dir, &va, &vb); - btMprVec3Normalize(&dir); - }else{ - btMprSimplexSetSize(portal, 4); - } - } - - return 0; +static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b, + const btMprCollisionDescription &colDesc, + btMprSimplex_t *portal) +{ + btVector3 dir, va, vb; + float dot; + int cont; + + // vertex 0 is center of portal + btFindOrigin(a, b, colDesc, btMprSimplexPointW(portal, 0)); + + // vertex 0 is center of portal + btMprSimplexSetSize(portal, 1); + + btVector3 zero = btVector3(0, 0, 0); + btVector3 *org = &zero; + + if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)) + { + // Portal's center lies on origin (0,0,0) => we know that objects + // intersect but we would need to know penetration info. + // So move center little bit... + btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); + btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va); + } + + // vertex 1 = support in direction of origin + btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v); + btMprVec3Scale(&dir, -1.f); + btMprVec3Normalize(&dir); + + btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 1)); + + btMprSimplexSetSize(portal, 2); + + // test if origin isn't outside of v1 + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir); + + if (btMprIsZero(dot) || dot < 0.f) + return -1; + + // vertex 2 + btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v, + &btMprSimplexPoint(portal, 1)->v); + if (btMprIsZero(btMprVec3Len2(&dir))) + { + if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)) + { + // origin lies on v1 + return 1; + } + else + { + // origin lies on v0-v1 segment + return 2; + } + } + + btMprVec3Normalize(&dir); + btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 2)); + + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir); + if (btMprIsZero(dot) || dot < 0.f) + return -1; + + btMprSimplexSetSize(portal, 3); + + // vertex 3 direction + btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Cross(&dir, &va, &vb); + btMprVec3Normalize(&dir); + + // it is better to form portal faces to be oriented "outside" origin + dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v); + if (dot > 0.f) + { + btMprSimplexSwap(portal, 1, 2); + btMprVec3Scale(&dir, -1.f); + } + + while (btMprSimplexSize(portal) < 4) + { + btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 3)); + + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir); + if (btMprIsZero(dot) || dot < 0.f) + return -1; + + cont = 0; + + // test if origin is outside (v1, v0, v3) - set v2 as v3 and + // continue + btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 3)->v); + dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !btMprIsZero(dot)) + { + btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3)); + cont = 1; + } + + if (!cont) + { + // test if origin is outside (v3, v0, v2) - set v1 as v3 and + // continue + btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 2)->v); + dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !btMprIsZero(dot)) + { + btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3)); + cont = 1; + } + } + + if (cont) + { + btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Cross(&dir, &va, &vb); + btMprVec3Normalize(&dir); + } + else + { + btMprSimplexSetSize(portal, 4); + } + } + + return 0; } template <typename btConvexTemplate> -static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal) +static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, + btMprSimplex_t *portal) { - btVector3 dir; - btMprSupport_t v4; + btVector3 dir; + btMprSupport_t v4; - for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++) - //while (1) + for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++) + //while (1) { - // compute direction outside the portal (from v0 throught v1,v2,v3 - // face) - btPortalDir(portal, &dir); + // compute direction outside the portal (from v0 through v1,v2,v3 + // face) + btPortalDir(portal, &dir); - // test if origin is inside the portal - if (portalEncapsulesOrigin(portal, &dir)) - return 0; + // test if origin is inside the portal + if (portalEncapsulesOrigin(portal, &dir)) + return 0; - // get next support point - - btMprSupport(a,b,colDesc, dir, &v4); + // get next support point + btMprSupport(a, b, colDesc, dir, &v4); - // test if v4 can expand portal to contain origin and if portal - // expanding doesn't reach given tolerance - if (!portalCanEncapsuleOrigin(portal, &v4, &dir) - || portalReachTolerance(portal, &v4, &dir)) + // test if v4 can expand portal to contain origin and if portal + // expanding doesn't reach given tolerance + if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir)) { - return -1; - } + return -1; + } - // v1-v2-v3 triangle must be rearranged to face outside Minkowski - // difference (direction from v0). - btExpandPortal(portal, &v4); - } + // v1-v2-v3 triangle must be rearranged to face outside Minkowski + // difference (direction from v0). + btExpandPortal(portal, &v4); + } - return -1; + return -1; } static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos) { + btVector3 zero = btVector3(0, 0, 0); + btVector3 *origin = &zero; - btVector3 zero = btVector3(0,0,0); - btVector3* origin = &zero; - - btVector3 dir; - size_t i; - float b[4], sum, inv; - btVector3 vec, p1, p2; + btVector3 dir; + size_t i; + float b[4], sum, inv; + btVector3 vec, p1, p2; - btPortalDir(portal, &dir); + btPortalDir(portal, &dir); - // use barycentric coordinates of tetrahedron to find origin - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 2)->v); - b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); + // use barycentric coordinates of tetrahedron to find origin + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 2)->v); + b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 2)->v); - b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 2)->v); + b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v, - &btMprSimplexPoint(portal, 1)->v); - b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v, + &btMprSimplexPoint(portal, 1)->v); + b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 1)->v); - b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 1)->v); + b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); sum = b[0] + b[1] + b[2] + b[3]; - if (btMprIsZero(sum) || sum < 0.f){ + if (btMprIsZero(sum) || sum < 0.f) + { b[0] = 0.f; - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 3)->v); - b[1] = btMprVec3Dot(&vec, &dir); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 1)->v); - b[2] = btMprVec3Dot(&vec, &dir); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 2)->v); - b[3] = btMprVec3Dot(&vec, &dir); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 3)->v); + b[1] = btMprVec3Dot(&vec, &dir); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 1)->v); + b[2] = btMprVec3Dot(&vec, &dir); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 2)->v); + b[3] = btMprVec3Dot(&vec, &dir); sum = b[1] + b[2] + b[3]; } inv = 1.f / sum; - btMprVec3Copy(&p1, origin); - btMprVec3Copy(&p2, origin); - for (i = 0; i < 4; i++){ - btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1); - btMprVec3Scale(&vec, b[i]); - btMprVec3Add(&p1, &vec); - - btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2); - btMprVec3Scale(&vec, b[i]); - btMprVec3Add(&p2, &vec); - } - btMprVec3Scale(&p1, inv); - btMprVec3Scale(&p2, inv); + btMprVec3Copy(&p1, origin); + btMprVec3Copy(&p2, origin); + for (i = 0; i < 4; i++) + { + btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1); + btMprVec3Scale(&vec, b[i]); + btMprVec3Add(&p1, &vec); + + btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2); + btMprVec3Scale(&vec, b[i]); + btMprVec3Add(&p2, &vec); + } + btMprVec3Scale(&p1, inv); + btMprVec3Scale(&p2, inv); #ifdef MPR_AVERAGE_CONTACT_POSITIONS - btMprVec3Copy(pos, &p1); - btMprVec3Add(pos, &p2); - btMprVec3Scale(pos, 0.5); + btMprVec3Copy(pos, &p1); + btMprVec3Add(pos, &p2); + btMprVec3Scale(pos, 0.5); #else - btMprVec3Copy(pos, &p2); -#endif//MPR_AVERAGE_CONTACT_POSITIONS + btMprVec3Copy(pos, &p2); +#endif //MPR_AVERAGE_CONTACT_POSITIONS } inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b) { - btVector3 ab; - btMprVec3Sub2(&ab, a, b); - return btMprVec3Len2(&ab); + btVector3 ab; + btMprVec3Sub2(&ab, a, b); + return btMprVec3Len2(&ab); } inline float _btMprVec3PointSegmentDist2(const btVector3 *P, - const btVector3 *x0, - const btVector3 *b, - btVector3 *witness) -{ - // The computation comes from solving equation of segment: - // S(t) = x0 + t.d - // where - x0 is initial point of segment - // - d is direction of segment from x0 (|d| > 0) - // - t belongs to <0, 1> interval - // - // Than, distance from a segment to some point P can be expressed: - // D(t) = |x0 + t.d - P|^2 - // which is distance from any point on segment. Minimization - // of this function brings distance from P to segment. - // Minimization of D(t) leads to simple quadratic equation that's - // solving is straightforward. - // - // Bonus of this method is witness point for free. - - float dist, t; - btVector3 d, a; - - // direction of segment - btMprVec3Sub2(&d, b, x0); - - // precompute vector from P to x0 - btMprVec3Sub2(&a, x0, P); - - t = -1.f * btMprVec3Dot(&a, &d); - t /= btMprVec3Len2(&d); - - if (t < 0.f || btMprIsZero(t)){ - dist = btMprVec3Dist2(x0, P); - if (witness) - btMprVec3Copy(witness, x0); - }else if (t > 1.f || btMprEq(t, 1.f)){ - dist = btMprVec3Dist2(b, P); - if (witness) - btMprVec3Copy(witness, b); - }else{ - if (witness){ - btMprVec3Copy(witness, &d); - btMprVec3Scale(witness, t); - btMprVec3Add(witness, x0); - dist = btMprVec3Dist2(witness, P); - }else{ - // recycling variables - btMprVec3Scale(&d, t); - btMprVec3Add(&d, &a); - dist = btMprVec3Len2(&d); - } - } - - return dist; -} - + const btVector3 *x0, + const btVector3 *b, + btVector3 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + float dist, t; + btVector3 d, a; + + // direction of segment + btMprVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + btMprVec3Sub2(&a, x0, P); + + t = -1.f * btMprVec3Dot(&a, &d); + t /= btMprVec3Len2(&d); + + if (t < 0.f || btMprIsZero(t)) + { + dist = btMprVec3Dist2(x0, P); + if (witness) + btMprVec3Copy(witness, x0); + } + else if (t > 1.f || btMprEq(t, 1.f)) + { + dist = btMprVec3Dist2(b, P); + if (witness) + btMprVec3Copy(witness, b); + } + else + { + if (witness) + { + btMprVec3Copy(witness, &d); + btMprVec3Scale(witness, t); + btMprVec3Add(witness, x0); + dist = btMprVec3Dist2(witness, P); + } + else + { + // recycling variables + btMprVec3Scale(&d, t); + btMprVec3Add(&d, &a); + dist = btMprVec3Len2(&d); + } + } + return dist; +} inline float btMprVec3PointTriDist2(const btVector3 *P, - const btVector3 *x0, const btVector3 *B, - const btVector3 *C, - btVector3 *witness) -{ - // Computation comes from analytic expression for triangle (x0, B, C) - // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and - // Then equation for distance is: - // D(s, t) = | T(s, t) - P |^2 - // This leads to minimization of quadratic function of two variables. - // The solution from is taken only if s is between 0 and 1, t is - // between 0 and 1 and t + s < 1, otherwise distance from segment is - // computed. - - btVector3 d1, d2, a; - float u, v, w, p, q, r; - float s, t, dist, dist2; - btVector3 witness2; - - btMprVec3Sub2(&d1, B, x0); - btMprVec3Sub2(&d2, C, x0); - btMprVec3Sub2(&a, x0, P); - - u = btMprVec3Dot(&a, &a); - v = btMprVec3Dot(&d1, &d1); - w = btMprVec3Dot(&d2, &d2); - p = btMprVec3Dot(&a, &d1); - q = btMprVec3Dot(&a, &d2); - r = btMprVec3Dot(&d1, &d2); + const btVector3 *x0, const btVector3 *B, + const btVector3 *C, + btVector3 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + btVector3 d1, d2, a; + float u, v, w, p, q, r; + float s, t, dist, dist2; + btVector3 witness2; + + btMprVec3Sub2(&d1, B, x0); + btMprVec3Sub2(&d2, C, x0); + btMprVec3Sub2(&a, x0, P); + + u = btMprVec3Dot(&a, &a); + v = btMprVec3Dot(&d1, &d1); + w = btMprVec3Dot(&d2, &d2); + p = btMprVec3Dot(&a, &d1); + q = btMprVec3Dot(&a, &d2); + r = btMprVec3Dot(&d1, &d2); btScalar div = (w * v - r * r); if (btMprIsZero(div)) { - s=-1; - } else + s = -1; + } + else { s = (q * r - w * p) / div; t = (-s * r - q) / w; } - if ((btMprIsZero(s) || s > 0.f) - && (btMprEq(s, 1.f) || s < 1.f) - && (btMprIsZero(t) || t > 0.f) - && (btMprEq(t, 1.f) || t < 1.f) - && (btMprEq(t + s, 1.f) || t + s < 1.f)){ - - if (witness){ - btMprVec3Scale(&d1, s); - btMprVec3Scale(&d2, t); - btMprVec3Copy(witness, x0); - btMprVec3Add(witness, &d1); - btMprVec3Add(witness, &d2); - - dist = btMprVec3Dist2(witness, P); - }else{ - dist = s * s * v; - dist += t * t * w; - dist += 2.f * s * t * r; - dist += 2.f * s * p; - dist += 2.f * t * q; - dist += u; - } - }else{ - dist = _btMprVec3PointSegmentDist2(P, x0, B, witness); - - dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2); - if (dist2 < dist){ - dist = dist2; - if (witness) - btMprVec3Copy(witness, &witness2); - } - - dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2); - if (dist2 < dist){ - dist = dist2; - if (witness) - btMprVec3Copy(witness, &witness2); - } - } - - return dist; + if ((btMprIsZero(s) || s > 0.f) && (btMprEq(s, 1.f) || s < 1.f) && (btMprIsZero(t) || t > 0.f) && (btMprEq(t, 1.f) || t < 1.f) && (btMprEq(t + s, 1.f) || t + s < 1.f)) + { + if (witness) + { + btMprVec3Scale(&d1, s); + btMprVec3Scale(&d2, t); + btMprVec3Copy(witness, x0); + btMprVec3Add(witness, &d1); + btMprVec3Add(witness, &d2); + + dist = btMprVec3Dist2(witness, P); + } + else + { + dist = s * s * v; + dist += t * t * w; + dist += 2.f * s * t * r; + dist += 2.f * s * p; + dist += 2.f * t * q; + dist += u; + } + } + else + { + dist = _btMprVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist) + { + dist = dist2; + if (witness) + btMprVec3Copy(witness, &witness2); + } + + dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist) + { + dist = dist2; + if (witness) + btMprVec3Copy(witness, &witness2); + } + } + + return dist; } template <typename btConvexTemplate> -static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal, - float *depth, btVector3 *pdir, btVector3 *pos) +static void btFindPenetr(const btConvexTemplate &a, const btConvexTemplate &b, + const btMprCollisionDescription &colDesc, + btMprSimplex_t *portal, + float *depth, btVector3 *pdir, btVector3 *pos) { - btVector3 dir; - btMprSupport_t v4; - unsigned long iterations; + btVector3 dir; + btMprSupport_t v4; + unsigned long iterations; - btVector3 zero = btVector3(0,0,0); - btVector3* origin = &zero; + btVector3 zero = btVector3(0, 0, 0); + btVector3 *origin = &zero; - - iterations = 1UL; - for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++) - //while (1) + iterations = 1UL; + for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++) + //while (1) { - // compute portal direction and obtain next support point - btPortalDir(portal, &dir); - - btMprSupport(a,b,colDesc, dir, &v4); + // compute portal direction and obtain next support point + btPortalDir(portal, &dir); + btMprSupport(a, b, colDesc, dir, &v4); - // reached tolerance -> find penetration info - if (portalReachTolerance(portal, &v4, &dir) - || iterations ==BT_MPR_MAX_ITERATIONS) + // reached tolerance -> find penetration info + if (portalReachTolerance(portal, &v4, &dir) || iterations == BT_MPR_MAX_ITERATIONS) { - *depth = btMprVec3PointTriDist2(origin,&btMprSimplexPoint(portal, 1)->v,&btMprSimplexPoint(portal, 2)->v,&btMprSimplexPoint(portal, 3)->v,pdir); - *depth = BT_MPR_SQRT(*depth); - + *depth = btMprVec3PointTriDist2(origin, &btMprSimplexPoint(portal, 1)->v, &btMprSimplexPoint(portal, 2)->v, &btMprSimplexPoint(portal, 3)->v, pdir); + *depth = BT_MPR_SQRT(*depth); + if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z())) { - *pdir = dir; - } + } btMprVec3Normalize(pdir); - - // barycentric coordinates: - btFindPos(portal, pos); + // barycentric coordinates: + btFindPos(portal, pos); - return; - } + return; + } - btExpandPortal(portal, &v4); + btExpandPortal(portal, &v4); - iterations++; - } + iterations++; + } } -static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos) +static void btFindPenetrTouch(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos) { - // Touching contact on portal's v1 - so depth is zero and direction - // is unimportant and pos can be guessed - *depth = 0.f; - btVector3 zero = btVector3(0,0,0); - btVector3* origin = &zero; - + // Touching contact on portal's v1 - so depth is zero and direction + // is unimportant and pos can be guessed + *depth = 0.f; + btVector3 zero = btVector3(0, 0, 0); + btVector3 *origin = &zero; btMprVec3Copy(dir, origin); #ifdef MPR_AVERAGE_CONTACT_POSITIONS - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); - btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); - btMprVec3Scale(pos, 0.5); + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); + btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Scale(pos, 0.5); #else - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); #endif } static void btFindPenetrSegment(btMprSimplex_t *portal, - float *depth, btVector3 *dir, btVector3 *pos) + float *depth, btVector3 *dir, btVector3 *pos) { - - // Origin lies on v0-v1 segment. - // Depth is distance to v1, direction also and position must be - // computed + // Origin lies on v0-v1 segment. + // Depth is distance to v1, direction also and position must be + // computed #ifdef MPR_AVERAGE_CONTACT_POSITIONS - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); - btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); - btMprVec3Scale(pos, 0.5f); + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); + btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Scale(pos, 0.5f); #else - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); -#endif//MPR_AVERAGE_CONTACT_POSITIONS - - btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v); - *depth = BT_MPR_SQRT(btMprVec3Len2(dir)); - btMprVec3Normalize(dir); + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); +#endif //MPR_AVERAGE_CONTACT_POSITIONS - + btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v); + *depth = BT_MPR_SQRT(btMprVec3Len2(dir)); + btMprVec3Normalize(dir); } - template <typename btConvexTemplate> -inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - float *depthOut, btVector3* dirOut, btVector3* posOut) +inline int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, + const btMprCollisionDescription &colDesc, + float *depthOut, btVector3 *dirOut, btVector3 *posOut) { - - btMprSimplex_t portal; + btMprSimplex_t portal; + // Phase 1: Portal discovery + int result = btDiscoverPortal(a, b, colDesc, &portal); - // Phase 1: Portal discovery - int result = btDiscoverPortal(a,b,colDesc, &portal); - - //sepAxis[pairIndex] = *pdir;//or -dir? switch (result) { - case 0: + case 0: { // Phase 2: Portal refinement - - result = btRefinePortal(a,b,colDesc, &portal); + + result = btRefinePortal(a, b, colDesc, &portal); if (result < 0) return -1; // Phase 3. Penetration info - btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut); - - + btFindPenetr(a, b, colDesc, &portal, depthOut, dirOut, posOut); + break; } - case 1: + case 1: { - // Touching contact on portal's v1. + // Touching contact on portal's v1. btFindPenetrTouch(&portal, depthOut, dirOut, posOut); - result=0; + result = 0; break; } - case 2: + case 2: { - - btFindPenetrSegment( &portal, depthOut, dirOut, posOut); - result=0; + btFindPenetrSegment(&portal, depthOut, dirOut, posOut); + result = 0; break; } - default: + default: { //if (res < 0) //{ - // Origin isn't inside portal - no collision. - result = -1; + // Origin isn't inside portal - no collision. + result = -1; //} } }; - + return result; }; - -template<typename btConvexTemplate, typename btMprDistanceTemplate> -inline int btComputeMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, const - btMprCollisionDescription& colDesc, btMprDistanceTemplate* distInfo) +template <typename btConvexTemplate, typename btMprDistanceTemplate> +inline int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo) { - btVector3 dir,pos; + btVector3 dir, pos; float depth; - int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos); - if (res==0) + int res = btMprPenetration(a, b, colDesc, &depth, &dir, &pos); + if (res == 0) { distInfo->m_distance = -depth; distInfo->m_pointOnB = pos; distInfo->m_normalBtoA = -dir; - distInfo->m_pointOnA = pos-distInfo->m_distance*dir; + distInfo->m_pointOnA = pos - distInfo->m_distance * dir; return 0; } return -1; } - - -#endif //BT_MPR_PENETRATION_H +#endif //BT_MPR_PENETRATION_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 4d92e853d3f..dca3e09267d 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -13,84 +13,86 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btPersistentManifold.h" #include "LinearMath/btTransform.h" +#include "LinearMath/btSerializer.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define btCollisionObjectData btCollisionObjectDoubleData +#else +#define btCollisionObjectData btCollisionObjectFloatData +#endif -btScalar gContactBreakingThreshold = btScalar(0.02); -ContactDestroyedCallback gContactDestroyedCallback = 0; -ContactProcessedCallback gContactProcessedCallback = 0; +btScalar gContactBreakingThreshold = btScalar(0.02); +ContactDestroyedCallback gContactDestroyedCallback = 0; +ContactProcessedCallback gContactProcessedCallback = 0; +ContactStartedCallback gContactStartedCallback = 0; +ContactEndedCallback gContactEndedCallback = 0; ///gContactCalcArea3Points will approximate the convex hull area using 3 points ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower -bool gContactCalcArea3Points = true; - +bool gContactCalcArea3Points = true; btPersistentManifold::btPersistentManifold() -:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), -m_body0(0), -m_body1(0), -m_cachedPoints (0), -m_index1a(0) + : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), + m_body0(0), + m_body1(0), + m_cachedPoints(0), + m_companionIdA(0), + m_companionIdB(0), + m_index1a(0) { } - - - #ifdef DEBUG_PERSISTENCY #include <stdio.h> -void btPersistentManifold::DebugPersistency() +void btPersistentManifold::DebugPersistency() { int i; - printf("DebugPersistency : numPoints %d\n",m_cachedPoints); - for (i=0;i<m_cachedPoints;i++) + printf("DebugPersistency : numPoints %d\n", m_cachedPoints); + for (i = 0; i < m_cachedPoints; i++) { - printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData); + printf("m_pointCache[%d].m_userPersistentData = %x\n", i, m_pointCache[i].m_userPersistentData); } } -#endif //DEBUG_PERSISTENCY +#endif //DEBUG_PERSISTENCY void btPersistentManifold::clearUserCache(btManifoldPoint& pt) { - void* oldPtr = pt.m_userPersistentData; if (oldPtr) { #ifdef DEBUG_PERSISTENCY int i; int occurance = 0; - for (i=0;i<m_cachedPoints;i++) + for (i = 0; i < m_cachedPoints; i++) { if (m_pointCache[i].m_userPersistentData == oldPtr) { occurance++; - if (occurance>1) + if (occurance > 1) printf("error in clearUserCache\n"); } } - btAssert(occurance<=0); -#endif //DEBUG_PERSISTENCY + btAssert(occurance <= 0); +#endif //DEBUG_PERSISTENCY if (pt.m_userPersistentData && gContactDestroyedCallback) { (*gContactDestroyedCallback)(pt.m_userPersistentData); pt.m_userPersistentData = 0; } - + #ifdef DEBUG_PERSISTENCY DebugPersistency(); #endif } - - } -static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3) +static inline btScalar calcArea4Points(const btVector3& p0, const btVector3& p1, const btVector3& p2, const btVector3& p3) { // It calculates possible 3 area constructed from random 4 points and returns the biggest one. - btVector3 a[3],b[3]; + btVector3 a[3], b[3]; a[0] = p0 - p1; a[1] = p0 - p2; a[2] = p0 - p3; @@ -103,100 +105,102 @@ static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,c btVector3 tmp1 = a[1].cross(b[1]); btVector3 tmp2 = a[2].cross(b[2]); - return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2()); + return btMax(btMax(tmp0.length2(), tmp1.length2()), tmp2.length2()); } -int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) +int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) { - //calculate 4 possible cases areas, and take biggest area - //also need to keep 'deepest' - - int maxPenetrationIndex = -1; + //calculate 4 possible cases areas, and take biggest area + //also need to keep 'deepest' + + int maxPenetrationIndex = -1; #define KEEP_DEEPEST_POINT 1 #ifdef KEEP_DEEPEST_POINT - btScalar maxPenetration = pt.getDistance(); - for (int i=0;i<4;i++) + btScalar maxPenetration = pt.getDistance(); + for (int i = 0; i < 4; i++) + { + if (m_pointCache[i].getDistance() < maxPenetration) { - if (m_pointCache[i].getDistance() < maxPenetration) - { - maxPenetrationIndex = i; - maxPenetration = m_pointCache[i].getDistance(); - } + maxPenetrationIndex = i; + maxPenetration = m_pointCache[i].getDistance(); } -#endif //KEEP_DEEPEST_POINT - - btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.)); + } +#endif //KEEP_DEEPEST_POINT + + btScalar res0(btScalar(0.)), res1(btScalar(0.)), res2(btScalar(0.)), res3(btScalar(0.)); if (gContactCalcArea3Points) { if (maxPenetrationIndex != 0) { - btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; - btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + btVector3 a0 = pt.m_localPointA - m_pointCache[1].m_localPointA; + btVector3 b0 = m_pointCache[3].m_localPointA - m_pointCache[2].m_localPointA; btVector3 cross = a0.cross(b0); res0 = cross.length2(); } if (maxPenetrationIndex != 1) { - btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA; - btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + btVector3 a1 = pt.m_localPointA - m_pointCache[0].m_localPointA; + btVector3 b1 = m_pointCache[3].m_localPointA - m_pointCache[2].m_localPointA; btVector3 cross = a1.cross(b1); res1 = cross.length2(); } if (maxPenetrationIndex != 2) { - btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA; - btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA; + btVector3 a2 = pt.m_localPointA - m_pointCache[0].m_localPointA; + btVector3 b2 = m_pointCache[3].m_localPointA - m_pointCache[1].m_localPointA; btVector3 cross = a2.cross(b2); res2 = cross.length2(); } if (maxPenetrationIndex != 3) { - btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA; - btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA; + btVector3 a3 = pt.m_localPointA - m_pointCache[0].m_localPointA; + btVector3 b3 = m_pointCache[2].m_localPointA - m_pointCache[1].m_localPointA; btVector3 cross = a3.cross(b3); res3 = cross.length2(); } - } + } else { - if(maxPenetrationIndex != 0) { - res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); + if (maxPenetrationIndex != 0) + { + res0 = calcArea4Points(pt.m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[2].m_localPointA, m_pointCache[3].m_localPointA); } - if(maxPenetrationIndex != 1) { - res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); + if (maxPenetrationIndex != 1) + { + res1 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[2].m_localPointA, m_pointCache[3].m_localPointA); } - if(maxPenetrationIndex != 2) { - res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA); + if (maxPenetrationIndex != 2) + { + res2 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[3].m_localPointA); } - if(maxPenetrationIndex != 3) { - res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA); + if (maxPenetrationIndex != 3) + { + res3 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[2].m_localPointA); } } - btVector4 maxvec(res0,res1,res2,res3); + btVector4 maxvec(res0, res1, res2, res3); int biggestarea = maxvec.closestAxis4(); return biggestarea; - } - int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const { - btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); + btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); int size = getNumContacts(); int nearestPoint = -1; - for( int i = 0; i < size; i++ ) + for (int i = 0; i < size; i++) { - const btManifoldPoint &mp = m_pointCache[i]; + const btManifoldPoint& mp = m_pointCache[i]; - btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA; + btVector3 diffA = mp.m_localPointA - newPoint.m_localPointA; const btScalar distToManiPoint = diffA.dot(diffA); - if( distToManiPoint < shortestDist ) + if (distToManiPoint < shortestDist) { shortestDist = distToManiPoint; nearestPoint = i; @@ -211,7 +215,7 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool { btAssert(validContactDistance(newPoint)); } - + int insertIndex = getNumContacts(); if (insertIndex == MANIFOLD_CACHE_SIZE) { @@ -222,84 +226,225 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool insertIndex = 0; #endif clearUserCache(m_pointCache[insertIndex]); - - } else + } + else { m_cachedPoints++; - - } - if (insertIndex<0) - insertIndex=0; + if (insertIndex < 0) + insertIndex = 0; - btAssert(m_pointCache[insertIndex].m_userPersistentData==0); + btAssert(m_pointCache[insertIndex].m_userPersistentData == 0); m_pointCache[insertIndex] = newPoint; return insertIndex; } -btScalar btPersistentManifold::getContactBreakingThreshold() const +btScalar btPersistentManifold::getContactBreakingThreshold() const { return m_contactBreakingThreshold; } - - -void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB) +void btPersistentManifold::refreshContactPoints(const btTransform& trA, const btTransform& trB) { int i; #ifdef DEBUG_PERSISTENCY printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n", - trA.getOrigin().getX(), - trA.getOrigin().getY(), - trA.getOrigin().getZ(), - trB.getOrigin().getX(), - trB.getOrigin().getY(), - trB.getOrigin().getZ()); -#endif //DEBUG_PERSISTENCY + trA.getOrigin().getX(), + trA.getOrigin().getY(), + trA.getOrigin().getZ(), + trB.getOrigin().getX(), + trB.getOrigin().getY(), + trB.getOrigin().getZ()); +#endif //DEBUG_PERSISTENCY /// first refresh worldspace positions and distance - for (i=getNumContacts()-1;i>=0;i--) + for (i = getNumContacts() - 1; i >= 0; i--) { - btManifoldPoint &manifoldPoint = m_pointCache[i]; - manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA ); - manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB ); - manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB); + btManifoldPoint& manifoldPoint = m_pointCache[i]; + manifoldPoint.m_positionWorldOnA = trA(manifoldPoint.m_localPointA); + manifoldPoint.m_positionWorldOnB = trB(manifoldPoint.m_localPointB); + manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB); manifoldPoint.m_lifeTime++; } - /// then + /// then btScalar distance2d; - btVector3 projectedDifference,projectedPoint; - for (i=getNumContacts()-1;i>=0;i--) + btVector3 projectedDifference, projectedPoint; + for (i = getNumContacts() - 1; i >= 0; i--) { - - btManifoldPoint &manifoldPoint = m_pointCache[i]; + btManifoldPoint& manifoldPoint = m_pointCache[i]; //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) if (!validContactDistance(manifoldPoint)) { removeContactPoint(i); - } else + } + else { + //todo: friction anchor may require the contact to be around a bit longer //contact also becomes invalid when relative movement orthogonal to normal exceeds margin projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1; projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint; distance2d = projectedDifference.dot(projectedDifference); - if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() ) + if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold()) { removeContactPoint(i); - } else + } + else { //contact point processed callback if (gContactProcessedCallback) - (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1); + (*gContactProcessedCallback)(manifoldPoint, (void*)m_body0, (void*)m_body1); } } } #ifdef DEBUG_PERSISTENCY DebugPersistency(); -#endif // +#endif // } +int btPersistentManifold::calculateSerializeBufferSize() const +{ + return sizeof(btPersistentManifoldData); +} +const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const +{ + btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer; + memset(dataOut, 0, sizeof(btPersistentManifoldData)); + + dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0()); + dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1()); + dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold(); + dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold(); + dataOut->m_numCachedPoints = manifold->getNumContacts(); + dataOut->m_companionIdA = manifold->m_companionIdA; + dataOut->m_companionIdB = manifold->m_companionIdB; + dataOut->m_index1a = manifold->m_index1a; + dataOut->m_objectType = manifold->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + const btManifoldPoint& pt = manifold->getContactPoint(i); + dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse; + dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS; + dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1; + dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2; + pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]); + pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]); + dataOut->m_pointCacheDistance[i] = pt.m_distance1; + dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1; + dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1; + dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime; + dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM; + dataOut->m_pointCacheContactERP[i] = pt.m_contactERP; + dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM; + dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags; + dataOut->m_pointCacheIndex0[i] = pt.m_index0; + dataOut->m_pointCacheIndex1[i] = pt.m_index1; + dataOut->m_pointCachePartId0[i] = pt.m_partId0; + dataOut->m_pointCachePartId1[i] = pt.m_partId1; + pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]); + dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction; + pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]); + dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction; + dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction; + dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution; + dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1; + dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2; + } + return btPersistentManifoldDataName; +} +void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr) +{ + m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; + m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; + m_cachedPoints = manifoldDataPtr->m_numCachedPoints; + m_companionIdA = manifoldDataPtr->m_companionIdA; + m_companionIdB = manifoldDataPtr->m_companionIdB; + //m_index1a = manifoldDataPtr->m_index1a; + m_objectType = manifoldDataPtr->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + btManifoldPoint& pt = m_pointCache[i]; + + pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i]; + pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; + pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; + pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]); + pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); + pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; + pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; + pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; + pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; + pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; + pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; + pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; + pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; + pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; + pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; + pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; + pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; + pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); + pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; + pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); + pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; + pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; + pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; + pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; + pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; + } +} - +void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr) +{ + m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; + m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; + m_cachedPoints = manifoldDataPtr->m_numCachedPoints; + m_companionIdA = manifoldDataPtr->m_companionIdA; + m_companionIdB = manifoldDataPtr->m_companionIdB; + //m_index1a = manifoldDataPtr->m_index1a; + m_objectType = manifoldDataPtr->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + btManifoldPoint& pt = m_pointCache[i]; + + pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i]; + pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; + pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; + pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]); + pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); + pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; + pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; + pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; + pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; + pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; + pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; + pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; + pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; + pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; + pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; + pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; + pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; + pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); + pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; + pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); + pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; + pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; + pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; + pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; + pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; + } +} diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 5026397f67f..0e26da0ebe7 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BT_PERSISTENT_MANIFOLD_H #define BT_PERSISTENT_MANIFOLD_H - #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "btManifoldPoint.h" @@ -24,14 +23,24 @@ class btCollisionObject; #include "LinearMath/btAlignedAllocator.h" struct btCollisionResult; +struct btCollisionObjectDoubleData; +struct btCollisionObjectFloatData; ///maximum contact breaking and merging threshold extern btScalar gContactBreakingThreshold; +#ifndef SWIG +class btPersistentManifold; + typedef bool (*ContactDestroyedCallback)(void* userPersistentData); -typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1); -extern ContactDestroyedCallback gContactDestroyedCallback; +typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp, void* body0, void* body1); +typedef void (*ContactStartedCallback)(btPersistentManifold* const& manifold); +typedef void (*ContactEndedCallback)(btPersistentManifold* const& manifold); +extern ContactDestroyedCallback gContactDestroyedCallback; extern ContactProcessedCallback gContactProcessedCallback; +extern ContactStartedCallback gContactStartedCallback; +extern ContactEndedCallback gContactEndedCallback; +#endif //SWIG //the enum starts at 1024 to avoid type conflicts with btTypedConstraint enum btContactManifoldTypes @@ -50,70 +59,74 @@ enum btContactManifoldTypes ///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points ///note that some pairs of objects might have more then one contact manifold. - -ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject -//ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject +//ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject +ATTRIBUTE_ALIGNED16(class) +btPersistentManifold : public btTypedObject { - btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; /// this two body pointers can point to the physics rigidbody class. const btCollisionObject* m_body0; const btCollisionObject* m_body1; - int m_cachedPoints; + int m_cachedPoints; - btScalar m_contactBreakingThreshold; - btScalar m_contactProcessingThreshold; + btScalar m_contactBreakingThreshold; + btScalar m_contactProcessingThreshold; - /// sort cached points so most isolated points come first - int sortCachedPoints(const btManifoldPoint& pt); + int sortCachedPoints(const btManifoldPoint& pt); - int findContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt); + int findContactPoint(const btManifoldPoint* unUsed, int numUnused, const btManifoldPoint& pt); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - int m_companionIdA; - int m_companionIdB; + int m_companionIdA; + int m_companionIdB; int m_index1a; btPersistentManifold(); - btPersistentManifold(const btCollisionObject* body0,const btCollisionObject* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold) + btPersistentManifold(const btCollisionObject* body0, const btCollisionObject* body1, int, btScalar contactBreakingThreshold, btScalar contactProcessingThreshold) : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), - m_body0(body0),m_body1(body1),m_cachedPoints(0), - m_contactBreakingThreshold(contactBreakingThreshold), - m_contactProcessingThreshold(contactProcessingThreshold) + m_body0(body0), + m_body1(body1), + m_cachedPoints(0), + m_contactBreakingThreshold(contactBreakingThreshold), + m_contactProcessingThreshold(contactProcessingThreshold), + m_companionIdA(0), + m_companionIdB(0), + m_index1a(0) { } - SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0;} - SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1;} + SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0; } + SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1; } - void setBodies(const btCollisionObject* body0,const btCollisionObject* body1) + void setBodies(const btCollisionObject* body0, const btCollisionObject* body1) { m_body0 = body0; m_body1 = body1; } - void clearUserCache(btManifoldPoint& pt); + void clearUserCache(btManifoldPoint & pt); #ifdef DEBUG_PERSISTENCY - void DebugPersistency(); -#endif // - - SIMD_FORCE_INLINE int getNumContacts() const { return m_cachedPoints;} + void DebugPersistency(); +#endif // + + SIMD_FORCE_INLINE int getNumContacts() const + { + return m_cachedPoints; + } /// the setNumContacts API is usually not used, except when you gather/fill all contacts manually void setNumContacts(int cachedPoints) { m_cachedPoints = cachedPoints; } - SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const { btAssert(index < m_cachedPoints); @@ -127,110 +140,235 @@ public: } ///@todo: get this margin from the current physics / collision environment - btScalar getContactBreakingThreshold() const; + btScalar getContactBreakingThreshold() const; - btScalar getContactProcessingThreshold() const + btScalar getContactProcessingThreshold() const { return m_contactProcessingThreshold; } - + void setContactBreakingThreshold(btScalar contactBreakingThreshold) { m_contactBreakingThreshold = contactBreakingThreshold; } - void setContactProcessingThreshold(btScalar contactProcessingThreshold) + void setContactProcessingThreshold(btScalar contactProcessingThreshold) { m_contactProcessingThreshold = contactProcessingThreshold; } - - - int getCacheEntry(const btManifoldPoint& newPoint) const; - int addManifoldPoint( const btManifoldPoint& newPoint, bool isPredictive=false); + int addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive = false); - void removeContactPoint (int index) + void removeContactPoint(int index) { clearUserCache(m_pointCache[index]); int lastUsedIndex = getNumContacts() - 1; -// m_pointCache[index] = m_pointCache[lastUsedIndex]; - if(index != lastUsedIndex) + // m_pointCache[index] = m_pointCache[lastUsedIndex]; + if (index != lastUsedIndex) { - m_pointCache[index] = m_pointCache[lastUsedIndex]; + m_pointCache[index] = m_pointCache[lastUsedIndex]; //get rid of duplicated userPersistentData pointer m_pointCache[lastUsedIndex].m_userPersistentData = 0; m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f; - m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false; + m_pointCache[lastUsedIndex].m_prevRHS = 0.f; + m_pointCache[lastUsedIndex].m_contactPointFlags = 0; m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f; m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f; m_pointCache[lastUsedIndex].m_lifeTime = 0; } - btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0); + btAssert(m_pointCache[lastUsedIndex].m_userPersistentData == 0); m_cachedPoints--; + + if (gContactEndedCallback && m_cachedPoints == 0) + { + gContactEndedCallback(this); + } } - void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex) + void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex) { btAssert(validContactDistance(newPoint)); #define MAINTAIN_PERSISTENCY 1 #ifdef MAINTAIN_PERSISTENCY - int lifeTime = m_pointCache[insertIndex].getLifeTime(); - btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; - btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; - btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; -// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized; - - - - btAssert(lifeTime>=0); - void* cache = m_pointCache[insertIndex].m_userPersistentData; - - m_pointCache[insertIndex] = newPoint; + int lifeTime = m_pointCache[insertIndex].getLifeTime(); + btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; + btScalar prevRHS = m_pointCache[insertIndex].m_prevRHS; + btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; + btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; + + bool replacePoint = true; + ///we keep existing contact points for friction anchors + ///if the friction force is within the Coulomb friction cone + if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) + { + // printf("appliedImpulse=%f\n", appliedImpulse); + // printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1); + // printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2); + // printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction); + btScalar mu = m_pointCache[insertIndex].m_combinedFriction; + btScalar eps = 0; //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7 + btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2; + btScalar b = eps + mu * appliedImpulse; + b = b * b; + replacePoint = (a) > (b); + } - m_pointCache[insertIndex].m_userPersistentData = cache; - m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; - m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; - m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; - + if (replacePoint) + { + btAssert(lifeTime >= 0); + void* cache = m_pointCache[insertIndex].m_userPersistentData; + + m_pointCache[insertIndex] = newPoint; + m_pointCache[insertIndex].m_userPersistentData = cache; + m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; + m_pointCache[insertIndex].m_prevRHS = prevRHS; + m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; + m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; + } m_pointCache[insertIndex].m_lifeTime = lifeTime; #else clearUserCache(m_pointCache[insertIndex]); m_pointCache[insertIndex] = newPoint; - + #endif } - bool validContactDistance(const btManifoldPoint& pt) const { return pt.m_distance1 <= getContactBreakingThreshold(); } /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin - void refreshContactPoints( const btTransform& trA,const btTransform& trB); + void refreshContactPoints(const btTransform& trA, const btTransform& trB); - - SIMD_FORCE_INLINE void clearManifold() + SIMD_FORCE_INLINE void clearManifold() { int i; - for (i=0;i<m_cachedPoints;i++) + for (i = 0; i < m_cachedPoints; i++) { clearUserCache(m_pointCache[i]); } + + if (gContactEndedCallback && m_cachedPoints) + { + gContactEndedCallback(this); + } m_cachedPoints = 0; } + int calculateSerializeBufferSize() const; + const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const; + void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr); + void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr); +}; +// clang-format off -} -; +struct btPersistentManifoldDoubleData +{ + btVector3DoubleData m_pointCacheLocalPointA[4]; + btVector3DoubleData m_pointCacheLocalPointB[4]; + btVector3DoubleData m_pointCachePositionWorldOnA[4]; + btVector3DoubleData m_pointCachePositionWorldOnB[4]; + btVector3DoubleData m_pointCacheNormalWorldOnB[4]; + btVector3DoubleData m_pointCacheLateralFrictionDir1[4]; + btVector3DoubleData m_pointCacheLateralFrictionDir2[4]; + double m_pointCacheDistance[4]; + double m_pointCacheAppliedImpulse[4]; + double m_pointCachePrevRHS[4]; + double m_pointCacheCombinedFriction[4]; + double m_pointCacheCombinedRollingFriction[4]; + double m_pointCacheCombinedSpinningFriction[4]; + double m_pointCacheCombinedRestitution[4]; + int m_pointCachePartId0[4]; + int m_pointCachePartId1[4]; + int m_pointCacheIndex0[4]; + int m_pointCacheIndex1[4]; + int m_pointCacheContactPointFlags[4]; + double m_pointCacheAppliedImpulseLateral1[4]; + double m_pointCacheAppliedImpulseLateral2[4]; + double m_pointCacheContactMotion1[4]; + double m_pointCacheContactMotion2[4]; + double m_pointCacheContactCFM[4]; + double m_pointCacheCombinedContactStiffness1[4]; + double m_pointCacheContactERP[4]; + double m_pointCacheCombinedContactDamping1[4]; + double m_pointCacheFrictionCFM[4]; + int m_pointCacheLifeTime[4]; + + int m_numCachedPoints; + int m_companionIdA; + int m_companionIdB; + int m_index1a; + int m_objectType; + double m_contactBreakingThreshold; + double m_contactProcessingThreshold; + int m_padding; + btCollisionObjectDoubleData *m_body0; + btCollisionObjectDoubleData *m_body1; +}; +struct btPersistentManifoldFloatData +{ + btVector3FloatData m_pointCacheLocalPointA[4]; + btVector3FloatData m_pointCacheLocalPointB[4]; + btVector3FloatData m_pointCachePositionWorldOnA[4]; + btVector3FloatData m_pointCachePositionWorldOnB[4]; + btVector3FloatData m_pointCacheNormalWorldOnB[4]; + btVector3FloatData m_pointCacheLateralFrictionDir1[4]; + btVector3FloatData m_pointCacheLateralFrictionDir2[4]; + float m_pointCacheDistance[4]; + float m_pointCacheAppliedImpulse[4]; + float m_pointCachePrevRHS[4]; + float m_pointCacheCombinedFriction[4]; + float m_pointCacheCombinedRollingFriction[4]; + float m_pointCacheCombinedSpinningFriction[4]; + float m_pointCacheCombinedRestitution[4]; + int m_pointCachePartId0[4]; + int m_pointCachePartId1[4]; + int m_pointCacheIndex0[4]; + int m_pointCacheIndex1[4]; + int m_pointCacheContactPointFlags[4]; + float m_pointCacheAppliedImpulseLateral1[4]; + float m_pointCacheAppliedImpulseLateral2[4]; + float m_pointCacheContactMotion1[4]; + float m_pointCacheContactMotion2[4]; + float m_pointCacheContactCFM[4]; + float m_pointCacheCombinedContactStiffness1[4]; + float m_pointCacheContactERP[4]; + float m_pointCacheCombinedContactDamping1[4]; + float m_pointCacheFrictionCFM[4]; + int m_pointCacheLifeTime[4]; + + int m_numCachedPoints; + int m_companionIdA; + int m_companionIdB; + int m_index1a; + + int m_objectType; + float m_contactBreakingThreshold; + float m_contactProcessingThreshold; + int m_padding; + + btCollisionObjectFloatData *m_body0; + btCollisionObjectFloatData *m_body1; +}; + +// clang-format on + +#ifdef BT_USE_DOUBLE_PRECISION +#define btPersistentManifoldData btPersistentManifoldDoubleData +#define btPersistentManifoldDataName "btPersistentManifoldDoubleData" +#else +#define btPersistentManifoldData btPersistentManifoldFloatData +#define btPersistentManifoldDataName "btPersistentManifoldFloatData" +#endif //BT_USE_DOUBLE_PRECISION -#endif //BT_PERSISTENT_MANIFOLD_H +#endif //BT_PERSISTENT_MANIFOLD_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h index 18da171011a..0900eb6e85a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h @@ -18,38 +18,33 @@ subject to the following restrictions: #include "btDiscreteCollisionDetectorInterface.h" - - struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result { - - btVector3 m_normalOnBInWorld; btVector3 m_pointInWorld; - btScalar m_distance;//negative means penetration + btScalar m_distance; //negative means penetration - bool m_hasResult; + bool m_hasResult; - btPointCollector () - : m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false) + btPointCollector() + : m_distance(btScalar(BT_LARGE_FLOAT)), m_hasResult(false) { } - virtual void setShapeIdentifiersA(int partId0,int index0) + virtual void setShapeIdentifiersA(int partId0, int index0) { (void)partId0; (void)index0; - } - virtual void setShapeIdentifiersB(int partId1,int index1) + virtual void setShapeIdentifiersB(int partId1, int index1) { (void)partId1; (void)index1; } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { - if (depth< m_distance) + if (depth < m_distance) { m_hasResult = true; m_normalOnBInWorld = normalOnBInWorld; @@ -60,5 +55,4 @@ struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result } }; -#endif //BT_POINT_COLLECTOR_H - +#endif //BT_POINT_COLLECTOR_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp index d5f4a964bf3..9d1836037d5 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -13,45 +13,42 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans ///Separating axis rest based on work from Pierre Terdiman, see ///And contact clipping based on work from Simon Hobbs - #include "btPolyhedralContactClipping.h" #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" -#include <float.h> //for FLT_MAX +#include <float.h> //for FLT_MAX -int gExpectedNbTests=0; +int gExpectedNbTests = 0; int gActualNbTests = 0; bool gUseInternalObject = true; // Clips a face to the back of a plane -void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS) +void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS) { - int ve; btScalar ds, de; int numVerts = pVtxIn.size(); if (numVerts < 2) return; - btVector3 firstVertex=pVtxIn[pVtxIn.size()-1]; + btVector3 firstVertex = pVtxIn[pVtxIn.size() - 1]; btVector3 endVertex = pVtxIn[0]; - - ds = planeNormalWS.dot(firstVertex)+planeEqWS; + + ds = planeNormalWS.dot(firstVertex) + planeEqWS; for (ve = 0; ve < numVerts; ve++) { - endVertex=pVtxIn[ve]; + endVertex = pVtxIn[ve]; - de = planeNormalWS.dot(endVertex)+planeEqWS; + de = planeNormalWS.dot(endVertex) + planeEqWS; - if (ds<0) + if (ds < 0) { - if (de<0) + if (de < 0) { // Start < 0, end < 0, so output endVertex ppVtxOut.push_back(endVertex); @@ -59,15 +56,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex else { // Start < 0, end >= 0, so output intersection - ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de)))); } } else { - if (de<0) + if (de < 0) { // Start >= 0, end < 0 so output intersection and end - ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de)))); ppVtxOut.push_back(endVertex); } } @@ -76,47 +73,44 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex } } - -static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB) +static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB) { - btScalar Min0,Max0; - btScalar Min1,Max1; - btVector3 witnesPtMinA,witnesPtMaxA; - btVector3 witnesPtMinB,witnesPtMaxB; + btScalar Min0, Max0; + btScalar Min1, Max1; + btVector3 witnesPtMinA, witnesPtMaxA; + btVector3 witnesPtMinB, witnesPtMaxB; - hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA); - hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB); + hullA.project(transA, sep_axis, Min0, Max0, witnesPtMinA, witnesPtMaxA); + hullB.project(transB, sep_axis, Min1, Max1, witnesPtMinB, witnesPtMaxB); - if(Max0<Min1 || Max1<Min0) + if (Max0 < Min1 || Max1 < Min0) return false; btScalar d0 = Max0 - Min1; - btAssert(d0>=0.0f); + btAssert(d0 >= 0.0f); btScalar d1 = Max1 - Min0; - btAssert(d1>=0.0f); - if (d0<d1) + btAssert(d1 >= 0.0f); + if (d0 < d1) { depth = d0; witnessPointA = witnesPtMaxA; witnessPointB = witnesPtMinB; - - } else + } + else { depth = d1; witnessPointA = witnesPtMinA; witnessPointB = witnesPtMaxB; } - + return true; } - - -static int gActualSATPairTests=0; +static int gActualSATPairTests = 0; inline bool IsAlmostZero(const btVector3& v) { - if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6) return false; + if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false; return true; } @@ -125,9 +119,9 @@ inline bool IsAlmostZero(const btVector3& v) inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3]) { // This version is ~11.000 cycles (4%) faster overall in one of the tests. -// IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK); -// IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK); -// IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK); + // IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK); + // IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK); + // IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK); p[0] = sv[0] < 0.0f ? -extents[0] : extents[0]; p[1] = sv[1] < 0.0f ? -extents[1] : extents[1]; p[2] = sv[2] < 0.0f ? -extents[2] : extents[2]; @@ -140,90 +134,94 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans const btVector3& r1 = rot[1]; const btVector3& r2 = rot[2]; - const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z(); - const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z(); - const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z(); + const btScalar x = r0.x() * in.x() + r1.x() * in.y() + r2.x() * in.z(); + const btScalar y = r0.y() * in.x() + r1.y() * in.y() + r2.y() * in.z(); + const btScalar z = r0.z() * in.x() + r1.z() * in.y() + r2.z() * in.z(); out.setValue(x, y, z); } - bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin) +bool TestInternalObjects(const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin) { const btScalar dp = delta_c.dot(axis); btVector3 localAxis0; - InverseTransformPoint3x3(localAxis0, axis,trans0); + InverseTransformPoint3x3(localAxis0, axis, trans0); btVector3 localAxis1; - InverseTransformPoint3x3(localAxis1, axis,trans1); + InverseTransformPoint3x3(localAxis1, axis, trans1); btScalar p0[3]; BoxSupport(convex0.m_extents, localAxis0, p0); btScalar p1[3]; BoxSupport(convex1.m_extents, localAxis1, p1); - const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z(); - const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z(); + const btScalar Radius0 = p0[0] * localAxis0.x() + p0[1] * localAxis0.y() + p0[2] * localAxis0.z(); + const btScalar Radius1 = p1[0] * localAxis1.x() + p1[1] * localAxis1.y() + p1[2] * localAxis1.z(); - const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius; - const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius; + const btScalar MinRadius = Radius0 > convex0.m_radius ? Radius0 : convex0.m_radius; + const btScalar MaxRadius = Radius1 > convex1.m_radius ? Radius1 : convex1.m_radius; const btScalar MinMaxRadius = MaxRadius + MinRadius; const btScalar d0 = MinMaxRadius + dp; const btScalar d1 = MinMaxRadius - dp; - const btScalar depth = d0<d1 ? d0:d1; - if(depth>dmin) + const btScalar depth = d0 < d1 ? d0 : d1; + if (depth > dmin) return false; return true; } -#endif //TEST_INTERNAL_OBJECTS +#endif //TEST_INTERNAL_OBJECTS - - - SIMD_FORCE_INLINE void btSegmentsClosestPoints( +SIMD_FORCE_INLINE void btSegmentsClosestPoints( btVector3& ptsVector, btVector3& offsetA, btVector3& offsetB, btScalar& tA, btScalar& tB, const btVector3& translation, const btVector3& dirA, btScalar hlenA, - const btVector3& dirB, btScalar hlenB ) + const btVector3& dirB, btScalar hlenB) { // compute the parameters of the closest points on each line segment - btScalar dirA_dot_dirB = btDot(dirA,dirB); - btScalar dirA_dot_trans = btDot(dirA,translation); - btScalar dirB_dot_trans = btDot(dirB,translation); + btScalar dirA_dot_dirB = btDot(dirA, dirB); + btScalar dirA_dot_trans = btDot(dirA, translation); + btScalar dirB_dot_trans = btDot(dirB, translation); btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - if ( denom == 0.0f ) { + if (denom == 0.0f) + { tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) + } + else + { + tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom; + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } tB = tA * dirA_dot_dirB - dirB_dot_trans; - if ( tB < -hlenB ) { + if (tB < -hlenB) + { tB = -hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; - } else if ( tB > hlenB ) { + } + else if (tB > hlenB) + { tB = hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } @@ -235,44 +233,42 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans ptsVector = translation - offsetA + offsetB; } - - -bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut) +bool btPolyhedralContactClipping::findSeparatingAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut) { gActualSATPairTests++; -//#ifdef TEST_INTERNAL_OBJECTS + //#ifdef TEST_INTERNAL_OBJECTS const btVector3 c0 = transA * hullA.m_localCenter; const btVector3 c1 = transB * hullB.m_localCenter; const btVector3 DeltaC2 = c0 - c1; -//#endif + //#endif btScalar dmin = FLT_MAX; - int curPlaneTests=0; + int curPlaneTests = 0; int numFacesA = hullA.m_faces.size(); // Test normals from hullA - for(int i=0;i<numFacesA;i++) + for (int i = 0; i < numFacesA; i++) { const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]); btVector3 faceANormalWS = transA.getBasis() * Normal; - if (DeltaC2.dot(faceANormalWS)<0) - faceANormalWS*=-1.f; + if (DeltaC2.dot(faceANormalWS) < 0) + faceANormalWS *= -1.f; curPlaneTests++; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; - if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar d; - btVector3 wA,wB; - if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB)) + btVector3 wA, wB; + if (!TestSepAxis(hullA, hullB, transA, transB, faceANormalWS, d, wA, wB)) return false; - if(d<dmin) + if (d < dmin) { dmin = d; sep = faceANormalWS; @@ -281,92 +277,89 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& int numFacesB = hullB.m_faces.size(); // Test normals from hullB - for(int i=0;i<numFacesB;i++) + for (int i = 0; i < numFacesB; i++) { const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]); btVector3 WorldNormal = transB.getBasis() * Normal; - if (DeltaC2.dot(WorldNormal)<0) - WorldNormal *=-1.f; + if (DeltaC2.dot(WorldNormal) < 0) + WorldNormal *= -1.f; curPlaneTests++; #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; - if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin)) + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar d; - btVector3 wA,wB; - if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB)) + btVector3 wA, wB; + if (!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d, wA, wB)) return false; - if(d<dmin) + if (d < dmin) { dmin = d; sep = WorldNormal; } } - btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend; - int edgeA=-1; - int edgeB=-1; + btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend; + int edgeA = -1; + int edgeB = -1; btVector3 worldEdgeA; btVector3 worldEdgeB; - btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0); - + btVector3 witnessPointA(0, 0, 0), witnessPointB(0, 0, 0); int curEdgeEdge = 0; // Test edges - for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++) + for (int e0 = 0; e0 < hullA.m_uniqueEdges.size(); e0++) { const btVector3 edge0 = hullA.m_uniqueEdges[e0]; const btVector3 WorldEdge0 = transA.getBasis() * edge0; - for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++) + for (int e1 = 0; e1 < hullB.m_uniqueEdges.size(); e1++) { const btVector3 edge1 = hullB.m_uniqueEdges[e1]; const btVector3 WorldEdge1 = transB.getBasis() * edge1; btVector3 Cross = WorldEdge0.cross(WorldEdge1); curEdgeEdge++; - if(!IsAlmostZero(Cross)) + if (!IsAlmostZero(Cross)) { Cross = Cross.normalize(); - if (DeltaC2.dot(Cross)<0) + if (DeltaC2.dot(Cross) < 0) Cross *= -1.f; - #ifdef TEST_INTERNAL_OBJECTS gExpectedNbTests++; - if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin)) + if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin)) continue; gActualNbTests++; #endif btScalar dist; - btVector3 wA,wB; - if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB)) + btVector3 wA, wB; + if (!TestSepAxis(hullA, hullB, transA, transB, Cross, dist, wA, wB)) return false; - if(dist<dmin) + if (dist < dmin) { dmin = dist; sep = Cross; - edgeA=e0; - edgeB=e1; + edgeA = e0; + edgeB = e1; worldEdgeA = WorldEdge0; worldEdgeB = WorldEdge1; - witnessPointA=wA; - witnessPointB=wB; + witnessPointA = wA; + witnessPointB = wB; } } } - } - if (edgeA>=0&&edgeB>=0) + if (edgeA >= 0 && edgeB >= 0) { -// printf("edge-edge\n"); + // printf("edge-edge\n"); //add an edge-edge contact btVector3 ptsVector; @@ -375,57 +368,55 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& btScalar tA; btScalar tB; - btVector3 translation = witnessPointB-witnessPointA; + btVector3 translation = witnessPointB - witnessPointA; btVector3 dirA = worldEdgeA; btVector3 dirB = worldEdgeB; - + btScalar hlenB = 1e30f; btScalar hlenA = 1e30f; - btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB, - translation, - dirA, hlenA, - dirB,hlenB); + btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, + translation, + dirA, hlenA, + dirB, hlenB); btScalar nlSqrt = ptsVector.length2(); - if (nlSqrt>SIMD_EPSILON) + if (nlSqrt > SIMD_EPSILON) { btScalar nl = btSqrt(nlSqrt); - ptsVector *= 1.f/nl; - if (ptsVector.dot(DeltaC2)<0.f) + ptsVector *= 1.f / nl; + if (ptsVector.dot(DeltaC2) < 0.f) { - ptsVector*=-1.f; + ptsVector *= -1.f; } btVector3 ptOnB = witnessPointB + offsetB; btScalar distance = nl; - resultOut.addContactPoint(ptsVector, ptOnB,-distance); + resultOut.addContactPoint(ptsVector, ptOnB, -distance); } - } - - if((DeltaC2.dot(sep))<0.0f) + if ((DeltaC2.dot(sep)) < 0.0f) sep = -sep; return true; } -void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut) { - btVertexArray worldVertsB2; + worldVertsB2.resize(0); btVertexArray* pVtxIn = &worldVertsB1; btVertexArray* pVtxOut = &worldVertsB2; pVtxOut->reserve(pVtxIn->size()); - int closestFaceA=-1; + int closestFaceA = -1; { btScalar dmin = FLT_MAX; - for(int face=0;face<hullA.m_faces.size();face++) + for (int face = 0; face < hullA.m_faces.size(); face++) { const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]); const btVector3 faceANormalWS = transA.getBasis() * Normal; - + btScalar d = faceANormalWS.dot(separatingNormal); if (d < dmin) { @@ -434,69 +425,66 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin } } } - if (closestFaceA<0) + if (closestFaceA < 0) return; const btFace& polyA = hullA.m_faces[closestFaceA]; - // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face int numVerticesA = polyA.m_indices.size(); - for(int e0=0;e0<numVerticesA;e0++) + for (int e0 = 0; e0 < numVerticesA; e0++) { const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]]; - const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]]; + const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]]; const btVector3 edge0 = a - b; const btVector3 WorldEdge0 = transA.getBasis() * edge0; - btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]); + btVector3 worldPlaneAnormal1 = transA.getBasis() * btVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); - btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0); - btVector3 worldA1 = transA*a; + btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1); //.cross(WorldEdge0); + btVector3 worldA1 = transA * a; btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1); - + //int otherFace=0; #ifdef BLA1 int otherFace = polyA.m_connectedFaces[e0]; - btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]); + btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]); btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3]; - btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; - btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); -#else + btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal; + btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin()); +#else btVector3 planeNormalWS = planeNormalWS1; - btScalar planeEqWS=planeEqWS1; - + btScalar planeEqWS = planeEqWS1; + #endif //clip face - clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); - btSwap(pVtxIn,pVtxOut); + clipFace(*pVtxIn, *pVtxOut, planeNormalWS, planeEqWS); + btSwap(pVtxIn, pVtxOut); pVtxOut->resize(0); } - - -//#define ONLY_REPORT_DEEPEST_POINT + //#define ONLY_REPORT_DEEPEST_POINT btVector3 point; - // only keep points that are behind the witness face { - btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]); + btVector3 localPlaneNormal(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]); btScalar localPlaneEq = polyA.m_plane[3]; - btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; - btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); - for (int i=0;i<pVtxIn->size();i++) + btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal; + btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin()); + for (int i = 0; i < pVtxIn->size(); i++) { btVector3 vtx = pVtxIn->at(i); - btScalar depth = planeNormalWS.dot(vtx)+planeEqWS; - if (depth <=minDist) + btScalar depth = planeNormalWS.dot(vtx) + planeEqWS; + if (depth <= minDist) { -// printf("clamped: depth=%f to minDist=%f\n",depth,minDist); + // printf("clamped: depth=%f to minDist=%f\n",depth,minDist); depth = minDist; } - if (depth <=maxDist) + if (depth <= maxDist) { btVector3 point = pVtxIn->at(i); #ifdef ONLY_REPORT_DEEPEST_POINT @@ -507,40 +495,32 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin { printf("error in btPolyhedralContactClipping depth = %f\n", depth); printf("likely wrong separatingNormal passed in\n"); - } -#endif - resultOut.addContactPoint(separatingNormal,point,depth); + } +#endif + resultOut.addContactPoint(separatingNormal, point, depth); #endif } } } #ifdef ONLY_REPORT_DEEPEST_POINT - if (curMaxDist<maxDist) + if (curMaxDist < maxDist) { - resultOut.addContactPoint(separatingNormal,point,curMaxDist); + resultOut.addContactPoint(separatingNormal, point, curMaxDist); } -#endif //ONLY_REPORT_DEEPEST_POINT - +#endif //ONLY_REPORT_DEEPEST_POINT } - - - - -void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btScalar minDist, btScalar maxDist, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, btDiscreteCollisionDetectorInterface::Result& resultOut) { - btVector3 separatingNormal = separatingNormal1.normalized(); -// const btVector3 c0 = transA * hullA.m_localCenter; -// const btVector3 c1 = transB * hullB.m_localCenter; + // const btVector3 c0 = transA * hullA.m_localCenter; + // const btVector3 c1 = transB * hullB.m_localCenter; //const btVector3 DeltaC2 = c0 - c1; - - - int closestFaceB=-1; + int closestFaceB = -1; btScalar dmax = -FLT_MAX; { - for(int face=0;face<hullB.m_faces.size();face++) + for (int face = 0; face < hullB.m_faces.size(); face++) { const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]); const btVector3 WorldNormal = transB.getBasis() * Normal; @@ -552,19 +532,17 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin } } } - btVertexArray worldVertsB1; - { - const btFace& polyB = hullB.m_faces[closestFaceB]; - const int numVertices = polyB.m_indices.size(); - for(int e0=0;e0<numVertices;e0++) - { - const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]]; - worldVertsB1.push_back(transB*b); - } - } - - - if (closestFaceB>=0) - clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut); + worldVertsB1.resize(0); + { + const btFace& polyB = hullB.m_faces[closestFaceB]; + const int numVertices = polyB.m_indices.size(); + for (int e0 = 0; e0 < numVertices; e0++) + { + const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]]; + worldVertsB1.push_back(transB * b); + } + } + if (closestFaceB >= 0) + clipFaceAgainstHull(separatingNormal, hullA, transA, worldVertsB1, worldVertsB2, minDist, maxDist, resultOut); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h index b87bd4f3245..328f6424bc8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h @@ -13,14 +13,11 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans - #ifndef BT_POLYHEDRAL_CONTACT_CLIPPING_H #define BT_POLYHEDRAL_CONTACT_CLIPPING_H - #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btTransform.h" #include "btDiscreteCollisionDetectorInterface.h" @@ -32,15 +29,14 @@ typedef btAlignedObjectArray<btVector3> btVertexArray; // Clips a face to the back of a plane struct btPolyhedralContactClipping { - static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut); - static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut); + static void clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, const btScalar minDist, btScalar maxDist, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, btDiscreteCollisionDetectorInterface::Result& resultOut); - static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut); + static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut); - ///the clipFace method is used internally - static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS); + static bool findSeparatingAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut); + ///the clipFace method is used internally + static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS); }; -#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H - +#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index 786efd18200..3d11e5bce5f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -23,39 +23,38 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "btRaycastCallback.h" -btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags) - : - m_from(from), - m_to(to), - //@BP Mod - m_flags(flags), - m_hitFraction(btScalar(1.)) +btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from, const btVector3& to, unsigned int flags) + : m_from(from), + m_to(to), + //@BP Mod + m_flags(flags), + m_hitFraction(btScalar(1.)) { - } - - -void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) +void btTriangleRaycastCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { - const btVector3 &vert0=triangle[0]; - const btVector3 &vert1=triangle[1]; - const btVector3 &vert2=triangle[2]; + const btVector3& vert0 = triangle[0]; + const btVector3& vert1 = triangle[1]; + const btVector3& vert2 = triangle[2]; + + btVector3 v10; + v10 = vert1 - vert0; + btVector3 v20; + v20 = vert2 - vert0; - btVector3 v10; v10 = vert1 - vert0 ; - btVector3 v20; v20 = vert2 - vert0 ; + btVector3 triangleNormal; + triangleNormal = v10.cross(v20); - btVector3 triangleNormal; triangleNormal = v10.cross( v20 ); - const btScalar dist = vert0.dot(triangleNormal); - btScalar dist_a = triangleNormal.dot(m_from) ; - dist_a-= dist; + btScalar dist_a = triangleNormal.dot(m_from); + dist_a -= dist; btScalar dist_b = triangleNormal.dot(m_to); dist_b -= dist; - if ( dist_a * dist_b >= btScalar(0.0) ) + if (dist_a * dist_b >= btScalar(0.0)) { - return ; // same sign + return; // same sign } if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a <= btScalar(0.0))) @@ -64,52 +63,52 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, return; } - - const btScalar proj_length=dist_a-dist_b; - const btScalar distance = (dist_a)/(proj_length); + const btScalar proj_length = dist_a - dist_b; + const btScalar distance = (dist_a) / (proj_length); // Now we have the intersection point on the plane, we'll see if it's inside the triangle // Add an epsilon as a tolerance for the raycast, // in case the ray hits exacly on the edge of the triangle. // It must be scaled for the triangle size. - - if(distance < m_hitFraction) - { - - btScalar edge_tolerance =triangleNormal.length2(); + if (distance < m_hitFraction) + { + btScalar edge_tolerance = triangleNormal.length2(); edge_tolerance *= btScalar(-0.0001); - btVector3 point; point.setInterpolate3( m_from, m_to, distance); + btVector3 point; + point.setInterpolate3(m_from, m_to, distance); { - btVector3 v0p; v0p = vert0 - point; - btVector3 v1p; v1p = vert1 - point; - btVector3 cp0; cp0 = v0p.cross( v1p ); - - if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) + btVector3 v0p; + v0p = vert0 - point; + btVector3 v1p; + v1p = vert1 - point; + btVector3 cp0; + cp0 = v0p.cross(v1p); + + if ((btScalar)(cp0.dot(triangleNormal)) >= edge_tolerance) { - - - btVector3 v2p; v2p = vert2 - point; + btVector3 v2p; + v2p = vert2 - point; btVector3 cp1; - cp1 = v1p.cross( v2p); - if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) + cp1 = v1p.cross(v2p); + if ((btScalar)(cp1.dot(triangleNormal)) >= edge_tolerance) { btVector3 cp2; cp2 = v2p.cross(v0p); - - if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) + + if ((btScalar)(cp2.dot(triangleNormal)) >= edge_tolerance) { - //@BP Mod - // Triangle normal isn't normalized - triangleNormal.normalize(); + //@BP Mod + // Triangle normal isn't normalized + triangleNormal.normalize(); - //@BP Mod - Allow for unflipped normal when raycasting against backfaces + //@BP Mod - Allow for unflipped normal when raycasting against backfaces if (((m_flags & kF_KeepUnflippedNormal) == 0) && (dist_a <= btScalar(0.0))) { - m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex); + m_hitFraction = reportHit(-triangleNormal, distance, partId, triangleIndex); } else { - m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex); + m_hitFraction = reportHit(triangleNormal, distance, partId, triangleIndex); } } } @@ -118,8 +117,7 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, } } - -btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin) +btTriangleConvexcastCallback::btTriangleConvexcastCallback(const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin) { m_convexShape = convexShape; m_convexShapeFrom = convexShapeFrom; @@ -130,14 +128,13 @@ btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* m_allowedPenetration = 0.f; } -void -btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex) +void btTriangleConvexcastCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { - btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]); - triangleShape.setMargin(m_triangleCollisionMargin); + btTriangleShape triangleShape(triangle[0], triangle[1], triangle[2]); + triangleShape.setMargin(m_triangleCollisionMargin); - btVoronoiSimplexSolver simplexSolver; - btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; + btVoronoiSimplexSolver simplexSolver; + btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; //#define USE_SUBSIMPLEX_CONVEX_CAST 1 //if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below @@ -145,21 +142,21 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver); #else //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver); - btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver); -#endif //#USE_SUBSIMPLEX_CONVEX_CAST - + btContinuousConvexCollision convexCaster(m_convexShape, &triangleShape, &simplexSolver, &gjkEpaPenetrationSolver); +#endif //#USE_SUBSIMPLEX_CONVEX_CAST + btConvexCast::CastResult castResult; castResult.m_fraction = btScalar(1.); castResult.m_allowedPenetration = m_allowedPenetration; - if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult)) + if (convexCaster.calcTimeOfImpact(m_convexShapeFrom, m_convexShapeTo, m_triangleToWorld, m_triangleToWorld, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) - { + { if (castResult.m_fraction < m_hitFraction) { -/* btContinuousConvexCast's normal is already in world space */ -/* + /* btContinuousConvexCast's normal is already in world space */ + /* #ifdef USE_SUBSIMPLEX_CONVEX_CAST //rotate normal into worldspace castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal; @@ -167,11 +164,11 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, */ castResult.m_normal.normalize(); - reportHit (castResult.m_normal, - castResult.m_hitPoint, - castResult.m_fraction, - partId, - triangleIndex); + reportHit(castResult.m_normal, + castResult.m_hitPoint, + castResult.m_fraction, + partId, + triangleIndex); } } } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index f2ed0cd39c4..2d0df718a23 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -21,35 +21,34 @@ subject to the following restrictions: struct btBroadphaseProxy; class btConvexShape; -class btTriangleRaycastCallback: public btTriangleCallback +class btTriangleRaycastCallback : public btTriangleCallback { public: - //input btVector3 m_from; btVector3 m_to; - //@BP Mod - allow backface filtering and unflipped normals - enum EFlags - { - kF_None = 0, - kF_FilterBackfaces = 1 << 0, - kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle - ///SubSimplexConvexCastRaytest is the default, even if kF_None is set. - kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm - kF_UseGjkConvexCastRaytest = 1 << 3, - kF_Terminator = 0xFFFFFFFF - }; - unsigned int m_flags; + //@BP Mod - allow backface filtering and unflipped normals + enum EFlags + { + kF_None = 0, + kF_FilterBackfaces = 1 << 0, + kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle + ///SubSimplexConvexCastRaytest is the default, even if kF_None is set. + kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm + kF_UseGjkConvexCastRaytest = 1 << 3, + kF_DisableHeightfieldAccelerator = 1 << 4, //don't use the heightfield raycast accelerator. See https://github.com/bulletphysics/bullet3/pull/2062 + kF_Terminator = 0xFFFFFFFF + }; + unsigned int m_flags; + + btScalar m_hitFraction; - btScalar m_hitFraction; + btTriangleRaycastCallback(const btVector3& from, const btVector3& to, unsigned int flags = 0); - btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0); - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0; - + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; }; class btTriangleConvexcastCallback : public btTriangleCallback @@ -63,12 +62,11 @@ public: btScalar m_triangleCollisionMargin; btScalar m_allowedPenetration; - btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin); + btTriangleConvexcastCallback(const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin); - virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex); + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; }; -#endif //BT_RAYCAST_TRI_CALLBACK_H - +#endif //BT_RAYCAST_TRI_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h index da8a13914c9..ccd227109d2 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_SIMPLEX_SOLVER_INTERFACE_H #define BT_SIMPLEX_SOLVER_INTERFACE_H @@ -31,33 +29,30 @@ subject to the following restrictions: /// voronoi regions or barycentric coordinates class btSimplexSolverInterface { - public: - virtual ~btSimplexSolverInterface() {}; +public: + virtual ~btSimplexSolverInterface(){}; virtual void reset() = 0; virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0; - + virtual bool closest(btVector3& v) = 0; virtual btScalar maxVertex() = 0; virtual bool fullSimplex() const = 0; - virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0; + virtual int getSimplex(btVector3* pBuf, btVector3* qBuf, btVector3* yBuf) const = 0; virtual bool inSimplex(const btVector3& w) = 0; - + virtual void backup_closest(btVector3& v) = 0; virtual bool emptySimplex() const = 0; virtual void compute_points(btVector3& p1, btVector3& p2) = 0; - virtual int numVertices() const =0; - - + virtual int numVertices() const = 0; }; #endif -#endif //BT_SIMPLEX_SOLVER_INTERFACE_H - +#endif //BT_SIMPLEX_SOLVER_INTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index ec638f60ba5..37458339e70 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSubSimplexConvexCast.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" @@ -22,32 +21,26 @@ subject to the following restrictions: #include "btPointCollector.h" #include "LinearMath/btTransformUtil.h" -btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) -:m_simplexSolver(simplexSolver), -m_convexA(convexA),m_convexB(convexB) +btSubsimplexConvexCast::btSubsimplexConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver) + : m_simplexSolver(simplexSolver), + m_convexA(convexA), + m_convexB(convexB) { } -///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. -///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 -#ifdef BT_USE_DOUBLE_PRECISION -#define MAX_ITERATIONS 64 -#else -#define MAX_ITERATIONS 32 -#endif -bool btSubsimplexConvexCast::calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) -{ +bool btSubsimplexConvexCast::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) +{ m_simplexSolver->reset(); - btVector3 linVelA,linVelB; - linVelA = toA.getOrigin()-fromA.getOrigin(); - linVelB = toB.getOrigin()-fromB.getOrigin(); + btVector3 linVelA, linVelB; + linVelA = toA.getOrigin() - fromA.getOrigin(); + linVelB = toB.getOrigin() - fromB.getOrigin(); btScalar lambda = btScalar(0.); @@ -55,36 +48,31 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( btTransform interpolatedTransB = fromB; ///take relative motion - btVector3 r = (linVelA-linVelB); + btVector3 r = (linVelA - linVelB); btVector3 v; - - btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis())); - btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis())); - v = supVertexA-supVertexB; - int maxIter = MAX_ITERATIONS; + + btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r * fromA.getBasis())); + btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r * fromB.getBasis())); + v = supVertexA - supVertexB; + int maxIter = result.m_subSimplexCastMaxIterations; btVector3 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - + n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 c; - + btScalar dist2 = v.length2(); - btScalar dist2 = v.length2(); -#ifdef BT_USE_DOUBLE_PRECISION - btScalar epsilon = btScalar(0.0001); -#else - btScalar epsilon = btScalar(0.0001); -#endif //BT_USE_DOUBLE_PRECISION - btVector3 w,p; + + btVector3 w, p; btScalar VdotR; - - while ( (dist2 > epsilon) && maxIter--) + + while ((dist2 > result.m_subSimplexCastEpsilon) && maxIter--) { - supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis())); - supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis())); - w = supVertexA-supVertexB; + supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v * interpolatedTransA.getBasis())); + supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v * interpolatedTransB.getBasis())); + w = supVertexA - supVertexB; btScalar VdotW = v.dot(w); @@ -93,68 +81,63 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( return false; } - if ( VdotW > btScalar(0.)) + if (VdotW > btScalar(0.)) { VdotR = v.dot(r); - if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON)) + if (VdotR >= -(SIMD_EPSILON * SIMD_EPSILON)) return false; else { lambda = lambda - VdotW / VdotR; //interpolate to next lambda // x = s + lambda * r; - interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); - interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); + interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(), toA.getOrigin(), lambda); + interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda); //m_simplexSolver->reset(); //check next line - w = supVertexA-supVertexB; - + w = supVertexA - supVertexB; + n = v; - } - } + } ///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc. if (!m_simplexSolver->inSimplex(w)) - m_simplexSolver->addVertex( w, supVertexA , supVertexB); + m_simplexSolver->addVertex(w, supVertexA, supVertexB); if (m_simplexSolver->closest(v)) { dist2 = v.length2(); - + //todo: check this normal for validity //n=v; //printf("V=%f , %f, %f\n",v[0],v[1],v[2]); //printf("DIST2=%f\n",dist2); //printf("numverts = %i\n",m_simplexSolver->numVertices()); - } else + } + else { dist2 = btScalar(0.); - } + } } //int numiter = MAX_ITERATIONS - maxIter; -// printf("number of iterations: %d", numiter); - + // printf("number of iterations: %d", numiter); + //don't report a time of impact when moving 'away' from the hitnormal - result.m_fraction = lambda; - if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON)) + if (n.length2() >= (SIMD_EPSILON * SIMD_EPSILON)) result.m_normal = n.normalized(); else result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0)); //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (result.m_normal.dot(r)>=-result.m_allowedPenetration) + if (result.m_normal.dot(r) >= -result.m_allowedPenetration) return false; - btVector3 hitA,hitB; - m_simplexSolver->compute_points(hitA,hitB); - result.m_hitPoint=hitB; + btVector3 hitA, hitB; + m_simplexSolver->compute_points(hitA, hitB); + result.m_hitPoint = hitB; return true; } - - - - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h index 6c8127983eb..0638a30eb1a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_SUBSIMPLEX_CONVEX_CAST_H #define BT_SUBSIMPLEX_CONVEX_CAST_H @@ -28,23 +27,21 @@ class btConvexShape; class btSubsimplexConvexCast : public btConvexCast { btSimplexSolverInterface* m_simplexSolver; - const btConvexShape* m_convexA; - const btConvexShape* m_convexB; + const btConvexShape* m_convexA; + const btConvexShape* m_convexB; public: - - btSubsimplexConvexCast (const btConvexShape* shapeA,const btConvexShape* shapeB,btSimplexSolverInterface* simplexSolver); + btSubsimplexConvexCast(const btConvexShape* shapeA, const btConvexShape* shapeB, btSimplexSolverInterface* simplexSolver); //virtual ~btSubsimplexConvexCast(); ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector. - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result); - + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); }; -#endif //BT_SUBSIMPLEX_CONVEX_CAST_H +#endif //BT_SUBSIMPLEX_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index 23b4f79cfc2..8fda94d2adf 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -23,26 +23,24 @@ subject to the following restrictions: */ - #include "btVoronoiSimplexSolver.h" -#define VERTA 0 -#define VERTB 1 -#define VERTC 2 -#define VERTD 3 +#define VERTA 0 +#define VERTB 1 +#define VERTC 2 +#define VERTD 3 #define CATCH_DEGENERATE_TETRAHEDRON 1 -void btVoronoiSimplexSolver::removeVertex(int index) +void btVoronoiSimplexSolver::removeVertex(int index) { - - btAssert(m_numVertices>0); + btAssert(m_numVertices > 0); m_numVertices--; m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices]; } -void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts) +void btVoronoiSimplexSolver::reduceVertices(const btUsageBitfield& usedVerts) { if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) removeVertex(3); @@ -52,29 +50,22 @@ void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts) if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) removeVertex(1); - + if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) removeVertex(0); - } - - - - //clear the simplex, remove all the vertices void btVoronoiSimplexSolver::reset() { m_cachedValidClosest = false; m_numVertices = 0; m_needsUpdate = true; - m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + m_lastW = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); m_cachedBC.reset(); } - - - //add a vertex +//add a vertex void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q) { m_lastW = w; @@ -87,9 +78,8 @@ void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, c m_numVertices++; } -bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() +bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() { - if (m_needsUpdate) { m_cachedBC.reset(); @@ -98,127 +88,131 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() switch (numVertices()) { - case 0: + case 0: m_cachedValidClosest = false; break; - case 1: + case 1: { m_cachedP1 = m_simplexPointsP[0]; m_cachedP2 = m_simplexPointsQ[0]; - m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] + m_cachedV = m_cachedP1 - m_cachedP2; //== m_simplexVectorW[0] m_cachedBC.reset(); - m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); + m_cachedBC.setBarycentricCoordinates(btScalar(1.), btScalar(0.), btScalar(0.), btScalar(0.)); m_cachedValidClosest = m_cachedBC.isValid(); break; }; - case 2: + case 2: { - //closest point origin from line segment - const btVector3& from = m_simplexVectorW[0]; - const btVector3& to = m_simplexVectorW[1]; - btVector3 nearest; - - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 diff = p - from; - btVector3 v = to - from; - btScalar t = v.dot(diff); - - if (t > 0) { - btScalar dotVV = v.dot(v); - if (t < dotVV) { - t /= dotVV; - diff -= t*v; - m_cachedBC.m_usedVertices.usedVertexA = true; - m_cachedBC.m_usedVertices.usedVertexB = true; - } else { - t = 1; - diff -= v; - //reduce to 1 point - m_cachedBC.m_usedVertices.usedVertexB = true; - } - } else + //closest point origin from line segment + const btVector3& from = m_simplexVectorW[0]; + const btVector3& to = m_simplexVectorW[1]; + btVector3 nearest; + + btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 diff = p - from; + btVector3 v = to - from; + btScalar t = v.dot(diff); + + if (t > 0) + { + btScalar dotVV = v.dot(v); + if (t < dotVV) { - t = 0; - //reduce to 1 point + t /= dotVV; + diff -= t * v; m_cachedBC.m_usedVertices.usedVertexA = true; + m_cachedBC.m_usedVertices.usedVertexB = true; + } + else + { + t = 1; + diff -= v; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexB = true; } - m_cachedBC.setBarycentricCoordinates(1-t,t); - nearest = from + t*v; + } + else + { + t = 0; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexA = true; + } + m_cachedBC.setBarycentricCoordinates(1 - t, t); + nearest = from + t * v; - m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); - m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); - m_cachedV = m_cachedP1 - m_cachedP2; - - reduceVertices(m_cachedBC.m_usedVertices); + m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); + m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); + m_cachedV = m_cachedP1 - m_cachedP2; - m_cachedValidClosest = m_cachedBC.isValid(); - break; + reduceVertices(m_cachedBC.m_usedVertices); + + m_cachedValidClosest = m_cachedBC.isValid(); + break; } - case 3: - { - //closest point origin from triangle - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); + case 3: + { + //closest point origin from triangle + btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.)); - const btVector3& a = m_simplexVectorW[0]; - const btVector3& b = m_simplexVectorW[1]; - const btVector3& c = m_simplexVectorW[2]; + const btVector3& a = m_simplexVectorW[0]; + const btVector3& b = m_simplexVectorW[1]; + const btVector3& c = m_simplexVectorW[2]; - closestPtPointTriangle(p,a,b,c,m_cachedBC); - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; + closestPtPointTriangle(p, a, b, c, m_cachedBC); + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; - m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; - m_cachedV = m_cachedP1-m_cachedP2; + m_cachedV = m_cachedP1 - m_cachedP2; - reduceVertices (m_cachedBC.m_usedVertices); - m_cachedValidClosest = m_cachedBC.isValid(); + reduceVertices(m_cachedBC.m_usedVertices); + m_cachedValidClosest = m_cachedBC.isValid(); - break; + break; } - case 4: + case 4: { + btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.)); - - btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); - const btVector3& a = m_simplexVectorW[0]; const btVector3& b = m_simplexVectorW[1]; const btVector3& c = m_simplexVectorW[2]; const btVector3& d = m_simplexVectorW[3]; - bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); + bool hasSeparation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC); - if (hasSeperation) + if (hasSeparation) { - m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + - m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + - m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + - m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; - m_cachedV = m_cachedP1-m_cachedP2; - reduceVertices (m_cachedBC.m_usedVertices); - } else + m_cachedV = m_cachedP1 - m_cachedP2; + reduceVertices(m_cachedBC.m_usedVertices); + } + else { -// printf("sub distance got penetration\n"); + // printf("sub distance got penetration\n"); if (m_cachedBC.m_degenerate) { m_cachedValidClosest = false; - } else + } + else { m_cachedValidClosest = true; //degenerate case == false, penetration = true + zero - m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_cachedV.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } break; } @@ -228,7 +222,7 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() //closest point origin from tetrahedron break; } - default: + default: { m_cachedValidClosest = false; } @@ -236,7 +230,6 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() } return m_cachedValidClosest; - } //return/calculate the closest vertex @@ -247,13 +240,11 @@ bool btVoronoiSimplexSolver::closest(btVector3& v) return succes; } - - btScalar btVoronoiSimplexSolver::maxVertex() { int i, numverts = numVertices(); btScalar maxV = btScalar(0.); - for (i=0;i<numverts;i++) + for (i = 0; i < numverts; i++) { btScalar curLen2 = m_simplexVectorW[i].length2(); if (maxV < curLen2) @@ -262,13 +253,11 @@ btScalar btVoronoiSimplexSolver::maxVertex() return maxV; } - - - //return the current simplex -int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const +//return the current simplex +int btVoronoiSimplexSolver::getSimplex(btVector3* pBuf, btVector3* qBuf, btVector3* yBuf) const { int i; - for (i=0;i<numVertices();i++) + for (i = 0; i < numVertices(); i++) { yBuf[i] = m_simplexVectorW[i]; pBuf[i] = m_simplexPointsP[i]; @@ -277,20 +266,17 @@ int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVecto return numVertices(); } - - - bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) { bool found = false; int i, numverts = numVertices(); //btScalar maxV = btScalar(0.); - + //w is in the current (reduced) simplex - for (i=0;i<numverts;i++) + for (i = 0; i < numverts; i++) { #ifdef BT_USE_EQUAL_VERTEX_THRESHOLD - if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold) + if (m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold) #else if (m_simplexVectorW[i] == w) #endif @@ -303,199 +289,190 @@ bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) //check in case lastW is already removed if (w == m_lastW) return true; - + return found; } -void btVoronoiSimplexSolver::backup_closest(btVector3& v) +void btVoronoiSimplexSolver::backup_closest(btVector3& v) { v = m_cachedV; } - -bool btVoronoiSimplexSolver::emptySimplex() const +bool btVoronoiSimplexSolver::emptySimplex() const { return (numVertices() == 0); - } -void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) +void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) { updateClosestVectorAndPoints(); p1 = m_cachedP1; p2 = m_cachedP2; - } - - - -bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result) +bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btSubSimplexClosestResult& result) { result.m_usedVertices.reset(); - // Check if P in vertex region outside A - btVector3 ab = b - a; - btVector3 ac = c - a; - btVector3 ap = p - a; - btScalar d1 = ab.dot(ap); - btScalar d2 = ac.dot(ap); - if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) + // Check if P in vertex region outside A + btVector3 ab = b - a; + btVector3 ac = c - a; + btVector3 ap = p - a; + btScalar d1 = ab.dot(ap); + btScalar d2 = ac.dot(ap); + if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) { result.m_closestPointOnSimplex = a; result.m_usedVertices.usedVertexA = true; - result.setBarycentricCoordinates(1,0,0); - return true;// a; // barycentric coordinates (1,0,0) + result.setBarycentricCoordinates(1, 0, 0); + return true; // a; // barycentric coordinates (1,0,0) } - // Check if P in vertex region outside B - btVector3 bp = p - b; - btScalar d3 = ab.dot(bp); - btScalar d4 = ac.dot(bp); - if (d3 >= btScalar(0.0) && d4 <= d3) + // Check if P in vertex region outside B + btVector3 bp = p - b; + btScalar d3 = ab.dot(bp); + btScalar d4 = ac.dot(bp); + if (d3 >= btScalar(0.0) && d4 <= d3) { result.m_closestPointOnSimplex = b; result.m_usedVertices.usedVertexB = true; - result.setBarycentricCoordinates(0,1,0); + result.setBarycentricCoordinates(0, 1, 0); - return true; // b; // barycentric coordinates (0,1,0) + return true; // b; // barycentric coordinates (0,1,0) } - // Check if P in edge region of AB, if so return projection of P onto AB - btScalar vc = d1*d4 - d3*d2; - if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { - btScalar v = d1 / (d1 - d3); + // Check if P in edge region of AB, if so return projection of P onto AB + btScalar vc = d1 * d4 - d3 * d2; + if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) + { + btScalar v = d1 / (d1 - d3); result.m_closestPointOnSimplex = a + v * ab; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; - result.setBarycentricCoordinates(1-v,v,0); + result.setBarycentricCoordinates(1 - v, v, 0); return true; - //return a + v * ab; // barycentric coordinates (1-v,v,0) - } - - // Check if P in vertex region outside C - btVector3 cp = p - c; - btScalar d5 = ab.dot(cp); - btScalar d6 = ac.dot(cp); - if (d6 >= btScalar(0.0) && d5 <= d6) + //return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + btVector3 cp = p - c; + btScalar d5 = ab.dot(cp); + btScalar d6 = ac.dot(cp); + if (d6 >= btScalar(0.0) && d5 <= d6) { result.m_closestPointOnSimplex = c; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(0,0,1); - return true;//c; // barycentric coordinates (0,0,1) + result.setBarycentricCoordinates(0, 0, 1); + return true; //c; // barycentric coordinates (0,0,1) } - // Check if P in edge region of AC, if so return projection of P onto AC - btScalar vb = d5*d2 - d1*d6; - if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { - btScalar w = d2 / (d2 - d6); + // Check if P in edge region of AC, if so return projection of P onto AC + btScalar vb = d5 * d2 - d1 * d6; + if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) + { + btScalar w = d2 / (d2 - d6); result.m_closestPointOnSimplex = a + w * ac; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(1-w,0,w); + result.setBarycentricCoordinates(1 - w, 0, w); return true; - //return a + w * ac; // barycentric coordinates (1-w,0,w) - } + //return a + w * ac; // barycentric coordinates (1-w,0,w) + } + + // Check if P in edge region of BC, if so return projection of P onto BC + btScalar va = d3 * d6 - d5 * d4; + if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) + { + btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - // Check if P in edge region of BC, if so return projection of P onto BC - btScalar va = d3*d6 - d5*d4; - if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { - btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - result.m_closestPointOnSimplex = b + w * (c - b); result.m_usedVertices.usedVertexB = true; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(0,1-w,w); - return true; - // return b + w * (c - b); // barycentric coordinates (0,1-w,w) - } - - // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - btScalar denom = btScalar(1.0) / (va + vb + vc); - btScalar v = vb * denom; - btScalar w = vc * denom; - + result.setBarycentricCoordinates(0, 1 - w, w); + return true; + // return b + w * (c - b); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + btScalar denom = btScalar(1.0) / (va + vb + vc); + btScalar v = vb * denom; + btScalar w = vc * denom; + result.m_closestPointOnSimplex = a + ab * v + ac * w; result.m_usedVertices.usedVertexA = true; result.m_usedVertices.usedVertexB = true; result.m_usedVertices.usedVertexC = true; - result.setBarycentricCoordinates(1-v-w,v,w); - - return true; -// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w + result.setBarycentricCoordinates(1 - v - w, v, w); + return true; + // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w } - - - - /// Test if point p and d lie on opposite sides of plane through abc int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d) { - btVector3 normal = (b-a).cross(c-a); + btVector3 normal = (b - a).cross(c - a); - btScalar signp = (p - a).dot(normal); // [AP AB AC] - btScalar signd = (d - a).dot( normal); // [AD AB AC] + btScalar signp = (p - a).dot(normal); // [AP AB AC] + btScalar signd = (d - a).dot(normal); // [AD AB AC] #ifdef CATCH_DEGENERATE_TETRAHEDRON #ifdef BT_USE_DOUBLE_PRECISION -if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) + if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) { return -1; } #else if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) { -// printf("affine dependent/degenerate\n");// + // printf("affine dependent/degenerate\n");// return -1; } #endif #endif // Points on opposite sides if expression signs are opposite - return signp * signd < btScalar(0.); + return signp * signd < btScalar(0.); } - -bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult) +bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult) { btSubSimplexClosestResult tempResult; - // Start out assuming point inside all halfspaces, so closest to itself + // Start out assuming point inside all halfspaces, so closest to itself finalResult.m_closestPointOnSimplex = p; finalResult.m_usedVertices.reset(); - finalResult.m_usedVertices.usedVertexA = true; + finalResult.m_usedVertices.usedVertexA = true; finalResult.m_usedVertices.usedVertexB = true; finalResult.m_usedVertices.usedVertexC = true; finalResult.m_usedVertices.usedVertexD = true; - int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); + int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); - int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); - int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); - - if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) - { - finalResult.m_degenerate = true; - return false; - } + int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); + int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); - if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) - { - return false; - } + if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) + { + finalResult.m_degenerate = true; + return false; + } + if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) + { + return false; + } - btScalar bestSqDist = FLT_MAX; - // If point outside face abc then compute closest point on abc - if (pointOutsideABC) + btScalar bestSqDist = FLT_MAX; + // If point outside face abc then compute closest point on abc + if (pointOutsideABC) { - closestPtPointTriangle(p, a, b, c,tempResult); + closestPtPointTriangle(p, a, b, c, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; - - btScalar sqDist = (q - p).dot( q - p); - // Update best closest point if (squared) distance is less than current best - if (sqDist < bestSqDist) { + + btScalar sqDist = (q - p).dot(q - p); + // Update best closest point if (squared) distance is less than current best + if (sqDist < bestSqDist) + { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! @@ -504,25 +481,22 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTB], - tempResult.m_barycentricCoords[VERTC], - 0 - ); - + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC], + 0); } - } - + } // Repeat test for face acd - if (pointOutsideACD) + if (pointOutsideACD) { - closestPtPointTriangle(p, a, c, d,tempResult); + closestPtPointTriangle(p, a, c, d, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) + btScalar sqDist = (q - p).dot(q - p); + if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; @@ -532,52 +506,46 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - 0, - tempResult.m_barycentricCoords[VERTB], - tempResult.m_barycentricCoords[VERTC] - ); - + tempResult.m_barycentricCoords[VERTA], + 0, + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC]); } - } - // Repeat test for face adb + } + // Repeat test for face adb - if (pointOutsideADB) { - closestPtPointTriangle(p, a, d, b,tempResult); + closestPtPointTriangle(p, a, d, b, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) + btScalar sqDist = (q - p).dot(q - p); + if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; - + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTC], - 0, - tempResult.m_barycentricCoords[VERTB] - ); - + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + 0, + tempResult.m_barycentricCoords[VERTB]); } - } - // Repeat test for face bdc - + } + // Repeat test for face bdc if (pointOutsideBDC) { - closestPtPointTriangle(p, b, d, c,tempResult); + closestPtPointTriangle(p, b, d, c, tempResult); btVector3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! - btScalar sqDist = (q - p).dot( q - p); - if (sqDist < bestSqDist) + btScalar sqDist = (q - p).dot(q - p); + if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; @@ -588,25 +556,22 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.setBarycentricCoordinates( - 0, - tempResult.m_barycentricCoords[VERTA], - tempResult.m_barycentricCoords[VERTC], - tempResult.m_barycentricCoords[VERTB] - ); - + 0, + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + tempResult.m_barycentricCoords[VERTB]); } - } + } //help! we ended up full ! - + if (finalResult.m_usedVertices.usedVertexA && finalResult.m_usedVertices.usedVertexB && finalResult.m_usedVertices.usedVertexC && - finalResult.m_usedVertices.usedVertexD) + finalResult.m_usedVertices.usedVertexD) { return true; } - return true; + return true; } - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index 2f389e27e3f..24a0a8f2df8 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -13,23 +13,24 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #ifndef BT_VORONOI_SIMPLEX_SOLVER_H #define BT_VORONOI_SIMPLEX_SOLVER_H #include "btSimplexSolverInterface.h" - - #define VORONOI_SIMPLEX_MAX_VERTS 5 ///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure #define BT_USE_EQUAL_VERTEX_THRESHOLD -#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f +#ifdef BT_USE_DOUBLE_PRECISION +#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 1e-12f +#else +#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f +#endif //BT_USE_DOUBLE_PRECISION -struct btUsageBitfield{ +struct btUsageBitfield +{ btUsageBitfield() { reset(); @@ -42,140 +43,131 @@ struct btUsageBitfield{ usedVertexC = false; usedVertexD = false; } - unsigned short usedVertexA : 1; - unsigned short usedVertexB : 1; - unsigned short usedVertexC : 1; - unsigned short usedVertexD : 1; - unsigned short unused1 : 1; - unsigned short unused2 : 1; - unsigned short unused3 : 1; - unsigned short unused4 : 1; + unsigned short usedVertexA : 1; + unsigned short usedVertexB : 1; + unsigned short usedVertexC : 1; + unsigned short usedVertexD : 1; + unsigned short unused1 : 1; + unsigned short unused2 : 1; + unsigned short unused3 : 1; + unsigned short unused4 : 1; }; - -struct btSubSimplexClosestResult +struct btSubSimplexClosestResult { - btVector3 m_closestPointOnSimplex; + btVector3 m_closestPointOnSimplex; //MASK for m_usedVertices - //stores the simplex vertex-usage, using the MASK, + //stores the simplex vertex-usage, using the MASK, // if m_usedVertices & MASK then the related vertex is used - btUsageBitfield m_usedVertices; - btScalar m_barycentricCoords[4]; + btUsageBitfield m_usedVertices; + btScalar m_barycentricCoords[4]; bool m_degenerate; - void reset() + void reset() { m_degenerate = false; setBarycentricCoordinates(); m_usedVertices.reset(); } - bool isValid() + bool isValid() { bool valid = (m_barycentricCoords[0] >= btScalar(0.)) && - (m_barycentricCoords[1] >= btScalar(0.)) && - (m_barycentricCoords[2] >= btScalar(0.)) && - (m_barycentricCoords[3] >= btScalar(0.)); - + (m_barycentricCoords[1] >= btScalar(0.)) && + (m_barycentricCoords[2] >= btScalar(0.)) && + (m_barycentricCoords[3] >= btScalar(0.)); return valid; } - void setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.)) + void setBarycentricCoordinates(btScalar a = btScalar(0.), btScalar b = btScalar(0.), btScalar c = btScalar(0.), btScalar d = btScalar(0.)) { m_barycentricCoords[0] = a; m_barycentricCoords[1] = b; m_barycentricCoords[2] = c; m_barycentricCoords[3] = d; } - }; /// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin. /// Can be used with GJK, as an alternative to Johnson distance algorithm. #ifdef NO_VIRTUAL_INTERFACE -ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver +ATTRIBUTE_ALIGNED16(class) +btVoronoiSimplexSolver #else -ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver : public btSimplexSolverInterface +ATTRIBUTE_ALIGNED16(class) +btVoronoiSimplexSolver : public btSimplexSolverInterface #endif { public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - int m_numVertices; + int m_numVertices; - btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; - btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; - btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; + btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; - - - btVector3 m_cachedP1; - btVector3 m_cachedP2; - btVector3 m_cachedV; - btVector3 m_lastW; - - btScalar m_equalVertexThreshold; - bool m_cachedValidClosest; + btVector3 m_cachedP1; + btVector3 m_cachedP2; + btVector3 m_cachedV; + btVector3 m_lastW; + btScalar m_equalVertexThreshold; + bool m_cachedValidClosest; btSubSimplexClosestResult m_cachedBC; - bool m_needsUpdate; - - void removeVertex(int index); - void reduceVertices (const btUsageBitfield& usedVerts); - bool updateClosestVectorAndPoints(); + bool m_needsUpdate; - bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult); - int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); - bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result); + void removeVertex(int index); + void reduceVertices(const btUsageBitfield& usedVerts); + bool updateClosestVectorAndPoints(); -public: + bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult); + int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d); + bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btSubSimplexClosestResult& result); +public: btVoronoiSimplexSolver() - : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) + : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) { } - void reset(); - - void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); + void reset(); - void setEqualVertexThreshold(btScalar threshold) - { - m_equalVertexThreshold = threshold; - } + void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); - btScalar getEqualVertexThreshold() const - { - return m_equalVertexThreshold; - } + void setEqualVertexThreshold(btScalar threshold) + { + m_equalVertexThreshold = threshold; + } - bool closest(btVector3& v); + btScalar getEqualVertexThreshold() const + { + return m_equalVertexThreshold; + } - btScalar maxVertex(); + bool closest(btVector3 & v); - bool fullSimplex() const - { - return (m_numVertices == 4); - } + btScalar maxVertex(); - int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; + bool fullSimplex() const + { + return (m_numVertices == 4); + } - bool inSimplex(const btVector3& w); - - void backup_closest(btVector3& v) ; + int getSimplex(btVector3 * pBuf, btVector3 * qBuf, btVector3 * yBuf) const; - bool emptySimplex() const ; + bool inSimplex(const btVector3& w); - void compute_points(btVector3& p1, btVector3& p2) ; + void backup_closest(btVector3 & v); - int numVertices() const - { - return m_numVertices; - } + bool emptySimplex() const; + void compute_points(btVector3 & p1, btVector3 & p2); + int numVertices() const + { + return m_numVertices; + } }; -#endif //BT_VORONOI_SIMPLEX_SOLVER_H - +#endif //BT_VORONOI_SIMPLEX_SOLVER_H |