diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision')
33 files changed, 8067 insertions, 0 deletions
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h new file mode 100644 index 0000000000..9eb880b8df --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h @@ -0,0 +1,369 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#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 "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)) + + { + // 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) +{ + + 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(); +#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; + } + + 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; +} + + + + +#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp new file mode 100644 index 0000000000..940282f576 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -0,0 +1,242 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btContinuousConvexCollision.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" +#include "LinearMath/btTransformUtil.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + +#include "btGjkPairDetector.h" +#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 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) +{ + 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::ClosestPointInput input; + input.m_transformA = transA; + input.m_transformB = transB; + 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; + btTransform planeInConvex; + 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 vtxInPlaneWorld = transB * vtxInPlaneProjected; + btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal; + + pointCollector.addContactPoint( + normalOnSurfaceB, + vtxInPlaneWorld, + distance); + } +} + +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); + + + btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); + 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; + + + + btScalar lambda = btScalar(0.); + btVector3 v(1,0,0); + + int maxIter = MAX_ITERATIONS; + + btVector3 n; + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + bool hasResult = false; + btVector3 c; + + btScalar lastLambda = lambda; + //btScalar epsilon = btScalar(0.001); + + int numIter = 0; + //first solution, using GJK + + + btScalar radius = 0.001f; +// result.drawCoordSystem(sphereTr); + + btPointCollector pointCollector1; + + { + + computeClosestPoints(fromA,fromB,pointCollector1); + + hasResult = pointCollector1.m_hasResult; + c = pointCollector1.m_pointInWorld; + } + + if (hasResult) + { + btScalar dist; + dist = pointCollector1.m_distance + result.m_allowedPenetration; + n = pointCollector1.m_normalOnBInWorld; + btScalar projectedLinearVelocity = relLinVel.dot(n); + if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) + return false; + + //not close enough + while (dist > radius) + { + if (result.m_debugDrawer) + { + 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) + return false; + + dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); + + + + lambda = lambda + dLambda; + + if (lambda > btScalar(1.)) + return false; + + if (lambda < btScalar(0.)) + return false; + + + //todo: next check with relative epsilon + if (lambda <= lastLambda) + { + return false; + //n.setValue(0,0,0); + break; + } + lastLambda = lambda; + + + + //interpolate to next lambda + btTransform interpolatedTransA,interpolatedTransB,relativeTrans; + + 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.DebugDraw( lambda ); + + btPointCollector pointCollector; + computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector); + + if (pointCollector.m_hasResult) + { + dist = pointCollector.m_distance+result.m_allowedPenetration; + c = pointCollector.m_pointInWorld; + n = pointCollector.m_normalOnBInWorld; + } else + { + result.reportFailure(-1, numIter); + return false; + } + + numIter++; + if (numIter > maxIter) + { + result.reportFailure(-2, numIter); + return false; + } + } + + result.m_fraction = lambda; + result.m_normal = n; + result.m_hitPoint = c; + return true; + } + + return false; + +} + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h new file mode 100644 index 0000000000..bdc0572f75 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h @@ -0,0 +1,59 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef BT_CONTINUOUS_COLLISION_CONVEX_CAST_H +#define BT_CONTINUOUS_COLLISION_CONVEX_CAST_H + +#include "btConvexCast.h" +#include "btSimplexSolverInterface.h" +class btConvexPenetrationDepthSolver; +class btConvexShape; +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. +/// 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; + //second object is either a convex or a plane (code sharing) + const btConvexShape* m_convexB1; + const btStaticPlaneShape* m_planeShape; + + 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 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 + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp new file mode 100644 index 0000000000..d2a1310b23 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp @@ -0,0 +1,20 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btConvexCast.h" + +btConvexCast::~btConvexCast() +{ +} diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h new file mode 100644 index 0000000000..bfd79d03be --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -0,0 +1,73 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef BT_CONVEX_CAST_H +#define BT_CONVEX_CAST_H + +#include "LinearMath/btTransform.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btScalar.h" +class btMinkowskiSumShape; +#include "LinearMath/btIDebugDraw.h" + +/// 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 + { + //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;} + CastResult() + :m_fraction(btScalar(BT_LARGE_FLOAT)), + m_debugDrawer(0), + m_allowedPenetration(btScalar(0)) + { + } + + + virtual ~CastResult() {}; + + btTransform m_hitTransformA; + btTransform m_hitTransformB; + btVector3 m_normal; + btVector3 m_hitPoint; + btScalar m_fraction; //input and output + btIDebugDraw* m_debugDrawer; + btScalar m_allowedPenetration; + + }; + + + /// 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; +}; + +#endif //BT_CONVEX_CAST_H diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h new file mode 100644 index 0000000000..29620abffb --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -0,0 +1,40 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef BT_CONVEX_PENETRATION_DEPTH_H +#define BT_CONVEX_PENETRATION_DEPTH_H + +class btVector3; +#include "btSimplexSolverInterface.h" +class btConvexShape; +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; + + +}; +#endif //BT_CONVEX_PENETRATION_DEPTH_H + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h new file mode 100644 index 0000000000..0ea7b483cf --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -0,0 +1,90 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H +#define BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H + +#include "LinearMath/btTransform.h" +#include "LinearMath/btVector3.h" + +/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations +/// This interface allows to query for closest points and penetration depth between two (convex) objects +/// the closest point is on the second object (B), and the normal points from the surface on B towards A. +/// distance is between closest points on B and closest point on A. So you can calculate closest point on A +/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB +struct btDiscreteCollisionDetectorInterface +{ + + struct 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; + }; + + struct ClosestPointInput + { + ClosestPointInput() + :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)) + { + } + + btTransform m_transformA; + btTransform m_transformB; + btScalar m_maximumDistanceSquared; + }; + + 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; + +}; + +struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result +{ + btVector3 m_normalOnSurfaceB; + btVector3 m_closestPointInB; + btScalar m_distance; //negative means penetration ! + + protected: + btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) + { + } + + public: + virtual ~btStorageResult() {}; + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + if (depth < m_distance) + { + m_normalOnSurfaceB = normalOnBInWorld; + m_closestPointInB = pointInWorld; + m_distance = depth; + } + } +}; + +#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h new file mode 100644 index 0000000000..0b49b0ecc6 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h @@ -0,0 +1,41 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef GJK_COLLISION_DESCRIPTION_H +#define GJK_COLLISION_DESCRIPTION_H + +#include "LinearMath/btVector3.h" + +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() + { + } +}; + +#endif //GJK_COLLISION_DESCRIPTION_H + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp new file mode 100644 index 0000000000..bef697a0a1 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -0,0 +1,176 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "btGjkConvexCast.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "btGjkPairDetector.h" +#include "btPointCollector.h" +#include "LinearMath/btTransformUtil.h" + +#ifdef BT_USE_DOUBLE_PRECISION +#define MAX_ITERATIONS 64 +#else +#define MAX_ITERATIONS 32 +#endif + +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) +{ + + + 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(); + + btScalar radius = btScalar(0.001); + btScalar lambda = btScalar(0.); + btVector3 v(1,0,0); + + int maxIter = MAX_ITERATIONS; + + btVector3 n; + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + bool hasResult = false; + btVector3 c; + btVector3 r = (linVelA-linVelB); + + btScalar lastLambda = lambda; + //btScalar epsilon = btScalar(0.001); + + int numIter = 0; + //first solution, using GJK + + + btTransform identityTrans; + identityTrans.setIdentity(); + + +// result.drawCoordSystem(sphereTr); + + btPointCollector pointCollector; + + + btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver); + btGjkPairDetector::ClosestPointInput input; + + //we don't use margins during CCD + // gjk.setIgnoreMargin(true); + + input.m_transformA = fromA; + input.m_transformB = fromB; + gjk.getClosestPoints(input,pointCollector,0); + + hasResult = pointCollector.m_hasResult; + c = pointCollector.m_pointInWorld; + + if (hasResult) + { + btScalar dist; + dist = pointCollector.m_distance; + n = pointCollector.m_normalOnBInWorld; + + + + //not close enough + while (dist > radius) + { + numIter++; + if (numIter > maxIter) + { + return false; //todo: report a failure + } + btScalar dLambda = btScalar(0.); + + btScalar projectedLinearVelocity = r.dot(n); + + dLambda = dist / (projectedLinearVelocity); + + lambda = lambda - dLambda; + + if (lambda > btScalar(1.)) + return false; + + if (lambda < btScalar(0.)) + return false; + + //todo: next check with relative epsilon + if (lambda <= lastLambda) + { + return false; + //n.setValue(0,0,0); + break; + } + 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); + if (pointCollector.m_hasResult) + { + if (pointCollector.m_distance < btScalar(0.)) + { + result.m_fraction = lastLambda; + n = pointCollector.m_normalOnBInWorld; + result.m_normal=n; + result.m_hitPoint = pointCollector.m_pointInWorld; + return true; + } + c = pointCollector.m_pointInWorld; + n = pointCollector.m_normalOnBInWorld; + dist = pointCollector.m_distance; + } 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) + return false; + + result.m_fraction = lambda; + result.m_normal = n; + result.m_hitPoint = c; + return true; + } + + return false; + + +} + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h new file mode 100644 index 0000000000..6a42ee63b0 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h @@ -0,0 +1,50 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef BT_GJK_CONVEX_CAST_H +#define BT_GJK_CONVEX_CAST_H + +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +#include "LinearMath/btVector3.h" +#include "btConvexCast.h" +class btConvexShape; +class btMinkowskiSumShape; +#include "btSimplexSolverInterface.h" + +///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; + +public: + + 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); + +}; + +#endif //BT_GJK_CONVEX_CAST_H diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp new file mode 100644 index 0000000000..eefb974bbd --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -0,0 +1,1048 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the +use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be appreciated +but is not required. +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ +#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "btGjkEpa2.h" + +#if defined(DEBUG) || defined (_DEBUG) +#include <stdio.h> //for debug printf +#ifdef __SPU__ +#include <spu_printf.h> +#define printf spu_printf +#endif //__SPU__ +#endif + +namespace gjkepa2_impl +{ + + // Config + + /* GJK */ +#define GJK_MAX_ITERATIONS 128 + +#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) + + /* EPA */ +#define EPA_MAX_VERTICES 128 +#define EPA_MAX_ITERATIONS 255 + +#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 + #define EPA_ACCURACY ((btScalar)0.0001) + #define EPA_PLANE_EPS ((btScalar)0.00001) + #define EPA_INSIDE_EPS ((btScalar)0.01) +#endif + +#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__ + bool m_enableMargin; +#else + 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]->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)); + } + } +#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__ + + 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 + { + /* Types */ + struct sSV + { + btVector3 d,w; + }; + struct sSimplex + { + 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(); + } + 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; + } + /* 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_ACCURACY*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_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) + { + 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); + } + }; + + // EPA + struct EPA + { + /* 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 + { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(0),ff(0),nf(0) {} + }; + struct eStatus { enum _ { + Valid, + Touching, + Degenerated, + NonConvex, + 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(); + } + + + 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 = 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) + { + 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) + { + 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) + { + 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; } + } + 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)); + } + + return true; + } + + 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; + + 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=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; + } + } + 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* 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); + } + + }; + + // + 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); + } + +} + +// +// Api +// + +using namespace gjkepa2_impl; + +// +int btGjkEpaSolver2::StackSizeRequirement() +{ + 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) +{ + 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) + { + 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); + } + else + { + 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) +{ + 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: + { + 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) + { + 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; + } + break; + case GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + default: + { + } + } + return(false); +} + +#ifndef __SPU__ +// +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) + { + 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] = 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); + } + else + { + if(gjk_status==GJK::eStatus::Inside) + { + if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + { + const btVector3 delta= results.witnesses[0]- + results.witnesses[1]; + const btScalar length= delta.length(); + if (length >= SIMD_EPSILON) + results.normal = delta/length; + return(-length); + } + } + } + return(SIMD_INFINITY); +} + +// +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)); + else + return(true); +} +#endif //__SPU__ + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURACY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h new file mode 100644 index 0000000000..ac501d5ecf --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -0,0 +1,75 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the +use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be appreciated +but is not required. +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ +#ifndef BT_GJK_EPA2_H +#define BT_GJK_EPA2_H + +#include "BulletCollision/CollisionShapes/btConvexShape.h" + +///btGjkEpaSolver contributed under zlib by Nathanael Presson +struct btGjkEpaSolver2 +{ +struct sResults + { + 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; + }; + +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 Penetration(const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + 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__ + +}; + +#endif //BT_GJK_EPA2_H + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h new file mode 100644 index 0000000000..ce1f24bc50 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h @@ -0,0 +1,1035 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2014 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the +use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be appreciated +but is not required. +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +Initial GJK-EPA collision solver by Nathanael Presson, 2008 +Improvements and refactoring by Erwin Coumans, 2008-2014 +*/ +#ifndef BT_GJK_EPA3_H +#define BT_GJK_EPA3_H + +#include "LinearMath/btTransform.h" +#include "btGjkCollisionDescription.h" + + + +struct btGjkEpaSolver3 +{ +struct sResults + { + 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; + }; + + +}; + + + +#if defined(DEBUG) || defined (_DEBUG) +#include <stdio.h> //for debug printf +#ifdef __SPU__ +#include <spu_printf.h> +#define printf spu_printf +#endif //__SPU__ +#endif + + + + // 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 +{ + 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 +{ + 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); + } + + }; + + 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()); + + } + + +// +// Api +// + + + +// +template <typename btConvexTemplate> +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); + } +} + + +template <typename btConvexTemplate> +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); +} + +#if 0 +int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo) +{ + btGjkEpaSolver3::sResults results; + btVector3 guess = colDesc.m_firstDir; + + bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB, + colDesc.m_transformA,colDesc.m_transformB, + colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB, + guess, + results); + if (res) + { + if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside) + { + //normal could be 'swapped' + + distInfo->m_distance = results.distance; + distInfo->m_normalBtoA = results.normal; + btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0]; + btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB = results.normal; + lenSqr = results.normal.length2(); + } + + if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB /= btSqrt(lenSqr); + btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length(); + //only replace valid penetrations when the result is deeper (check) + //if ((distance2 < results.distance)) + { + distInfo->m_distance = distance2; + distInfo->m_pointOnA= results.witnesses[0]; + distInfo->m_pointOnB= results.witnesses[1]; + distInfo->m_normalBtoA= tmpNormalInB; + return 0; + } + } + } + + } + + return -1; +} +#endif + +template <typename btConvexTemplate, typename btDistanceInfoTemplate> +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; +} + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURARY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS + + + +#endif //BT_GJK_EPA3_H + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp new file mode 100644 index 0000000000..572ec36f56 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "BulletCollision/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) +{ + + (void)debugDraw; + (void)v; + (void)simplexSolver; + +// const btScalar radialmargin(btScalar(0.)); + + btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin()); + btGjkEpaSolver2::sResults results; + + + 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 + { + if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; + } + } + + return false; +} + + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h new file mode 100644 index 0000000000..1ed6340af3 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -0,0 +1,43 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H +#define BT_GJP_EPA_PENETRATION_DEPTH_H + +#include "btConvexPenetrationDepthSolver.h" + +///EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to +///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 : + +}; + +#endif // BT_GJP_EPA_PENETRATION_DEPTH_H + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp new file mode 100644 index 0000000000..257b026d9b --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -0,0 +1,467 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btGjkPairDetector.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" +#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" + + + +#if defined(DEBUG) || defined (_DEBUG) +//#define TEST_NON_VIRTUAL 1 +#include <stdio.h> //for debug printf +#ifdef __SPU__ +#include <spu_printf.h> +#define printf spu_printf +#endif //__SPU__ +#endif + +//must be above the machine epsilon +#ifdef BT_USE_DOUBLE_PRECISION + #define REL_ERROR2 btScalar(1.0e-12) + btScalar gGjkEpaPenetrationTolerance = 1e-7; +#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) +{ + (void)swapResults; + + getClosestPointsNonVirtual(input,output,debugDraw); +} + +#ifdef __SPU__ +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +#else +void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw) +#endif +{ + m_cachedSeparatingDistance = 0.f; + + btScalar distance=btScalar(0.); + btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); + + btVector3 pointOnA,pointOnB; + btTransform localTransA = input.m_transformA; + btTransform localTransB = input.m_transformB; + btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); + localTransA.getOrigin() -= positionOffset; + localTransB.getOrigin() -= positionOffset; + + bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d(); + + btScalar marginA = m_marginA; + btScalar marginB = m_marginB; + + gNumGjkChecks++; + + //for CCD we don't use margins + if (m_ignoreMargin) + { + marginA = btScalar(0.); + marginB = btScalar(0.); + } + + m_curIter = 0; + int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? + m_cachedSeparatingAxis.setValue(0,1,0); + + bool isValid = false; + bool checkSimplex = false; + bool checkPenetration = true; + m_degenerateSimplex = 0; + + m_lastUsedMethod = -1; + + { + btScalar squaredDistance = BT_LARGE_FLOAT; + btScalar delta = btScalar(0.); + + btScalar margin = marginA + marginB; + + + + m_simplexSolver->reset(); + + for ( ; ; ) + //while (true) + { + + btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); + btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); + + + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + + 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(); +#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; + } + + 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, shapeTypeA=%i,shapeTypeB=%i\n", + m_cachedSeparatingAxis.getX(), + m_cachedSeparatingAxis.getY(), + m_cachedSeparatingAxis.getZ(), + squaredDistance, + m_minkowskiA->getShapeType(), + m_minkowskiB->getShapeType()); + + #endif + break; + + } + + + bool check = (!m_simplexSolver->fullSimplex()); + //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); + + if (!check) + { + //do we need this backup_closest here ? +// m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + m_degenerateSimplex = 13; + break; + } + } + + if (checkSimplex) + { + m_simplexSolver->compute_points(pointOnA, pointOnB); + normalInB = 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; + + m_lastUsedMethod = 1; + } else + { + m_lastUsedMethod = 2; + } + } + + bool catchDegeneratePenetrationCase = + (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance)); + + //if (checkPenetration && !isValid) + if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) + { + //penetration case + + //if there is no way to handle penetrations, bail out + if (m_penetrationDepthSolver) + { + // Penetration depth case. + btVector3 tmpPointOnA,tmpPointOnB; + + gNumDeepPenetrationChecks++; + m_cachedSeparatingAxis.setZero(); + + bool isValid2 = m_penetrationDepthSolver->calcPenDepth( + *m_simplexSolver, + m_minkowskiA,m_minkowskiB, + localTransA,localTransB, + m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, + debugDraw + ); + + + 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(); + m_lastUsedMethod = 3; + //only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + + isValid = true; + + } else + { + m_lastUsedMethod = 8; + } + } else + { + m_lastUsedMethod = 9; + } + } else + + { + ///this is another degenerate case, where the initial GJK calculation reports a degenerate case + ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) + ///reports a valid positive distance. Use the results of the second GJK instead of failing. + ///thanks to Jacob.Langford for the reproduction case + ///http://code.google.com/p/bullet/issues/detail?id=250 + + + if (m_cachedSeparatingAxis.length2() > btScalar(0.)) + { + btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; + //only replace valid distances when the distance is less + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + pointOnA -= m_cachedSeparatingAxis * marginA ; + pointOnB += m_cachedSeparatingAxis * marginB ; + normalInB = m_cachedSeparatingAxis; + normalInB.normalize(); + + isValid = true; + m_lastUsedMethod = 6; + } else + { + m_lastUsedMethod = 5; + } + } + } + + } + + } + } + + + + if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) + { + + m_cachedSeparatingAxis = normalInB; + m_cachedSeparatingDistance = distance; + + { + ///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 + + 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; + } + + } + output.addContactPoint( + normalInB, + pointOnB+positionOffset, + distance); + + } + + +} + + + + + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h new file mode 100644 index 0000000000..feeae68621 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -0,0 +1,103 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + + +#ifndef BT_GJK_PAIR_DETECTOR_H +#define BT_GJK_PAIR_DETECTOR_H + +#include "btDiscreteCollisionDetectorInterface.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +class btConvexShape; +#include "btSimplexSolverInterface.h" +class btConvexPenetrationDepthSolver; + +/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface +class btGjkPairDetector : public btDiscreteCollisionDetectorInterface +{ + + + btVector3 m_cachedSeparatingAxis; + btConvexPenetrationDepthSolver* m_penetrationDepthSolver; + btSimplexSolverInterface* m_simplexSolver; + const btConvexShape* m_minkowskiA; + const btConvexShape* m_minkowskiB; + int m_shapeTypeA; + int m_shapeTypeB; + btScalar m_marginA; + btScalar m_marginB; + + 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; + + 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); + + void getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); + + + void setMinkowskiA(const btConvexShape* minkA) + { + m_minkowskiA = minkA; + } + + void setMinkowskiB(const btConvexShape* minkB) + { + m_minkowskiB = minkB; + } + void setCachedSeperatingAxis(const btVector3& seperatingAxis) + { + m_cachedSeparatingAxis = seperatingAxis; + } + + const btVector3& getCachedSeparatingAxis() const + { + return m_cachedSeparatingAxis; + } + btScalar getCachedSeparatingDistance() const + { + return m_cachedSeparatingDistance; + } + + void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) + { + m_penetrationDepthSolver = penetrationDepthSolver; + } + + ///don't use setIgnoreMargin, it's for Bullet's internal use + void setIgnoreMargin(bool ignoreMargin) + { + m_ignoreMargin = ignoreMargin; + } + + +}; + +#endif //BT_GJK_PAIR_DETECTOR_H diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h new file mode 100644 index 0000000000..571ad2c5f7 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -0,0 +1,180 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MANIFOLD_CONTACT_POINT_H +#define BT_MANIFOLD_CONTACT_POINT_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransformUtil.h" + +#ifdef PFX_USE_FREE_VECTORMATH + #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 + +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_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) + { + } + + 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_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_appliedImpulseLateral1; + btScalar m_appliedImpulseLateral2; + btScalar m_contactMotion1; + btScalar m_contactMotion2; + + union + { + btScalar m_contactCFM; + btScalar m_combinedContactStiffness1; + }; + + union + { + btScalar m_contactERP; + btScalar m_combinedContactDamping1; + }; + + 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/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp new file mode 100644 index 0000000000..fa45f49037 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -0,0 +1,361 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btMinkowskiPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" + +#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 + ) +{ + + (void)v; + + bool check2d= convexA->isConvex2d() && convexB->isConvex2d(); + + struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result + { + + btIntermediateResult():m_hasResult(false) + { + } + + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + btScalar m_depth; + bool m_hasResult; + + virtual void setShapeIdentifiersA(int partId0,int index0) + { + (void)partId0; + (void)index0; + } + virtual void setShapeIdentifiersB(int partId1,int index1) + { + (void)partId1; + (void)index1; + } + void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; + m_hasResult = true; + } + }; + + //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; + +#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]; + int i; + + int numSampleDirections = NUM_UNITSPHERE_POINTS; + + for (i=0;i<numSampleDirections;i++) + { + btVector3 norm = getPenetrationDirections()[i]; + seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ; + seperatingAxisInBBatch[i] = norm * transB.getBasis() ; + } + + { + int numPDA = convexA->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int i=0;i<numPDA;i++) + { + btVector3 norm; + convexA->getPreferredPenetrationDirection(i,norm); + norm = transA.getBasis() * norm; + getPenetrationDirections()[numSampleDirections] = norm; + seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); + seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); + numSampleDirections++; + } + } + } + + { + int numPDB = convexB->getNumPreferredPenetrationDirections(); + if (numPDB) + { + for (int i=0;i<numPDB;i++) + { + btVector3 norm; + convexB->getPreferredPenetrationDirection(i,norm); + norm = transB.getBasis() * norm; + getPenetrationDirections()[numSampleDirections] = norm; + seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); + seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); + numSampleDirections++; + } + } + } + + + + + convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); + convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); + + for (i=0;i<numSampleDirections;i++) + { + btVector3 norm = getPenetrationDirections()[i]; + if (check2d) + { + norm[2] = 0.f; + } + if (norm.length2()>0.01) + { + + seperatingAxisInA = seperatingAxisInABatch[i]; + seperatingAxisInB = seperatingAxisInBBatch[i]; + + pInA = supportVerticesABatch[i]; + qInB = supportVerticesBBatch[i]; + + pWorld = transA(pInA); + qWorld = transB(qInB); + if (check2d) + { + pWorld[2] = 0.f; + qWorld[2] = 0.f; + } + + w = qWorld - pWorld; + btScalar delta = norm.dot(w); + //find smallest delta + if (delta < minProj) + { + minProj = delta; + minNorm = norm; + minA = pWorld; + minB = qWorld; + } + } + } +#else + + int numSampleDirections = NUM_UNITSPHERE_POINTS; + +#ifndef __SPU__ + { + int numPDA = convexA->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int i=0;i<numPDA;i++) + { + btVector3 norm; + convexA->getPreferredPenetrationDirection(i,norm); + norm = transA.getBasis() * norm; + getPenetrationDirections()[numSampleDirections] = norm; + numSampleDirections++; + } + } + } + + { + int numPDB = convexB->getNumPreferredPenetrationDirections(); + if (numPDB) + { + for (int i=0;i<numPDB;i++) + { + btVector3 norm; + convexB->getPreferredPenetrationDirection(i,norm); + norm = transB.getBasis() * norm; + getPenetrationDirections()[numSampleDirections] = norm; + numSampleDirections++; + } + } + } +#endif // __SPU__ + + 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); + qWorld = transB(qInB); + w = qWorld - pWorld; + btScalar delta = norm.dot(w); + //find smallest delta + if (delta < minProj) + { + minProj = delta; + minNorm = norm; + minA = pWorld; + minB = qWorld; + } + } +#endif //USE_BATCHED_SUPPORT + + //add the margins + + 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()); + + + + + +//#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; + btScalar prj2 = minNorm.dot(vec); + debugDraw->drawLine(minA,minA+(minNorm*minProj),color); + + } +#endif //DEBUG_DRAW + + + + btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); + + btScalar offsetDist = minProj; + btVector3 offset = minNorm * offsetDist; + + + + btGjkPairDetector::ClosestPointInput input; + + btVector3 newOrg = transA.getOrigin() + offset; + + btTransform displacedTrans = transA; + displacedTrans.setOrigin(newOrg); + + input.m_transformA = displacedTrans; + input.m_transformB = transB; + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; + + btIntermediateResult res; + gjkdet.setCachedSeperatingAxis(-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; + + + 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); + } +#endif//DEBUG_DRAW + + + } + return res.m_hasResult; +} + +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)) + }; + + return sPenetrationDirections; +} + + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h new file mode 100644 index 0000000000..fd533b4fc3 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -0,0 +1,40 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H +#define BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H + +#include "btConvexPenetrationDepthSolver.h" + +///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. +///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. +class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver +{ +protected: + + 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 + ); +}; + +#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h new file mode 100644 index 0000000000..a22a0bae66 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h @@ -0,0 +1,908 @@ + +/*** + * --------------------------------- + * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz> + * + * This file was ported from mpr.c file, part of libccd. + * The Minkoski Portal Refinement implementation was ported + * to OpenCL by Erwin Coumans for the Bullet 3 Physics library. + * The original MPR idea and implementation is by Gary Snethen + * in XenoCollide, see http://github.com/erwincoumans/xenocollide + * + * Distributed under the OSI-approved BSD License (the "License"); + * see <http://www.opensource.org/licenses/bsd-license.php>. + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +///2014 Oct, Erwin Coumans, Use templates to avoid void* casts + +#ifndef BT_MPR_PENETRATION_H +#define BT_MPR_PENETRATION_H + +#define BT_DEBUG_MPR1 + +#include "LinearMath/btTransform.h" +#include "LinearMath/btAlignedObjectArray.h" + +//#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() + { + } +}; + +struct btMprDistanceInfo +{ + btVector3 m_pointOnA; + btVector3 m_pointOnB; + btVector3 m_normalBtoA; + btScalar m_distance; +}; + +#ifdef __cplusplus +#define BT_MPR_SQRT sqrtf +#else +#define BT_MPR_SQRT sqrt +#endif +#define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) +#define BT_MPR_FABS fabs + +#define BT_MPR_TOLERANCE 1E-6f +#define BT_MPR_MAX_ITERATIONS 1000 + +struct _btMprSupport_t +{ + 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 +{ + 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) +{ + return &s->ps[idx]; +} + +inline void btMprSimplexSetSize(btMprSimplex_t *s, int size) +{ + s->last = size - 1; +} + +#ifdef DEBUG_MPR +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()); +} +#endif //DEBUG_MPR + + + + +inline int btMprSimplexSize(const btMprSimplex_t *s) +{ + return s->last + 1; +} + + +inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} + +inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s) +{ + *d = *s; +} + +inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a) +{ + btMprSupportCopy(s->ps + pos, a); +} + + +inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2) +{ + btMprSupport_t 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; +} + + + +inline int btMprEq(float _a, float _b) +{ + float ab; + float a, b; + + 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; + } +} + + +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()); +} + + + + + + + + + + + +template <typename btConvexTemplate> +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; +} + +inline void btMprVec3Set(btVector3 *v, float x, float y, float z) +{ + v->setValue(x,y,z); +} + +inline void btMprVec3Add(btVector3 *v, const btVector3 *w) +{ + *v += *w; +} + +inline void btMprVec3Copy(btVector3 *v, const btVector3 *w) +{ + *v = *w; +} + +inline void btMprVec3Scale(btVector3 *d, float k) +{ + *d *= k; +} + +inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b) +{ + float dot; + + dot = btDot(*a,*b); + return dot; +} + + +inline float btMprVec3Len2(const btVector3 *v) +{ + return btMprVec3Dot(v, v); +} + +inline void btMprVec3Normalize(btVector3 *d) +{ + 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); + +} + + +inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) +{ + *d = *v - *w; +} + +inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir) +{ + 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); +} + + +inline int portalEncapsulesOrigin(const btMprSimplex_t *portal, + const btVector3 *dir) +{ + 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) +{ + float dv1, dv2, dv3, dv4; + float dot1, dot2, dot3; + + // 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); + + dot1 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; + + dot1 = BT_MPR_FMIN(dot1, dot2); + dot1 = BT_MPR_FMIN(dot1, dot3); + + return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE; +} + +inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, + const btMprSupport_t *v4, + const btVector3 *dir) +{ + 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); + } + } +} +template <typename btConvexTemplate> +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 pInA = a.getLocalSupportWithMargin(seperatingAxisInA); + btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB); + + 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; +} + +template <typename btConvexTemplate> +static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc, + btMprSimplex_t *portal) +{ + btVector3 dir; + btMprSupport_t v4; + + 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); + + // test if origin is inside the portal + if (portalEncapsulesOrigin(portal, &dir)) + return 0; + + // 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)) + { + return -1; + } + + // v1-v2-v3 triangle must be rearranged to face outside Minkowski + // difference (direction from v0). + btExpandPortal(portal, &v4); + } + + return -1; +} + +static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos) +{ + + btVector3 zero = btVector3(0,0,0); + btVector3* origin = &zero; + + btVector3 dir; + size_t i; + float b[4], sum, inv; + btVector3 vec, p1, p2; + + 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); + + 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, 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){ + 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); + + 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); +#ifdef MPR_AVERAGE_CONTACT_POSITIONS + btMprVec3Copy(pos, &p1); + btMprVec3Add(pos, &p2); + btMprVec3Scale(pos, 0.5); +#else + 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); +} + +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; +} + + + +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); + + btScalar div = (w * v - r * r); + if (btMprIsZero(div)) + { + 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; +} + +template <typename btConvexTemplate> +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 zero = btVector3(0,0,0); + btVector3* origin = &zero; + + + 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); + + + // 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); + + if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z())) + { + + *pdir = dir; + } + btMprVec3Normalize(pdir); + + // barycentric coordinates: + btFindPos(portal, pos); + + + return; + } + + btExpandPortal(portal, &v4); + + iterations++; + } +} + +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; + + + btMprVec3Copy(dir, origin); +#ifdef MPR_AVERAGE_CONTACT_POSITIONS + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); + btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Scale(pos, 0.5); +#else + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); +#endif +} + +static void btFindPenetrSegment(btMprSimplex_t *portal, + float *depth, btVector3 *dir, btVector3 *pos) +{ + + // 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); +#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); + + +} + + +template <typename btConvexTemplate> +inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, + const btMprCollisionDescription& colDesc, + float *depthOut, btVector3* dirOut, btVector3* posOut) +{ + + btMprSimplex_t portal; + + + // Phase 1: Portal discovery + int result = btDiscoverPortal(a,b,colDesc, &portal); + + + //sepAxis[pairIndex] = *pdir;//or -dir? + + switch (result) + { + case 0: + { + // Phase 2: Portal refinement + + result = btRefinePortal(a,b,colDesc, &portal); + if (result < 0) + return -1; + + // Phase 3. Penetration info + btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut); + + + break; + } + case 1: + { + // Touching contact on portal's v1. + btFindPenetrTouch(&portal, depthOut, dirOut, posOut); + result=0; + break; + } + case 2: + { + + btFindPenetrSegment( &portal, depthOut, dirOut, posOut); + result=0; + break; + } + default: + { + //if (res < 0) + //{ + // 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) +{ + btVector3 dir,pos; + float depth; + + 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; + return 0; + } + + return -1; +} + + + +#endif //BT_MPR_PENETRATION_H diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp new file mode 100644 index 0000000000..23aaece22b --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -0,0 +1,308 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btPersistentManifold.h" +#include "LinearMath/btTransform.h" + + +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; + + +btPersistentManifold::btPersistentManifold() +:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), +m_body0(0), +m_body1(0), +m_cachedPoints (0), +m_index1a(0) +{ +} + + + + +#ifdef DEBUG_PERSISTENCY +#include <stdio.h> +void btPersistentManifold::DebugPersistency() +{ + int 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); + } +} +#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++) + { + if (m_pointCache[i].m_userPersistentData == oldPtr) + { + occurance++; + if (occurance>1) + printf("error in clearUserCache\n"); + } + } + 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) +{ + // It calculates possible 3 area constructed from random 4 points and returns the biggest one. + + btVector3 a[3],b[3]; + a[0] = p0 - p1; + a[1] = p0 - p2; + a[2] = p0 - p3; + b[0] = p2 - p3; + b[1] = p1 - p3; + b[2] = p1 - p2; + + //todo: Following 3 cross production can be easily optimized by SIMD. + btVector3 tmp0 = a[0].cross(b[0]); + btVector3 tmp1 = a[1].cross(b[1]); + btVector3 tmp2 = a[2].cross(b[2]); + + return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2()); +} + +int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) +{ + //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++) + { + if (m_pointCache[i].getDistance() < maxPenetration) + { + maxPenetrationIndex = i; + maxPenetration = m_pointCache[i].getDistance(); + } + } +#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 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 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 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 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 != 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 != 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); + int biggestarea = maxvec.closestAxis4(); + return biggestarea; + +} + + +int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const +{ + btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); + int size = getNumContacts(); + int nearestPoint = -1; + for( int i = 0; i < size; i++ ) + { + const btManifoldPoint &mp = m_pointCache[i]; + + btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA; + const btScalar distToManiPoint = diffA.dot(diffA); + if( distToManiPoint < shortestDist ) + { + shortestDist = distToManiPoint; + nearestPoint = i; + } + } + return nearestPoint; +} + +int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive) +{ + if (!isPredictive) + { + btAssert(validContactDistance(newPoint)); + } + + int insertIndex = getNumContacts(); + if (insertIndex == MANIFOLD_CACHE_SIZE) + { +#if MANIFOLD_CACHE_SIZE >= 4 + //sort cache so best points come first, based on area + insertIndex = sortCachedPoints(newPoint); +#else + insertIndex = 0; +#endif + clearUserCache(m_pointCache[insertIndex]); + + } else + { + m_cachedPoints++; + + + } + if (insertIndex<0) + insertIndex=0; + + btAssert(m_pointCache[insertIndex].m_userPersistentData==0); + m_pointCache[insertIndex] = newPoint; + return insertIndex; +} + +btScalar btPersistentManifold::getContactBreakingThreshold() const +{ + return m_contactBreakingThreshold; +} + + + +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 + /// first refresh worldspace positions and distance + 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); + manifoldPoint.m_lifeTime++; + } + + /// then + btScalar distance2d; + btVector3 projectedDifference,projectedPoint; + for (i=getNumContacts()-1;i>=0;i--) + { + + btManifoldPoint &manifoldPoint = m_pointCache[i]; + //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) + if (!validContactDistance(manifoldPoint)) + { + removeContactPoint(i); + } 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() ) + { + removeContactPoint(i); + } else + { + //contact point processed callback + if (gContactProcessedCallback) + (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1); + } + } + } +#ifdef DEBUG_PERSISTENCY + DebugPersistency(); +#endif // +} + + + + + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h new file mode 100644 index 0000000000..f872c8e1c9 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -0,0 +1,268 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_PERSISTENT_MANIFOLD_H +#define BT_PERSISTENT_MANIFOLD_H + + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "btManifoldPoint.h" +class btCollisionObject; +#include "LinearMath/btAlignedAllocator.h" + +struct btCollisionResult; + +///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); +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 +{ + MIN_CONTACT_MANIFOLD_TYPE = 1024, + BT_PERSISTENT_MANIFOLD_TYPE +}; + +#define MANIFOLD_CACHE_SIZE 4 + +///btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase. +///Those contact points are created by the collision narrow phase. +///The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time. +///updates/refreshes old contact points, and throw them away if necessary (distance becomes too large) +///reduces the cache to 4 points, when more then 4 points are added, using following rules: +///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 +{ + + 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; + + btScalar m_contactBreakingThreshold; + btScalar m_contactProcessingThreshold; + + + /// sort cached points so most isolated points come first + int sortCachedPoints(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_index1a; + + btPersistentManifold(); + + 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) + { + } + + 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) + { + m_body0 = body0; + m_body1 = body1; + } + + void clearUserCache(btManifoldPoint& pt); + +#ifdef DEBUG_PERSISTENCY + 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); + return m_pointCache[index]; + } + + SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index) + { + btAssert(index < m_cachedPoints); + return m_pointCache[index]; + } + + ///@todo: get this margin from the current physics / collision environment + btScalar getContactBreakingThreshold() const; + + btScalar getContactProcessingThreshold() const + { + return m_contactProcessingThreshold; + } + + void setContactBreakingThreshold(btScalar contactBreakingThreshold) + { + m_contactBreakingThreshold = contactBreakingThreshold; + } + + void setContactProcessingThreshold(btScalar contactProcessingThreshold) + { + m_contactProcessingThreshold = contactProcessingThreshold; + } + + + + + int getCacheEntry(const btManifoldPoint& newPoint) const; + + int addManifoldPoint( const btManifoldPoint& newPoint, bool isPredictive=false); + + 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]; + //get rid of duplicated userPersistentData pointer + m_pointCache[lastUsedIndex].m_userPersistentData = 0; + m_pointCache[lastUsedIndex].m_appliedImpulse = 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); + m_cachedPoints--; + + if (gContactEndedCallback && m_cachedPoints == 0) + { + gContactEndedCallback(this); + } + } + 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 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); + } + + 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_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); + + + SIMD_FORCE_INLINE void clearManifold() + { + int i; + for (i=0;i<m_cachedPoints;i++) + { + clearUserCache(m_pointCache[i]); + } + + if (gContactEndedCallback && m_cachedPoints) + { + gContactEndedCallback(this); + } + m_cachedPoints = 0; + } + + + +} +; + + + + + +#endif //BT_PERSISTENT_MANIFOLD_H diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h new file mode 100644 index 0000000000..18da171011 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h @@ -0,0 +1,64 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_POINT_COLLECTOR_H +#define BT_POINT_COLLECTOR_H + +#include "btDiscreteCollisionDetectorInterface.h" + + + +struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result +{ + + + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + btScalar m_distance;//negative means penetration + + bool m_hasResult; + + btPointCollector () + : m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false) + { + } + + virtual void setShapeIdentifiersA(int partId0,int index0) + { + (void)partId0; + (void)index0; + + } + virtual void setShapeIdentifiersB(int partId1,int index1) + { + (void)partId1; + (void)index1; + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + if (depth< m_distance) + { + m_hasResult = true; + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + //negative means penetration + m_distance = depth; + } + } +}; + +#endif //BT_POINT_COLLECTOR_H + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp new file mode 100644 index 0000000000..ea380bc5f1 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -0,0 +1,570 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +///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 + +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) +{ + + int ve; + btScalar ds, de; + int numVerts = pVtxIn.size(); + if (numVerts < 2) + return; + + btVector3 firstVertex=pVtxIn[pVtxIn.size()-1]; + btVector3 endVertex = pVtxIn[0]; + + ds = planeNormalWS.dot(firstVertex)+planeEqWS; + + for (ve = 0; ve < numVerts; ve++) + { + endVertex=pVtxIn[ve]; + + de = planeNormalWS.dot(endVertex)+planeEqWS; + + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut.push_back(endVertex); + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + } + } + else + { + 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(endVertex); + } + } + firstVertex = endVertex; + ds = de; + } +} + + +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; + + hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA); + hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB); + + if(Max0<Min1 || Max1<Min0) + return false; + + btScalar d0 = Max0 - Min1; + btAssert(d0>=0.0f); + btScalar d1 = Max1 - Min0; + btAssert(d1>=0.0f); + if (d0<d1) + { + depth = d0; + witnessPointA = witnesPtMaxA; + witnessPointB = witnesPtMinB; + + } else + { + depth = d1; + witnessPointA = witnesPtMinA; + witnessPointB = witnesPtMaxB; + } + + return true; +} + + + +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; + return true; +} + +#ifdef TEST_INTERNAL_OBJECTS + +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); + 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]; +} + +void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr) +{ + const btMatrix3x3& rot = tr.getBasis(); + const btVector3& r0 = rot[0]; + 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(); + + 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) +{ + const btScalar dp = delta_c.dot(axis); + + btVector3 localAxis0; + InverseTransformPoint3x3(localAxis0, axis,trans0); + btVector3 localAxis1; + 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 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) + return false; + return true; +} +#endif //TEST_INTERNAL_OBJECTS + + + + 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 ) +{ + // 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 denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; + + if ( denom == 0.0f ) { + tA = 0.0f; + } else { + tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } + + tB = tA * dirA_dot_dirB - dirB_dot_trans; + + if ( tB < -hlenB ) { + tB = -hlenB; + tA = tB * dirA_dot_dirB + dirA_dot_trans; + + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } else if ( tB > hlenB ) { + tB = hlenB; + tA = tB * dirA_dot_dirB + dirA_dot_trans; + + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } + + // compute the closest points relative to segment centers. + + offsetA = dirA * tA; + offsetB = dirB * tB; + + ptsVector = translation - offsetA + offsetB; +} + + + +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 + const btVector3 c0 = transA * hullA.m_localCenter; + const btVector3 c1 = transB * hullB.m_localCenter; + const btVector3 DeltaC2 = c0 - c1; +//#endif + + btScalar dmin = FLT_MAX; + int curPlaneTests=0; + + int numFacesA = hullA.m_faces.size(); + // Test normals from hullA + 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; + + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + 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)) + return false; + + if(d<dmin) + { + dmin = d; + sep = faceANormalWS; + } + } + + int numFacesB = hullB.m_faces.size(); + // Test normals from hullB + 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; + + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + 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)) + return false; + + if(d<dmin) + { + dmin = d; + sep = WorldNormal; + } + } + + btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend; + int edgeA=-1; + int edgeB=-1; + btVector3 worldEdgeA; + btVector3 worldEdgeB; + btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0); + + + int curEdgeEdge = 0; + // Test edges + 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++) + { + const btVector3 edge1 = hullB.m_uniqueEdges[e1]; + const btVector3 WorldEdge1 = transB.getBasis() * edge1; + + btVector3 Cross = WorldEdge0.cross(WorldEdge1); + curEdgeEdge++; + if(!IsAlmostZero(Cross)) + { + Cross = Cross.normalize(); + if (DeltaC2.dot(Cross)<0) + Cross *= -1.f; + + +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + 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)) + return false; + + if(dist<dmin) + { + dmin = dist; + sep = Cross; + edgeA=e0; + edgeB=e1; + worldEdgeA = WorldEdge0; + worldEdgeB = WorldEdge1; + witnessPointA=wA; + witnessPointB=wB; + } + } + } + + } + + if (edgeA>=0&&edgeB>=0) + { +// printf("edge-edge\n"); + //add an edge-edge contact + + btVector3 ptsVector; + btVector3 offsetA; + btVector3 offsetB; + btScalar tA; + btScalar tB; + + 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); + + btScalar nlSqrt = ptsVector.length2(); + if (nlSqrt>SIMD_EPSILON) + { + btScalar nl = btSqrt(nlSqrt); + ptsVector *= 1.f/nl; + if (ptsVector.dot(DeltaC2)<0.f) + { + ptsVector*=-1.f; + } + btVector3 ptOnB = witnessPointB + offsetB; + btScalar distance = nl; + resultOut.addContactPoint(ptsVector, ptOnB,-distance); + } + + } + + + if((DeltaC2.dot(sep))<0.0f) + sep = -sep; + + return true; +} + +void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +{ + worldVertsB2.resize(0); + btVertexArray* pVtxIn = &worldVertsB1; + btVertexArray* pVtxOut = &worldVertsB2; + pVtxOut->reserve(pVtxIn->size()); + + int closestFaceA=-1; + { + btScalar dmin = FLT_MAX; + 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) + { + dmin = d; + closestFaceA = face; + } + } + } + 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 + int numVerticesA = polyA.m_indices.size(); + 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 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 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]); + btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3]; + + btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; + btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); +#else + btVector3 planeNormalWS = planeNormalWS1; + btScalar planeEqWS=planeEqWS1; + +#endif + //clip face + + clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); + btSwap(pVtxIn,pVtxOut); + pVtxOut->resize(0); + } + + + +//#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]); + 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 vtx = pVtxIn->at(i); + btScalar depth = planeNormalWS.dot(vtx)+planeEqWS; + if (depth <=minDist) + { +// printf("clamped: depth=%f to minDist=%f\n",depth,minDist); + depth = minDist; + } + + if (depth <=maxDist) + { + btVector3 point = pVtxIn->at(i); +#ifdef ONLY_REPORT_DEEPEST_POINT + curMaxDist = depth; +#else +#if 0 + if (depth<-3) + { + printf("error in btPolyhedralContactClipping depth = %f\n", depth); + printf("likely wrong separatingNormal passed in\n"); + } +#endif + resultOut.addContactPoint(separatingNormal,point,depth); +#endif + } + } + } +#ifdef ONLY_REPORT_DEEPEST_POINT + if (curMaxDist<maxDist) + { + resultOut.addContactPoint(separatingNormal,point,curMaxDist); + } +#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,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 DeltaC2 = c0 - c1; + + + + int closestFaceB=-1; + btScalar dmax = -FLT_MAX; + { + 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; + btScalar d = WorldNormal.dot(separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + 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/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h new file mode 100644 index 0000000000..30e3db687b --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h @@ -0,0 +1,49 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +///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" + +class btConvexPolyhedron; + +typedef btAlignedObjectArray<btVector3> btVertexArray; + +// Clips a face to the back of a plane +struct btPolyhedralContactClipping +{ + + 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 void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut); + + + 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 + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp new file mode 100644 index 0000000000..786efd1820 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -0,0 +1,178 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +//#include <stdio.h> + +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" +#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.)) +{ + +} + + + +void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) +{ + 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 triangleNormal; triangleNormal = v10.cross( v20 ); + + const btScalar dist = vert0.dot(triangleNormal); + 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) ) + { + return ; // same sign + } + + if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a <= btScalar(0.0))) + { + // Backface, skip check + return; + } + + + 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(); + edge_tolerance *= btScalar(-0.0001); + 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 v2p; v2p = vert2 - point; + btVector3 cp1; + cp1 = v1p.cross( v2p); + if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) + { + btVector3 cp2; + cp2 = v2p.cross(v0p); + + if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) + { + //@BP Mod + // Triangle normal isn't normalized + triangleNormal.normalize(); + + //@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); + } + else + { + m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex); + } + } + } + } + } + } +} + + +btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin) +{ + m_convexShape = convexShape; + m_convexShapeFrom = convexShapeFrom; + m_convexShapeTo = convexShapeTo; + m_triangleToWorld = triangleToWorld; + m_hitFraction = 1.0f; + m_triangleCollisionMargin = triangleCollisionMargin; + m_allowedPenetration = 0.f; +} + +void +btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex) +{ + btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]); + triangleShape.setMargin(m_triangleCollisionMargin); + + btVoronoiSimplexSolver simplexSolver; + btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; + +//#define USE_SUBSIMPLEX_CONVEX_CAST 1 +//if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + 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 + + 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)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < m_hitFraction) + { +/* 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; +#endif //USE_SUBSIMPLEX_CONVEX_CAST +*/ + castResult.m_normal.normalize(); + + reportHit (castResult.m_normal, + castResult.m_hitPoint, + castResult.m_fraction, + partId, + triangleIndex); + } + } + } +} diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h new file mode 100644 index 0000000000..f2ed0cd39c --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -0,0 +1,74 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_RAYCAST_TRI_CALLBACK_H +#define BT_RAYCAST_TRI_CALLBACK_H + +#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "LinearMath/btTransform.h" +struct btBroadphaseProxy; +class btConvexShape; + +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; + + btScalar m_hitFraction; + + 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; + +}; + +class btTriangleConvexcastCallback : public btTriangleCallback +{ +public: + const btConvexShape* m_convexShape; + btTransform m_convexShapeFrom; + btTransform m_convexShapeTo; + btTransform m_triangleToWorld; + btScalar m_hitFraction; + btScalar m_triangleCollisionMargin; + btScalar m_allowedPenetration; + + 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 btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; +}; + +#endif //BT_RAYCAST_TRI_CALLBACK_H + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h new file mode 100644 index 0000000000..da8a13914c --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -0,0 +1,63 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef BT_SIMPLEX_SOLVER_INTERFACE_H +#define BT_SIMPLEX_SOLVER_INTERFACE_H + +#include "LinearMath/btVector3.h" + +#define NO_VIRTUAL_INTERFACE 1 +#ifdef NO_VIRTUAL_INTERFACE +#include "btVoronoiSimplexSolver.h" +#define btSimplexSolverInterface btVoronoiSimplexSolver +#else + +/// btSimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices +/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on +/// voronoi regions or barycentric coordinates +class 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 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; + + +}; +#endif +#endif //BT_SIMPLEX_SOLVER_INTERFACE_H + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp new file mode 100644 index 0000000000..ec638f60ba --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -0,0 +1,160 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btSubSimplexConvexCast.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" + +#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" +#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) +{ +} + +///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) +{ + + m_simplexSolver->reset(); + + btVector3 linVelA,linVelB; + linVelA = toA.getOrigin()-fromA.getOrigin(); + linVelB = toB.getOrigin()-fromB.getOrigin(); + + btScalar lambda = btScalar(0.); + + btTransform interpolatedTransA = fromA; + btTransform interpolatedTransB = fromB; + + ///take relative motion + 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 n; + n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + + btVector3 c; + + + + + 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; + btScalar VdotR; + + while ( (dist2 > epsilon) && maxIter--) + { + supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis())); + supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis())); + w = supVertexA-supVertexB; + + btScalar VdotW = v.dot(w); + + if (lambda > btScalar(1.0)) + { + return false; + } + + if ( VdotW > btScalar(0.)) + { + VdotR = v.dot(r); + + 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); + //m_simplexSolver->reset(); + //check next line + 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); + + 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 + { + dist2 = btScalar(0.); + } + } + + //int numiter = MAX_ITERATIONS - maxIter; +// 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)) + 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) + return false; + + btVector3 hitA,hitB; + m_simplexSolver->compute_points(hitA,hitB); + result.m_hitPoint=hitB; + return true; +} + + + + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h new file mode 100644 index 0000000000..6c8127983e --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h @@ -0,0 +1,50 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef BT_SUBSIMPLEX_CONVEX_CAST_H +#define BT_SUBSIMPLEX_CONVEX_CAST_H + +#include "btConvexCast.h" +#include "btSimplexSolverInterface.h" +class btConvexShape; + +/// btSubsimplexConvexCast implements Gino van den Bergens' paper +///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" +/// GJK based Ray Cast, optimized version +/// Objects should not start in overlap, otherwise results are not defined. +class btSubsimplexConvexCast : public btConvexCast +{ + btSimplexSolverInterface* m_simplexSolver; + const btConvexShape* m_convexA; + const btConvexShape* m_convexB; + +public: + + 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); + +}; + +#endif //BT_SUBSIMPLEX_CONVEX_CAST_H diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp new file mode 100644 index 0000000000..756373c9b5 --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -0,0 +1,612 @@ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + + Elsevier CDROM license agreements grants nonexclusive license to use the software + for any purpose, commercial or non-commercial as long as the following credit is included + identifying the original source of the software: + + Parts of the source are "from the book Real-Time Collision Detection by + Christer Ericson, published by Morgan Kaufmann Publishers, + (c) 2005 Elsevier Inc." + +*/ + + +#include "btVoronoiSimplexSolver.h" + +#define VERTA 0 +#define VERTB 1 +#define VERTC 2 +#define VERTD 3 + +#define CATCH_DEGENERATE_TETRAHEDRON 1 +void btVoronoiSimplexSolver::removeVertex(int index) +{ + + 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) +{ + if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) + removeVertex(3); + + if ((numVertices() >= 3) && (!usedVerts.usedVertexC)) + removeVertex(2); + + 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_cachedBC.reset(); +} + + + + //add a vertex +void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q) +{ + m_lastW = w; + m_needsUpdate = true; + + m_simplexVectorW[m_numVertices] = w; + m_simplexPointsP[m_numVertices] = p; + m_simplexPointsQ[m_numVertices] = q; + + m_numVertices++; +} + +bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() +{ + + if (m_needsUpdate) + { + m_cachedBC.reset(); + + m_needsUpdate = false; + + switch (numVertices()) + { + case 0: + m_cachedValidClosest = false; + break; + case 1: + { + m_cachedP1 = m_simplexPointsP[0]; + m_cachedP2 = m_simplexPointsQ[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_cachedValidClosest = m_cachedBC.isValid(); + break; + }; + 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 + { + 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_cachedValidClosest = m_cachedBC.isValid(); + break; + } + 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]; + + 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_cachedV = m_cachedP1-m_cachedP2; + + reduceVertices (m_cachedBC.m_usedVertices); + m_cachedValidClosest = m_cachedBC.isValid(); + + break; + } + case 4: + { + + + 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 hasSeparation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); + + 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_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_cachedV = m_cachedP1-m_cachedP2; + reduceVertices (m_cachedBC.m_usedVertices); + } else + { +// printf("sub distance got penetration\n"); + + if (m_cachedBC.m_degenerate) + { + m_cachedValidClosest = false; + } else + { + m_cachedValidClosest = true; + //degenerate case == false, penetration = true + zero + m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + } + break; + } + + m_cachedValidClosest = m_cachedBC.isValid(); + + //closest point origin from tetrahedron + break; + } + default: + { + m_cachedValidClosest = false; + } + }; + } + + return m_cachedValidClosest; + +} + +//return/calculate the closest vertex +bool btVoronoiSimplexSolver::closest(btVector3& v) +{ + bool succes = updateClosestVectorAndPoints(); + v = m_cachedV; + return succes; +} + + + +btScalar btVoronoiSimplexSolver::maxVertex() +{ + int i, numverts = numVertices(); + btScalar maxV = btScalar(0.); + for (i=0;i<numverts;i++) + { + btScalar curLen2 = m_simplexVectorW[i].length2(); + if (maxV < curLen2) + maxV = curLen2; + } + return maxV; +} + + + + //return the current simplex +int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const +{ + int i; + for (i=0;i<numVertices();i++) + { + yBuf[i] = m_simplexVectorW[i]; + pBuf[i] = m_simplexPointsP[i]; + qBuf[i] = m_simplexPointsQ[i]; + } + 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++) + { +#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD + if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold) +#else + if (m_simplexVectorW[i] == w) +#endif + { + found = true; + break; + } + } + + //check in case lastW is already removed + if (w == m_lastW) + return true; + + return found; +} + +void btVoronoiSimplexSolver::backup_closest(btVector3& v) +{ + v = m_cachedV; +} + + +bool btVoronoiSimplexSolver::emptySimplex() const +{ + return (numVertices() == 0); + +} + +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) +{ + 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)) + { + result.m_closestPointOnSimplex = a; + result.m_usedVertices.usedVertexA = true; + 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) + { + result.m_closestPointOnSimplex = b; + result.m_usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(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); + result.m_closestPointOnSimplex = a + v * ab; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexB = true; + 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) + { + result.m_closestPointOnSimplex = c; + result.m_usedVertices.usedVertexC = true; + 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); + result.m_closestPointOnSimplex = a + w * ac; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1-w,0,w); + return true; + //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)); + + 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.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 + +} + + + + + +/// 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); + + 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))) + { + return -1; + } +#else + if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) + { +// printf("affine dependent/degenerate\n");// + return -1; + } +#endif + +#endif + // Points on opposite sides if expression signs are opposite + 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) +{ + btSubSimplexClosestResult tempResult; + + // 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.usedVertexB = true; + finalResult.m_usedVertices.usedVertexC = true; + finalResult.m_usedVertices.usedVertexD = true; + + 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; + } + + if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) + { + return false; + } + + + btScalar bestSqDist = FLT_MAX; + // If point outside face abc then compute closest point on abc + if (pointOutsideABC) + { + 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) { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + //convert result bitmask! + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + 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 + ); + + } + } + + + // Repeat test for face acd + if (pointOutsideACD) + { + closestPtPointTriangle(p, a, c, d,tempResult); + btVector3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + + 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.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] + ); + + } + } + // Repeat test for face adb + + + if (pointOutsideADB) + { + closestPtPointTriangle(p, a, d, b,tempResult); + btVector3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + + 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] + ); + + } + } + // Repeat test for face bdc + + + if (pointOutsideBDC) + { + closestPtPointTriangle(p, b, d, c,tempResult); + btVector3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + btScalar sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + // + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; + + finalResult.setBarycentricCoordinates( + 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) + { + return true; + } + + return true; +} + diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h new file mode 100644 index 0000000000..80fd490f4e --- /dev/null +++ b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -0,0 +1,185 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#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 + +#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{ + btUsageBitfield() + { + reset(); + } + + void reset() + { + usedVertexA = false; + usedVertexB = false; + 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; +}; + + +struct btSubSimplexClosestResult +{ + btVector3 m_closestPointOnSimplex; + //MASK for m_usedVertices + //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]; + bool m_degenerate; + + void reset() + { + m_degenerate = false; + setBarycentricCoordinates(); + m_usedVertices.reset(); + } + bool isValid() + { + bool valid = (m_barycentricCoords[0] >= 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.)) + { + 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 +#else +ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver : public btSimplexSolverInterface +#endif +{ +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + 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_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 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) + { + } + void reset(); + + void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); + + void setEqualVertexThreshold(btScalar threshold) + { + m_equalVertexThreshold = threshold; + } + + btScalar getEqualVertexThreshold() const + { + return m_equalVertexThreshold; + } + + bool closest(btVector3& v); + + btScalar maxVertex(); + + bool fullSimplex() const + { + return (m_numVertices == 4); + } + + int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; + + bool inSimplex(const btVector3& w); + + void backup_closest(btVector3& v) ; + + bool emptySimplex() const ; + + void compute_points(btVector3& p1, btVector3& p2) ; + + int numVertices() const + { + return m_numVertices; + } + + +}; + +#endif //BT_VORONOI_SIMPLEX_SOLVER_H + |