diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/NarrowPhaseCollision')
33 files changed, 0 insertions, 8892 deletions
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h deleted file mode 100644 index 3c82133037..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h +++ /dev/null @@ -1,354 +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 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis(); - btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis(); - - btVector3 pInA = a.getLocalSupportWithoutMargin(separatingAxisInA); - btVector3 qInB = b.getLocalSupportWithoutMargin(separatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); - - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared)) - { - m_degenerateSimplex = 10; - checkSimplex = true; - //checkPenetration = false; - break; - } - - //exit 0: the new point is already in the simplex, or we didn't come any closer - if (simplexSolver.inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * colDesc.m_gjkRelError2; - - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) - { - m_degenerateSimplex = 2; - } - else - { - m_degenerateSimplex = 11; - } - checkSimplex = true; - break; - } - - //add current vertex to simplex - simplexSolver.addVertex(w, pWorld, qWorld); - btVector3 newCachedSeparatingAxis; - - //calculate the closest point to the origin (update vector v) - if (!simplexSolver.closest(newCachedSeparatingAxis)) - { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } - - if (newCachedSeparatingAxis.length2() < colDesc.m_gjkRelError2) - { - m_cachedSeparatingAxis = newCachedSeparatingAxis; - m_degenerateSimplex = 6; - checkSimplex = true; - break; - } - - btScalar previousSquaredDistance = squaredDistance; - squaredDistance = newCachedSeparatingAxis.length2(); -#if 0 - ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo - if (squaredDistance>previousSquaredDistance) - { - 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/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp deleted file mode 100644 index 38df8d4808..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ /dev/null @@ -1,222 +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 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 += dLambda; - - if (lambda > btScalar(1.) || lambda < btScalar(0.)) - return false; - - //todo: next check with relative epsilon - if (lambda <= lastLambda) - { - return false; - //n.setValue(0,0,0); - //break; - } - 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 > MAX_ITERATIONS) - { - 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/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h deleted file mode 100644 index 67b2205c36..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h +++ /dev/null @@ -1,53 +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 in between motion globally consistent. -/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops -class btContinuousConvexCollision : public btConvexCast -{ - btSimplexSolverInterface* m_simplexSolver; - btConvexPenetrationDepthSolver* m_penetrationDepthSolver; - const btConvexShape* m_convexA; - //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/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp deleted file mode 100644 index d2a1310b23..0000000000 --- a/thirdparty/bullet/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/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.h deleted file mode 100644 index 77b19be599..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btConvexCast.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_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" - -#ifdef BT_USE_DOUBLE_PRECISION -#define MAX_CONVEX_CAST_ITERATIONS 64 -#define MAX_CONVEX_CAST_EPSILON (SIMD_EPSILON * 10) -#else -#define MAX_CONVEX_CAST_ITERATIONS 32 -#define MAX_CONVEX_CAST_EPSILON btScalar(0.0001) -#endif -///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. -///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 -//will need to digg deeper to make the algorithm more robust -//since, a large epsilon can cause an early termination with false -//positive results (ray intersections that shouldn't be there) - -/// btConvexCast is an interface for Casting -class btConvexCast -{ -public: - virtual ~btConvexCast(); - - ///RayResult stores the closest result - /// alternatively, add a callback method to decide about closest/all results - struct CastResult - { - //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)), - m_subSimplexCastMaxIterations(MAX_CONVEX_CAST_ITERATIONS), - m_subSimplexCastEpsilon(MAX_CONVEX_CAST_EPSILON) - { - } - - 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; - - int m_subSimplexCastMaxIterations; - btScalar m_subSimplexCastEpsilon; - - }; - - /// cast a convex against another convex object - virtual bool calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) = 0; -}; - -#endif //BT_CONVEX_CAST_H diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h deleted file mode 100644 index 65c9df9340..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ /dev/null @@ -1,35 +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/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h deleted file mode 100644 index d1bbb1a46e..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ /dev/null @@ -1,85 +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/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h deleted file mode 100644 index c9fd84bebf..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h +++ /dev/null @@ -1,39 +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/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp deleted file mode 100644 index 9d61e75dac..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ /dev/null @@ -1,164 +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/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h deleted file mode 100644 index ef5979173e..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h +++ /dev/null @@ -1,46 +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/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp deleted file mode 100644 index 7d53f8624a..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ /dev/null @@ -1,1104 +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); -} - -} // namespace gjkepa2_impl - -// -// 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; - results.distance = length - margin; - return results.distance; - } - 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/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h deleted file mode 100644 index 893daea3f5..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +++ /dev/null @@ -1,73 +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/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h deleted file mode 100644 index 6fedbbb3e5..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h +++ /dev/null @@ -1,1063 +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/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp deleted file mode 100644 index 07629229ab..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ /dev/null @@ -1,81 +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; - - btVector3 guessVectors[] = { - btVector3(transformB.getOrigin() - transformA.getOrigin()).safeNormalize(), - btVector3(transformA.getOrigin() - transformB.getOrigin()).safeNormalize(), - btVector3(0, 0, 1), - btVector3(0, 1, 0), - btVector3(1, 0, 0), - btVector3(1, 1, 0), - btVector3(1, 1, 1), - btVector3(0, 1, 1), - btVector3(1, 0, 1), - }; - - int numVectors = sizeof(guessVectors) / sizeof(btVector3); - - for (int i = 0; i < numVectors; i++) - { - simplexSolver.reset(); - btVector3 guessVector = guessVectors[i]; - - btGjkEpaSolver2::sResults results; - - if (btGjkEpaSolver2::Penetration(pConvexA, transformA, - pConvexB, transformB, - guessVector, results)) - - { - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return true; - } - else - { - if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results)) - { - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return false; - } - } - } - - //failed to find a distance/penetration - wWitnessOnA.setValue(0, 0, 0); - wWitnessOnB.setValue(0, 0, 0); - v.setValue(0, 0, 0); - return false; -} diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h deleted file mode 100644 index 92d6df1729..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ /dev/null @@ -1,40 +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/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp deleted file mode 100644 index 5af93cb2fb..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ /dev/null @@ -1,1183 +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 = 1.0e-12; -#else -#define REL_ERROR2 btScalar(1.0e-6) -btScalar gGjkEpaPenetrationTolerance = 0.001; -#endif - - -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); -} - -static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb) -{ - btVector3 separatingAxisInA = (dir)*localTransA.getBasis(); - btVector3 separatingAxisInB = (-dir) * localTransB.getBasis(); - - btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); - btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); - - btVector3 pInA = pInANoMargin; - btVector3 qInB = qInBNoMargin; - - supAworld = localTransA(pInA); - supBworld = localTransB(qInB); - - if (check2d) - { - supAworld[2] = 0.f; - supBworld[2] = 0.f; - } - - aMinb = supAworld - supBworld; -} - -struct btSupportVector -{ - btVector3 v; //!< Support point in minkowski sum - btVector3 v1; //!< Support point in obj1 - btVector3 v2; //!< Support point in obj2 -}; - -struct btSimplex -{ - btSupportVector ps[4]; - int last; //!< index of last added point -}; - -static btVector3 ccd_vec3_origin(0, 0, 0); - -inline void btSimplexInit(btSimplex *s) -{ - s->last = -1; -} - -inline int btSimplexSize(const btSimplex *s) -{ - return s->last + 1; -} - -inline const btSupportVector *btSimplexPoint(const btSimplex *s, int idx) -{ - // here is no check on boundaries - return &s->ps[idx]; -} -inline void btSupportCopy(btSupportVector *d, const btSupportVector *s) -{ - *d = *s; -} - -inline void btVec3Copy(btVector3 *v, const btVector3 *w) -{ - *v = *w; -} - -inline void ccdVec3Add(btVector3 *v, const btVector3 *w) -{ - v->m_floats[0] += w->m_floats[0]; - v->m_floats[1] += w->m_floats[1]; - v->m_floats[2] += w->m_floats[2]; -} - -inline void ccdVec3Sub(btVector3 *v, const btVector3 *w) -{ - *v -= *w; -} -inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) -{ - *d = (*v) - (*w); -} -inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b) -{ - btScalar dot; - dot = a->dot(*b); - - return dot; -} - -inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3 *b) -{ - btVector3 ab; - btVec3Sub2(&ab, a, b); - return btVec3Dot(&ab, &ab); -} - -inline void btVec3Scale(btVector3 *d, btScalar k) -{ - d->m_floats[0] *= k; - d->m_floats[1] *= k; - d->m_floats[2] *= k; -} - -inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b) -{ - d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]); - d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]); - d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]); -} - -inline void btTripleCross(const btVector3 *a, const btVector3 *b, - const btVector3 *c, btVector3 *d) -{ - btVector3 e; - btVec3Cross(&e, a, b); - btVec3Cross(d, &e, c); -} - -inline int ccdEq(btScalar _a, btScalar _b) -{ - btScalar ab; - btScalar a, b; - - ab = btFabs(_a - _b); - if (btFabs(ab) < SIMD_EPSILON) - return 1; - - a = btFabs(_a); - b = btFabs(_b); - if (b > a) - { - return ab < SIMD_EPSILON * b; - } - else - { - return ab < SIMD_EPSILON * a; - } -} - -btScalar ccdVec3X(const btVector3 *v) -{ - return v->x(); -} - -btScalar ccdVec3Y(const btVector3 *v) -{ - return v->y(); -} - -btScalar ccdVec3Z(const btVector3 *v) -{ - return v->z(); -} -inline int btVec3Eq(const btVector3 *a, const btVector3 *b) -{ - return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) && ccdEq(ccdVec3Z(a), ccdVec3Z(b)); -} - -inline void btSimplexAdd(btSimplex *s, const btSupportVector *v) -{ - // here is no check on boundaries in sake of speed - ++s->last; - btSupportCopy(s->ps + s->last, v); -} - -inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a) -{ - btSupportCopy(s->ps + pos, a); -} - -inline void btSimplexSetSize(btSimplex *s, int size) -{ - s->last = size - 1; -} - -inline const btSupportVector *ccdSimplexLast(const btSimplex *s) -{ - return btSimplexPoint(s, s->last); -} - -inline int ccdSign(btScalar val) -{ - if (btFuzzyZero(val)) - { - return 0; - } - else if (val < btScalar(0)) - { - return -1; - } - return 1; -} - -inline btScalar btVec3PointSegmentDist2(const btVector3 *P, - const btVector3 *x0, - const btVector3 *b, - btVector3 *witness) -{ - // The computation comes from solving equation of segment: - // S(t) = x0 + t.d - // where - x0 is initial point of segment - // - d is direction of segment from x0 (|d| > 0) - // - t belongs to <0, 1> interval - // - // Than, distance from a segment to some point P can be expressed: - // D(t) = |x0 + t.d - P|^2 - // which is distance from any point on segment. Minimization - // of this function brings distance from P to segment. - // Minimization of D(t) leads to simple quadratic equation that's - // solving is straightforward. - // - // Bonus of this method is witness point for free. - - btScalar dist, t; - btVector3 d, a; - - // direction of segment - btVec3Sub2(&d, b, x0); - - // precompute vector from P to x0 - btVec3Sub2(&a, x0, P); - - t = -btScalar(1.) * btVec3Dot(&a, &d); - t /= btVec3Dot(&d, &d); - - if (t < btScalar(0) || btFuzzyZero(t)) - { - dist = ccdVec3Dist2(x0, P); - if (witness) - btVec3Copy(witness, x0); - } - else if (t > btScalar(1) || ccdEq(t, btScalar(1))) - { - dist = ccdVec3Dist2(b, P); - if (witness) - btVec3Copy(witness, b); - } - else - { - if (witness) - { - btVec3Copy(witness, &d); - btVec3Scale(witness, t); - ccdVec3Add(witness, x0); - dist = ccdVec3Dist2(witness, P); - } - else - { - // recycling variables - btVec3Scale(&d, t); - ccdVec3Add(&d, &a); - dist = btVec3Dot(&d, &d); - } - } - - return dist; -} - -btScalar btVec3PointTriDist2(const btVector3 *P, - const btVector3 *x0, const btVector3 *B, - const btVector3 *C, - btVector3 *witness) -{ - // Computation comes from analytic expression for triangle (x0, B, C) - // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and - // Then equation for distance is: - // D(s, t) = | T(s, t) - P |^2 - // This leads to minimization of quadratic function of two variables. - // The solution from is taken only if s is between 0 and 1, t is - // between 0 and 1 and t + s < 1, otherwise distance from segment is - // computed. - - btVector3 d1, d2, a; - double u, v, w, p, q, r; - double s, t, dist, dist2; - btVector3 witness2; - - btVec3Sub2(&d1, B, x0); - btVec3Sub2(&d2, C, x0); - btVec3Sub2(&a, x0, P); - - u = btVec3Dot(&a, &a); - v = btVec3Dot(&d1, &d1); - w = btVec3Dot(&d2, &d2); - p = btVec3Dot(&a, &d1); - q = btVec3Dot(&a, &d2); - r = btVec3Dot(&d1, &d2); - - s = (q * r - w * p) / (w * v - r * r); - t = (-s * r - q) / w; - - if ((btFuzzyZero(s) || s > btScalar(0)) && (ccdEq(s, btScalar(1)) || s < btScalar(1)) && (btFuzzyZero(t) || t > btScalar(0)) && (ccdEq(t, btScalar(1)) || t < btScalar(1)) && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1))) - { - if (witness) - { - btVec3Scale(&d1, s); - btVec3Scale(&d2, t); - btVec3Copy(witness, x0); - ccdVec3Add(witness, &d1); - ccdVec3Add(witness, &d2); - - dist = ccdVec3Dist2(witness, P); - } - else - { - dist = s * s * v; - dist += t * t * w; - dist += btScalar(2.) * s * t * r; - dist += btScalar(2.) * s * p; - dist += btScalar(2.) * t * q; - dist += u; - } - } - else - { - dist = btVec3PointSegmentDist2(P, x0, B, witness); - - dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2); - if (dist2 < dist) - { - dist = dist2; - if (witness) - btVec3Copy(witness, &witness2); - } - - dist2 = btVec3PointSegmentDist2(P, B, C, &witness2); - if (dist2 < dist) - { - dist = dist2; - if (witness) - btVec3Copy(witness, &witness2); - } - } - - return dist; -} - -static int btDoSimplex2(btSimplex *simplex, btVector3 *dir) -{ - const btSupportVector *A, *B; - btVector3 AB, AO, tmp; - btScalar dot; - - // get last added as A - A = ccdSimplexLast(simplex); - // get the other point - B = btSimplexPoint(simplex, 0); - // compute AB oriented segment - btVec3Sub2(&AB, &B->v, &A->v); - // compute AO vector - btVec3Copy(&AO, &A->v); - btVec3Scale(&AO, -btScalar(1)); - - // dot product AB . AO - dot = btVec3Dot(&AB, &AO); - - // check if origin doesn't lie on AB segment - btVec3Cross(&tmp, &AB, &AO); - if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0)) - { - return 1; - } - - // check if origin is in area where AB segment is - if (btFuzzyZero(dot) || dot < btScalar(0)) - { - // origin is in outside are of A - btSimplexSet(simplex, 0, A); - btSimplexSetSize(simplex, 1); - btVec3Copy(dir, &AO); - } - else - { - // origin is in area where AB segment is - - // keep simplex untouched and set direction to - // AB x AO x AB - btTripleCross(&AB, &AO, &AB, dir); - } - - return 0; -} - -static int btDoSimplex3(btSimplex *simplex, btVector3 *dir) -{ - const btSupportVector *A, *B, *C; - btVector3 AO, AB, AC, ABC, tmp; - btScalar dot, dist; - - // get last added as A - A = ccdSimplexLast(simplex); - // get the other points - B = btSimplexPoint(simplex, 1); - C = btSimplexPoint(simplex, 0); - - // check touching contact - dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0); - if (btFuzzyZero(dist)) - { - return 1; - } - - // check if triangle is really triangle (has area > 0) - // if not simplex can't be expanded and thus no itersection is found - if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v)) - { - return -1; - } - - // compute AO vector - btVec3Copy(&AO, &A->v); - btVec3Scale(&AO, -btScalar(1)); - - // compute AB and AC segments and ABC vector (perpendircular to triangle) - btVec3Sub2(&AB, &B->v, &A->v); - btVec3Sub2(&AC, &C->v, &A->v); - btVec3Cross(&ABC, &AB, &AC); - - btVec3Cross(&tmp, &ABC, &AC); - dot = btVec3Dot(&tmp, &AO); - if (btFuzzyZero(dot) || dot > btScalar(0)) - { - dot = btVec3Dot(&AC, &AO); - if (btFuzzyZero(dot) || dot > btScalar(0)) - { - // C is already in place - btSimplexSet(simplex, 1, A); - btSimplexSetSize(simplex, 2); - btTripleCross(&AC, &AO, &AC, dir); - } - else - { - dot = btVec3Dot(&AB, &AO); - if (btFuzzyZero(dot) || dot > btScalar(0)) - { - btSimplexSet(simplex, 0, B); - btSimplexSet(simplex, 1, A); - btSimplexSetSize(simplex, 2); - btTripleCross(&AB, &AO, &AB, dir); - } - else - { - btSimplexSet(simplex, 0, A); - btSimplexSetSize(simplex, 1); - btVec3Copy(dir, &AO); - } - } - } - else - { - btVec3Cross(&tmp, &AB, &ABC); - dot = btVec3Dot(&tmp, &AO); - if (btFuzzyZero(dot) || dot > btScalar(0)) - { - dot = btVec3Dot(&AB, &AO); - if (btFuzzyZero(dot) || dot > btScalar(0)) - { - btSimplexSet(simplex, 0, B); - btSimplexSet(simplex, 1, A); - btSimplexSetSize(simplex, 2); - btTripleCross(&AB, &AO, &AB, dir); - } - else - { - btSimplexSet(simplex, 0, A); - btSimplexSetSize(simplex, 1); - btVec3Copy(dir, &AO); - } - } - else - { - dot = btVec3Dot(&ABC, &AO); - if (btFuzzyZero(dot) || dot > btScalar(0)) - { - btVec3Copy(dir, &ABC); - } - else - { - btSupportVector tmp; - btSupportCopy(&tmp, C); - btSimplexSet(simplex, 0, B); - btSimplexSet(simplex, 1, &tmp); - - btVec3Copy(dir, &ABC); - btVec3Scale(dir, -btScalar(1)); - } - } - } - - return 0; -} - -static int btDoSimplex4(btSimplex *simplex, btVector3 *dir) -{ - const btSupportVector *A, *B, *C, *D; - btVector3 AO, AB, AC, AD, ABC, ACD, ADB; - int B_on_ACD, C_on_ADB, D_on_ABC; - int AB_O, AC_O, AD_O; - btScalar dist; - - // get last added as A - A = ccdSimplexLast(simplex); - // get the other points - B = btSimplexPoint(simplex, 2); - C = btSimplexPoint(simplex, 1); - D = btSimplexPoint(simplex, 0); - - // check if tetrahedron is really tetrahedron (has volume > 0) - // if it is not simplex can't be expanded and thus no intersection is - // found - dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0); - if (btFuzzyZero(dist)) - { - return -1; - } - - // check if origin lies on some of tetrahedron's face - if so objects - // intersect - dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0); - if (btFuzzyZero(dist)) - return 1; - dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0); - if (btFuzzyZero(dist)) - return 1; - dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0); - if (btFuzzyZero(dist)) - return 1; - dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0); - if (btFuzzyZero(dist)) - return 1; - - // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors - btVec3Copy(&AO, &A->v); - btVec3Scale(&AO, -btScalar(1)); - btVec3Sub2(&AB, &B->v, &A->v); - btVec3Sub2(&AC, &C->v, &A->v); - btVec3Sub2(&AD, &D->v, &A->v); - btVec3Cross(&ABC, &AB, &AC); - btVec3Cross(&ACD, &AC, &AD); - btVec3Cross(&ADB, &AD, &AB); - - // side (positive or negative) of B, C, D relative to planes ACD, ADB - // and ABC respectively - B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB)); - C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC)); - D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD)); - - // whether origin is on same side of ACD, ADB, ABC as B, C, D - // respectively - AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD; - AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB; - AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC; - - if (AB_O && AC_O && AD_O) - { - // origin is in tetrahedron - return 1; - // rearrange simplex to triangle and call btDoSimplex3() - } - else if (!AB_O) - { - // B is farthest from the origin among all of the tetrahedron's - // points, so remove it from the list and go on with the triangle - // case - - // D and C are in place - btSimplexSet(simplex, 2, A); - btSimplexSetSize(simplex, 3); - } - else if (!AC_O) - { - // C is farthest - btSimplexSet(simplex, 1, D); - btSimplexSet(simplex, 0, B); - btSimplexSet(simplex, 2, A); - btSimplexSetSize(simplex, 3); - } - else - { // (!AD_O) - btSimplexSet(simplex, 0, C); - btSimplexSet(simplex, 1, B); - btSimplexSet(simplex, 2, A); - btSimplexSetSize(simplex, 3); - } - - return btDoSimplex3(simplex, dir); -} - -static int btDoSimplex(btSimplex *simplex, btVector3 *dir) -{ - if (btSimplexSize(simplex) == 2) - { - // simplex contains segment only one segment - return btDoSimplex2(simplex, dir); - } - else if (btSimplexSize(simplex) == 3) - { - // simplex contains triangle - return btDoSimplex3(simplex, dir); - } - else - { // btSimplexSize(simplex) == 4 - // tetrahedron - this is the only shape which can encapsule origin - // so btDoSimplex4() also contains test on it - return btDoSimplex4(simplex, dir); - } -} - -#ifdef __SPU__ -void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw) -#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; - - - //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; - int status = -2; - btVector3 orgNormalInB(0, 0, 0); - btScalar margin = marginA + marginB; - - //we add a separate implementation to check if the convex shapes intersect - //See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang - //Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver - //and remove this temporary code from libCCD - //this fixes issue https://github.com/bulletphysics/bullet3/issues/1703 - //note, for large differences in shapes, use double precision build! - { - btScalar squaredDistance = BT_LARGE_FLOAT; - btScalar delta = btScalar(0.); - - btSimplex simplex1; - btSimplex *simplex = &simplex1; - btSimplexInit(simplex); - - btVector3 dir(1, 0, 0); - - { - btVector3 lastSupV; - btVector3 supAworld; - btVector3 supBworld; - btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV); - - btSupportVector last; - last.v = lastSupV; - last.v1 = supAworld; - last.v2 = supBworld; - - btSimplexAdd(simplex, &last); - - dir = -lastSupV; - - // start iterations - for (int iterations = 0; iterations < gGjkMaxIter; iterations++) - { - // obtain support point - btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV); - - // check if farthest point in Minkowski difference in direction dir - // isn't somewhere before origin (the test on negative dot product) - // - because if it is, objects are not intersecting at all. - btScalar delta = lastSupV.dot(dir); - if (delta < 0) - { - //no intersection, besides margin - status = -1; - break; - } - - // add last support vector to simplex - last.v = lastSupV; - last.v1 = supAworld; - last.v2 = supBworld; - - btSimplexAdd(simplex, &last); - - // if btDoSimplex returns 1 if objects intersect, -1 if objects don't - // intersect and 0 if algorithm should continue - - btVector3 newDir; - int do_simplex_res = btDoSimplex(simplex, &dir); - - if (do_simplex_res == 1) - { - status = 0; // intersection found - break; - } - else if (do_simplex_res == -1) - { - // intersection not found - status = -1; - break; - } - - if (btFuzzyZero(btVec3Dot(&dir, &dir))) - { - // intersection not found - status = -1; - } - - if (dir.length2() < SIMD_EPSILON) - { - //no intersection, besides margin - status = -1; - break; - } - - if (dir.fuzzyZero()) - { - // intersection not found - status = -1; - break; - } - } - } - - m_simplexSolver->reset(); - if (status == 0) - { - //status = 0; - //printf("Intersect!\n"); - } - - if (status == -1) - { - //printf("not intersect\n"); - } - //printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]); - if (1) - { - for (;;) - //while (true) - { - btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis(); - btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis(); - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - - if (check2d) - { - pWorld[2] = 0.f; - qWorld[2] = 0.f; - } - - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); - - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) - { - m_degenerateSimplex = 10; - checkSimplex = true; - //checkPenetration = false; - break; - } - - //exit 0: the new point is already in the simplex, or we didn't come any closer - if (m_simplexSolver->inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * REL_ERROR2; - - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) - { - m_degenerateSimplex = 2; - } - else - { - m_degenerateSimplex = 11; - } - checkSimplex = true; - break; - } - - //add current vertex to simplex - m_simplexSolver->addVertex(w, pWorld, qWorld); - btVector3 newCachedSeparatingAxis; - - //calculate the closest point to the origin (update vector v) - if (!m_simplexSolver->closest(newCachedSeparatingAxis)) - { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } - - if (newCachedSeparatingAxis.length2() < REL_ERROR2) - { - m_cachedSeparatingAxis = newCachedSeparatingAxis; - m_degenerateSimplex = 6; - checkSimplex = true; - break; - } - - btScalar previousSquaredDistance = squaredDistance; - squaredDistance = newCachedSeparatingAxis.length2(); -#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; - orgNormalInB = normalInB; - - 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)) || (status == 0)) - { - //penetration case - - //if there is no way to handle penetrations, bail out - if (m_penetrationDepthSolver) - { - // Penetration depth case. - btVector3 tmpPointOnA, tmpPointOnB; - - m_cachedSeparatingAxis.setZero(); - - bool isValid2 = m_penetrationDepthSolver->calcPenDepth( - *m_simplexSolver, - m_minkowskiA, m_minkowskiB, - localTransA, localTransB, - m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, - debugDraw); - - if (m_cachedSeparatingAxis.length2()) - { - 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; - } - } - } - } - else - { - //printf("EPA didn't return a valid value\n"); - } - } - } - } - - if (isValid && ((distance < 0) || (distance * distance < input.m_maximumDistanceSquared))) - { - m_cachedSeparatingAxis = normalInB; - m_cachedSeparatingDistance = distance; - if (1) - { - ///todo: need to track down this EPA penetration solver degeneracy - ///the penetration solver reports penetration but the contact normal - ///connecting the contact points is pointing in the opposite direction - ///until then, detect the issue and revert the normal - - btScalar d2 = 0.f; - { - btVector3 separatingAxisInA = (-orgNormalInB) * localTransA.getBasis(); - btVector3 separatingAxisInB = orgNormalInB * localTransB.getBasis(); - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - btVector3 w = pWorld - qWorld; - d2 = orgNormalInB.dot(w) - margin; - } - - btScalar d1 = 0; - { - btVector3 separatingAxisInA = (normalInB)*localTransA.getBasis(); - btVector3 separatingAxisInB = -normalInB * localTransB.getBasis(); - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - btVector3 w = pWorld - qWorld; - d1 = (-normalInB).dot(w) - margin; - } - btScalar d0 = 0.f; - { - btVector3 separatingAxisInA = (-normalInB) * input.m_transformA.getBasis(); - btVector3 separatingAxisInB = normalInB * input.m_transformB.getBasis(); - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - btVector3 w = pWorld - qWorld; - d0 = normalInB.dot(w) - margin; - } - - if (d1 > d0) - { - m_lastUsedMethod = 10; - normalInB *= -1; - } - - if (orgNormalInB.length2()) - { - if (d2 > d0 && d2 > d1 && d2 > distance) - { - normalInB = orgNormalInB; - distance = d2; - } - } - } - - output.addContactPoint( - normalInB, - pointOnB + positionOffset, - distance); - } - else - { - //printf("invalid gjk query\n"); - } -} diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h deleted file mode 100644 index faa02287ca..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ /dev/null @@ -1,93 +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 setCachedSeparatingAxis(const btVector3& separatingAxis) - { - m_cachedSeparatingAxis = separatingAxis; - } - - 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/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h deleted file mode 100644 index 573fc86bf9..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ /dev/null @@ -1,174 +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_prevRHS(0.f), - m_appliedImpulseLateral1(0.f), - m_appliedImpulseLateral2(0.f), - m_contactMotion1(0.f), - m_contactMotion2(0.f), - m_contactCFM(0.f), - m_contactERP(0.f), - m_frictionCFM(0.f), - m_lifeTime(0) - { - } - - btManifoldPoint(const btVector3& pointA, const btVector3& pointB, - const btVector3& normal, - btScalar distance) : m_localPointA(pointA), - m_localPointB(pointB), - m_normalWorldOnB(normal), - m_distance1(distance), - m_combinedFriction(btScalar(0.)), - m_combinedRollingFriction(btScalar(0.)), - m_combinedSpinningFriction(btScalar(0.)), - m_combinedRestitution(btScalar(0.)), - m_userPersistentData(0), - m_contactPointFlags(0), - m_appliedImpulse(0.f), - m_prevRHS(0.f), - m_appliedImpulseLateral1(0.f), - m_appliedImpulseLateral2(0.f), - m_contactMotion1(0.f), - m_contactMotion2(0.f), - m_contactCFM(0.f), - m_contactERP(0.f), - m_frictionCFM(0.f), - m_lifeTime(0) - { - } - - btVector3 m_localPointA; - btVector3 m_localPointB; - btVector3 m_positionWorldOnB; - ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity - btVector3 m_positionWorldOnA; - btVector3 m_normalWorldOnB; - - btScalar m_distance1; - btScalar m_combinedFriction; - btScalar m_combinedRollingFriction; //torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever - btScalar m_combinedSpinningFriction; //torsional friction around contact normal, useful for grasping objects - btScalar m_combinedRestitution; - - //BP mod, store contact triangles. - int m_partId0; - int m_partId1; - int m_index0; - int m_index1; - - mutable void* m_userPersistentData; - //bool m_lateralFrictionInitialized; - int m_contactPointFlags; - - btScalar m_appliedImpulse; - btScalar m_prevRHS; - btScalar m_appliedImpulseLateral1; - btScalar m_appliedImpulseLateral2; - btScalar m_contactMotion1; - btScalar m_contactMotion2; - - union { - btScalar m_contactCFM; - btScalar m_combinedContactStiffness1; - }; - - union { - btScalar m_contactERP; - btScalar m_combinedContactDamping1; - }; - - 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/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp deleted file mode 100644 index c042c24208..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ /dev/null @@ -1,336 +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 separatingAxisInA, separatingAxisInB; - btVector3 pInA, qInB, pWorld, qWorld, w; - -#ifndef __SPU__ -#define USE_BATCHED_SUPPORT 1 -#endif -#ifdef USE_BATCHED_SUPPORT - - btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; - btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; - btVector3 separatingAxisInABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; - btVector3 separatingAxisInBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; - int i; - - int numSampleDirections = NUM_UNITSPHERE_POINTS; - - for (i = 0; i < numSampleDirections; i++) - { - btVector3 norm = getPenetrationDirections()[i]; - separatingAxisInABatch[i] = (-norm) * transA.getBasis(); - separatingAxisInBBatch[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; - separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); - separatingAxisInBBatch[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; - separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); - separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); - numSampleDirections++; - } - } - } - - convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInABatch, supportVerticesABatch, numSampleDirections); - convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInBBatch, supportVerticesBBatch, numSampleDirections); - - for (i = 0; i < numSampleDirections; i++) - { - btVector3 norm = getPenetrationDirections()[i]; - if (check2d) - { - norm[2] = 0.f; - } - if (norm.length2() > 0.01) - { - separatingAxisInA = separatingAxisInABatch[i]; - separatingAxisInB = separatingAxisInBBatch[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]; - separatingAxisInA = (-norm) * transA.getBasis(); - separatingAxisInB = norm * transB.getBasis(); - pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA); - qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB); - pWorld = transA(pInA); - qWorld = transB(qInB); - w = qWorld - pWorld; - 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.setCachedSeparatingAxis(-minNorm); - gjkdet.getClosestPoints(input, res, debugDraw); - - btScalar correctedMinNorm = minProj - res.m_depth; - - //the penetration depth is over-estimated, relax it - btScalar penetration_relaxation = btScalar(1.); - minNorm *= penetration_relaxation; - - 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/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h deleted file mode 100644 index 8e3e393259..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ /dev/null @@ -1,36 +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/BulletCollision/NarrowPhaseCollision/btMprPenetration.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMprPenetration.h deleted file mode 100644 index 534a66d3fa..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMprPenetration.h +++ /dev/null @@ -1,884 +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 separatingAxisInA = dir * a.getWorldTransform().getBasis(); - btVector3 separatingAxisInB = -dir * b.getWorldTransform().getBasis(); - - btVector3 pInA = a.getLocalSupportWithMargin(separatingAxisInA); - btVector3 qInB = b.getLocalSupportWithMargin(separatingAxisInB); - - supp->v1 = a.getWorldTransform()(pInA); - supp->v2 = b.getWorldTransform()(qInB); - supp->v = supp->v1 - supp->v2; -} - -template <typename btConvexTemplate> -static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b, - const btMprCollisionDescription &colDesc, - btMprSimplex_t *portal) -{ - btVector3 dir, va, vb; - float dot; - int cont; - - // vertex 0 is center of portal - btFindOrigin(a, b, colDesc, btMprSimplexPointW(portal, 0)); - - // vertex 0 is center of portal - btMprSimplexSetSize(portal, 1); - - btVector3 zero = btVector3(0, 0, 0); - btVector3 *org = &zero; - - if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)) - { - // Portal's center lies on origin (0,0,0) => we know that objects - // intersect but we would need to know penetration info. - // So move center little bit... - btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); - btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va); - } - - // vertex 1 = support in direction of origin - btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v); - btMprVec3Scale(&dir, -1.f); - btMprVec3Normalize(&dir); - - btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 1)); - - btMprSimplexSetSize(portal, 2); - - // test if origin isn't outside of v1 - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir); - - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - // vertex 2 - btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v, - &btMprSimplexPoint(portal, 1)->v); - if (btMprIsZero(btMprVec3Len2(&dir))) - { - if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)) - { - // origin lies on v1 - return 1; - } - else - { - // origin lies on v0-v1 segment - return 2; - } - } - - btMprVec3Normalize(&dir); - btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 2)); - - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir); - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - btMprSimplexSetSize(portal, 3); - - // vertex 3 direction - btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Cross(&dir, &va, &vb); - btMprVec3Normalize(&dir); - - // it is better to form portal faces to be oriented "outside" origin - dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v); - if (dot > 0.f) - { - btMprSimplexSwap(portal, 1, 2); - btMprVec3Scale(&dir, -1.f); - } - - while (btMprSimplexSize(portal) < 4) - { - btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 3)); - - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir); - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - cont = 0; - - // test if origin is outside (v1, v0, v3) - set v2 as v3 and - // continue - btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 3)->v); - dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); - if (dot < 0.f && !btMprIsZero(dot)) - { - btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3)); - cont = 1; - } - - if (!cont) - { - // test if origin is outside (v3, v0, v2) - set v1 as v3 and - // continue - btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 2)->v); - dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); - if (dot < 0.f && !btMprIsZero(dot)) - { - btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3)); - cont = 1; - } - } - - if (cont) - { - btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Cross(&dir, &va, &vb); - btMprVec3Normalize(&dir); - } - else - { - btMprSimplexSetSize(portal, 4); - } - } - - return 0; -} - -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 through 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/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp deleted file mode 100644 index dca3e09267..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ /dev/null @@ -1,450 +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" -#include "LinearMath/btSerializer.h" - -#ifdef BT_USE_DOUBLE_PRECISION -#define btCollisionObjectData btCollisionObjectDoubleData -#else -#define btCollisionObjectData btCollisionObjectFloatData -#endif - -btScalar gContactBreakingThreshold = btScalar(0.02); -ContactDestroyedCallback gContactDestroyedCallback = 0; -ContactProcessedCallback gContactProcessedCallback = 0; -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_companionIdA(0), - m_companionIdB(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 // -} - -int btPersistentManifold::calculateSerializeBufferSize() const -{ - return sizeof(btPersistentManifoldData); -} - -const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const -{ - btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer; - memset(dataOut, 0, sizeof(btPersistentManifoldData)); - - dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0()); - dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1()); - dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold(); - dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold(); - dataOut->m_numCachedPoints = manifold->getNumContacts(); - dataOut->m_companionIdA = manifold->m_companionIdA; - dataOut->m_companionIdB = manifold->m_companionIdB; - dataOut->m_index1a = manifold->m_index1a; - dataOut->m_objectType = manifold->m_objectType; - - for (int i = 0; i < this->getNumContacts(); i++) - { - const btManifoldPoint& pt = manifold->getContactPoint(i); - dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse; - dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS; - dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1; - dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2; - pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]); - pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]); - pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]); - dataOut->m_pointCacheDistance[i] = pt.m_distance1; - dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1; - dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1; - dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime; - dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM; - dataOut->m_pointCacheContactERP[i] = pt.m_contactERP; - dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM; - dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags; - dataOut->m_pointCacheIndex0[i] = pt.m_index0; - dataOut->m_pointCacheIndex1[i] = pt.m_index1; - dataOut->m_pointCachePartId0[i] = pt.m_partId0; - dataOut->m_pointCachePartId1[i] = pt.m_partId1; - pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]); - pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]); - dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction; - pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]); - pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]); - dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction; - dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction; - dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution; - dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1; - dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2; - } - return btPersistentManifoldDataName; -} - -void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr) -{ - m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; - m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; - m_cachedPoints = manifoldDataPtr->m_numCachedPoints; - m_companionIdA = manifoldDataPtr->m_companionIdA; - m_companionIdB = manifoldDataPtr->m_companionIdB; - //m_index1a = manifoldDataPtr->m_index1a; - m_objectType = manifoldDataPtr->m_objectType; - - for (int i = 0; i < this->getNumContacts(); i++) - { - btManifoldPoint& pt = m_pointCache[i]; - - pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; - pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i]; - pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; - pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; - pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]); - pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]); - pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); - pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; - pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; - pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; - pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; - pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; - pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; - pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; - pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; - pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; - pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; - pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; - pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; - pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); - pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); - pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; - pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); - pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); - pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; - pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; - pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; - pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; - pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; - } -} - -void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr) -{ - m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; - m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; - m_cachedPoints = manifoldDataPtr->m_numCachedPoints; - m_companionIdA = manifoldDataPtr->m_companionIdA; - m_companionIdB = manifoldDataPtr->m_companionIdB; - //m_index1a = manifoldDataPtr->m_index1a; - m_objectType = manifoldDataPtr->m_objectType; - - for (int i = 0; i < this->getNumContacts(); i++) - { - btManifoldPoint& pt = m_pointCache[i]; - - pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; - pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i]; - pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; - pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; - pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]); - pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]); - pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); - pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; - pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; - pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; - pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; - pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; - pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; - pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; - pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; - pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; - pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; - pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; - pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; - pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); - pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); - pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; - pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); - pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); - pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; - pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; - pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; - pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; - pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; - } -} diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h deleted file mode 100644 index 0e26da0ebe..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ /dev/null @@ -1,374 +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; -struct btCollisionObjectDoubleData; -struct btCollisionObjectFloatData; - -///maximum contact breaking and merging threshold -extern btScalar gContactBreakingThreshold; - -#ifndef SWIG -class btPersistentManifold; - -typedef bool (*ContactDestroyedCallback)(void* userPersistentData); -typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp, void* body0, void* body1); -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), - m_companionIdA(0), - m_companionIdB(0), - m_index1a(0) - { - } - - SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0; } - SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1; } - - 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_prevRHS = 0.f; - m_pointCache[lastUsedIndex].m_contactPointFlags = 0; - m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f; - m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f; - m_pointCache[lastUsedIndex].m_lifeTime = 0; - } - - btAssert(m_pointCache[lastUsedIndex].m_userPersistentData == 0); - 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 prevRHS = m_pointCache[insertIndex].m_prevRHS; - btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; - btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; - - bool replacePoint = true; - ///we keep existing contact points for friction anchors - ///if the friction force is within the Coulomb friction cone - if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) - { - // printf("appliedImpulse=%f\n", appliedImpulse); - // printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1); - // printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2); - // printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction); - btScalar mu = m_pointCache[insertIndex].m_combinedFriction; - btScalar eps = 0; //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7 - btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2; - btScalar b = eps + mu * appliedImpulse; - b = b * b; - replacePoint = (a) > (b); - } - - if (replacePoint) - { - btAssert(lifeTime >= 0); - void* cache = m_pointCache[insertIndex].m_userPersistentData; - - m_pointCache[insertIndex] = newPoint; - m_pointCache[insertIndex].m_userPersistentData = cache; - m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; - m_pointCache[insertIndex].m_prevRHS = prevRHS; - m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; - m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; - } - - m_pointCache[insertIndex].m_lifeTime = lifeTime; -#else - clearUserCache(m_pointCache[insertIndex]); - m_pointCache[insertIndex] = newPoint; - -#endif - } - - bool validContactDistance(const btManifoldPoint& pt) const - { - return pt.m_distance1 <= getContactBreakingThreshold(); - } - /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin - void refreshContactPoints(const btTransform& trA, const btTransform& trB); - - 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; - } - - int calculateSerializeBufferSize() const; - const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const; - void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr); - void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr); -}; - -// clang-format off - -struct btPersistentManifoldDoubleData -{ - btVector3DoubleData m_pointCacheLocalPointA[4]; - btVector3DoubleData m_pointCacheLocalPointB[4]; - btVector3DoubleData m_pointCachePositionWorldOnA[4]; - btVector3DoubleData m_pointCachePositionWorldOnB[4]; - btVector3DoubleData m_pointCacheNormalWorldOnB[4]; - btVector3DoubleData m_pointCacheLateralFrictionDir1[4]; - btVector3DoubleData m_pointCacheLateralFrictionDir2[4]; - double m_pointCacheDistance[4]; - double m_pointCacheAppliedImpulse[4]; - double m_pointCachePrevRHS[4]; - double m_pointCacheCombinedFriction[4]; - double m_pointCacheCombinedRollingFriction[4]; - double m_pointCacheCombinedSpinningFriction[4]; - double m_pointCacheCombinedRestitution[4]; - int m_pointCachePartId0[4]; - int m_pointCachePartId1[4]; - int m_pointCacheIndex0[4]; - int m_pointCacheIndex1[4]; - int m_pointCacheContactPointFlags[4]; - double m_pointCacheAppliedImpulseLateral1[4]; - double m_pointCacheAppliedImpulseLateral2[4]; - double m_pointCacheContactMotion1[4]; - double m_pointCacheContactMotion2[4]; - double m_pointCacheContactCFM[4]; - double m_pointCacheCombinedContactStiffness1[4]; - double m_pointCacheContactERP[4]; - double m_pointCacheCombinedContactDamping1[4]; - double m_pointCacheFrictionCFM[4]; - int m_pointCacheLifeTime[4]; - - int m_numCachedPoints; - int m_companionIdA; - int m_companionIdB; - int m_index1a; - - int m_objectType; - double m_contactBreakingThreshold; - double m_contactProcessingThreshold; - int m_padding; - - btCollisionObjectDoubleData *m_body0; - btCollisionObjectDoubleData *m_body1; -}; - - -struct btPersistentManifoldFloatData -{ - btVector3FloatData m_pointCacheLocalPointA[4]; - btVector3FloatData m_pointCacheLocalPointB[4]; - btVector3FloatData m_pointCachePositionWorldOnA[4]; - btVector3FloatData m_pointCachePositionWorldOnB[4]; - btVector3FloatData m_pointCacheNormalWorldOnB[4]; - btVector3FloatData m_pointCacheLateralFrictionDir1[4]; - btVector3FloatData m_pointCacheLateralFrictionDir2[4]; - float m_pointCacheDistance[4]; - float m_pointCacheAppliedImpulse[4]; - float m_pointCachePrevRHS[4]; - float m_pointCacheCombinedFriction[4]; - float m_pointCacheCombinedRollingFriction[4]; - float m_pointCacheCombinedSpinningFriction[4]; - float m_pointCacheCombinedRestitution[4]; - int m_pointCachePartId0[4]; - int m_pointCachePartId1[4]; - int m_pointCacheIndex0[4]; - int m_pointCacheIndex1[4]; - int m_pointCacheContactPointFlags[4]; - float m_pointCacheAppliedImpulseLateral1[4]; - float m_pointCacheAppliedImpulseLateral2[4]; - float m_pointCacheContactMotion1[4]; - float m_pointCacheContactMotion2[4]; - float m_pointCacheContactCFM[4]; - float m_pointCacheCombinedContactStiffness1[4]; - float m_pointCacheContactERP[4]; - float m_pointCacheCombinedContactDamping1[4]; - float m_pointCacheFrictionCFM[4]; - int m_pointCacheLifeTime[4]; - - int m_numCachedPoints; - int m_companionIdA; - int m_companionIdB; - int m_index1a; - - int m_objectType; - float m_contactBreakingThreshold; - float m_contactProcessingThreshold; - int m_padding; - - btCollisionObjectFloatData *m_body0; - btCollisionObjectFloatData *m_body1; -}; - -// clang-format on - -#ifdef BT_USE_DOUBLE_PRECISION -#define btPersistentManifoldData btPersistentManifoldDoubleData -#define btPersistentManifoldDataName "btPersistentManifoldDoubleData" -#else -#define btPersistentManifoldData btPersistentManifoldFloatData -#define btPersistentManifoldDataName "btPersistentManifoldFloatData" -#endif //BT_USE_DOUBLE_PRECISION - -#endif //BT_PERSISTENT_MANIFOLD_H diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPointCollector.h deleted file mode 100644 index 0900eb6e85..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPointCollector.h +++ /dev/null @@ -1,58 +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/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp deleted file mode 100644 index 9d1836037d..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ /dev/null @@ -1,548 +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/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h deleted file mode 100644 index 328f6424bc..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h +++ /dev/null @@ -1,42 +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/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp deleted file mode 100644 index 3d11e5bce5..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ /dev/null @@ -1,175 +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/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h deleted file mode 100644 index 2d0df718a2..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ /dev/null @@ -1,72 +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_DisableHeightfieldAccelerator = 1 << 4, //don't use the heightfield raycast accelerator. See https://github.com/bulletphysics/bullet3/pull/2062 - kF_Terminator = 0xFFFFFFFF - }; - unsigned int m_flags; - - btScalar m_hitFraction; - - 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/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h deleted file mode 100644 index ccd227109d..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ /dev/null @@ -1,58 +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/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp deleted file mode 100644 index 37458339e7..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ /dev/null @@ -1,143 +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) -{ -} - - -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 = result.m_subSimplexCastMaxIterations; - - btVector3 n; - n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); - - btVector3 c; - - btScalar dist2 = v.length2(); - - - - btVector3 w, p; - btScalar VdotR; - - while ((dist2 > result.m_subSimplexCastEpsilon) && maxIter--) - { - supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v * interpolatedTransA.getBasis())); - supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v * interpolatedTransB.getBasis())); - w = supVertexA - supVertexB; - - 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/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h deleted file mode 100644 index 0638a30eb1..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h +++ /dev/null @@ -1,47 +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/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp deleted file mode 100644 index 8fda94d2ad..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ /dev/null @@ -1,577 +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/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h deleted file mode 100644 index 24a0a8f2df..0000000000 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ /dev/null @@ -1,173 +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 |