diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision')
33 files changed, 0 insertions, 8067 deletions
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h deleted file mode 100644 index 9eb880b8df..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h +++ /dev/null @@ -1,369 +0,0 @@ -/* -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 deleted file mode 100644 index 940282f576..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/* -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 deleted file mode 100644 index bdc0572f75..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h +++ /dev/null @@ -1,59 +0,0 @@ -/* -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 deleted file mode 100644 index d2a1310b23..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp +++ /dev/null @@ -1,20 +0,0 @@ -/* -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 deleted file mode 100644 index bfd79d03be..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ /dev/null @@ -1,73 +0,0 @@ -/* -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 deleted file mode 100644 index 29620abffb..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ /dev/null @@ -1,40 +0,0 @@ -/* -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 deleted file mode 100644 index 0ea7b483cf..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ /dev/null @@ -1,90 +0,0 @@ -/* -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 deleted file mode 100644 index 0b49b0ecc6..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h +++ /dev/null @@ -1,41 +0,0 @@ -/* -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 deleted file mode 100644 index bef697a0a1..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/* -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 deleted file mode 100644 index 6a42ee63b0..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -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 deleted file mode 100644 index eefb974bbd..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ /dev/null @@ -1,1048 +0,0 @@ -/* -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 deleted file mode 100644 index ac501d5ecf..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +++ /dev/null @@ -1,75 +0,0 @@ -/* -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 deleted file mode 100644 index ce1f24bc50..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h +++ /dev/null @@ -1,1035 +0,0 @@ -/* -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 deleted file mode 100644 index 572ec36f56..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* -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 deleted file mode 100644 index 1ed6340af3..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ /dev/null @@ -1,43 +0,0 @@ -/* -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 deleted file mode 100644 index 257b026d9b..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ /dev/null @@ -1,467 +0,0 @@ -/* -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 deleted file mode 100644 index feeae68621..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ /dev/null @@ -1,103 +0,0 @@ -/* -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 deleted file mode 100644 index 571ad2c5f7..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ /dev/null @@ -1,180 +0,0 @@ -/* -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 deleted file mode 100644 index fa45f49037..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ /dev/null @@ -1,361 +0,0 @@ -/* -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 deleted file mode 100644 index fd533b4fc3..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ /dev/null @@ -1,40 +0,0 @@ -/* -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 deleted file mode 100644 index a22a0bae66..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h +++ /dev/null @@ -1,908 +0,0 @@ - -/*** - * --------------------------------- - * 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 deleted file mode 100644 index 23aaece22b..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ /dev/null @@ -1,308 +0,0 @@ -/* -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 deleted file mode 100644 index f872c8e1c9..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ /dev/null @@ -1,268 +0,0 @@ -/* -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 deleted file mode 100644 index 18da171011..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h +++ /dev/null @@ -1,64 +0,0 @@ -/* -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 deleted file mode 100644 index ea380bc5f1..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ /dev/null @@ -1,570 +0,0 @@ -/* -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 deleted file mode 100644 index 30e3db687b..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h +++ /dev/null @@ -1,49 +0,0 @@ -/* -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 deleted file mode 100644 index 786efd1820..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/* -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 deleted file mode 100644 index f2ed0cd39c..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ /dev/null @@ -1,74 +0,0 @@ -/* -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 deleted file mode 100644 index da8a13914c..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ /dev/null @@ -1,63 +0,0 @@ -/* -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 deleted file mode 100644 index ec638f60ba..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/* -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 deleted file mode 100644 index 6c8127983e..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -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 deleted file mode 100644 index 756373c9b5..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ /dev/null @@ -1,612 +0,0 @@ - -/* -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 deleted file mode 100644 index 80fd490f4e..0000000000 --- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ /dev/null @@ -1,185 +0,0 @@ -/* -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 - |