summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision')
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h369
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp242
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h59
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp20
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h73
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h40
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h90
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h41
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp176
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h50
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp1048
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h75
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h1035
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp66
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h43
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp467
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h103
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h180
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp361
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h40
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h908
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp308
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h268
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h64
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp570
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h49
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp178
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h74
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h63
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp160
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h50
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp612
-rw-r--r--thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h185
33 files changed, 0 insertions, 8067 deletions
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h
deleted file mode 100644
index 9eb880b8df..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h
+++ /dev/null
@@ -1,369 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
-#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
-
-#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision...
-#include "btGjkEpa3.h"
-#include "btGjkCollisionDescription.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-
-
-
-
-
-
-template <typename btConvexTemplate>
-bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
- const btGjkCollisionDescription& colDesc,
- btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
-{
- (void)v;
-
- // const btScalar radialmargin(btScalar(0.));
-
- btVector3 guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input?
-
- btGjkEpaSolver3::sResults results;
-
-
- if(btGjkEpaSolver3_Penetration(a,b,guessVector,results))
-
- {
- // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
- //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
- wWitnessOnA = results.witnesses[0];
- wWitnessOnB = results.witnesses[1];
- v = results.normal;
- return true;
- } else
- {
- if(btGjkEpaSolver3_Distance(a,b,guessVector,results))
- {
- wWitnessOnA = results.witnesses[0];
- wWitnessOnB = results.witnesses[1];
- v = results.normal;
- return false;
- }
- }
- return false;
-}
-
-template <typename btConvexTemplate, typename btGjkDistanceTemplate>
-int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
-{
-
- bool m_catchDegeneracies = true;
- btScalar m_cachedSeparatingDistance = 0.f;
-
- btScalar distance=btScalar(0.);
- btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
-
- btVector3 pointOnA,pointOnB;
- btTransform localTransA = a.getWorldTransform();
- btTransform localTransB = b.getWorldTransform();
-
- btScalar marginA = a.getMargin();
- btScalar marginB = b.getMargin();
-
- int m_curIter = 0;
- int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN?
- btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
-
- bool isValid = false;
- bool checkSimplex = false;
- bool checkPenetration = true;
- int m_degenerateSimplex = 0;
-
- int m_lastUsedMethod = -1;
-
- {
- btScalar squaredDistance = BT_LARGE_FLOAT;
- btScalar delta = btScalar(0.);
-
- btScalar margin = marginA + marginB;
-
-
-
- simplexSolver.reset();
-
- for ( ; ; )
- //while (true)
- {
-
- btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
- btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();
-
- btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
- btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
-
- btVector3 pWorld = localTransA(pInA);
- btVector3 qWorld = localTransB(qInB);
-
-
-
- btVector3 w = pWorld - qWorld;
- delta = m_cachedSeparatingAxis.dot(w);
-
- // potential exit, they don't overlap
- if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
- {
- m_degenerateSimplex = 10;
- checkSimplex=true;
- //checkPenetration = false;
- break;
- }
-
- //exit 0: the new point is already in the simplex, or we didn't come any closer
- if (simplexSolver.inSimplex(w))
- {
- m_degenerateSimplex = 1;
- checkSimplex = true;
- break;
- }
- // are we getting any closer ?
- btScalar f0 = squaredDistance - delta;
- btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
-
- if (f0 <= f1)
- {
- if (f0 <= btScalar(0.))
- {
- m_degenerateSimplex = 2;
- } else
- {
- m_degenerateSimplex = 11;
- }
- checkSimplex = true;
- break;
- }
-
- //add current vertex to simplex
- simplexSolver.addVertex(w, pWorld, qWorld);
- btVector3 newCachedSeparatingAxis;
-
- //calculate the closest point to the origin (update vector v)
- if (!simplexSolver.closest(newCachedSeparatingAxis))
- {
- m_degenerateSimplex = 3;
- checkSimplex = true;
- break;
- }
-
- if(newCachedSeparatingAxis.length2()<colDesc.m_gjkRelError2)
- {
- m_cachedSeparatingAxis = newCachedSeparatingAxis;
- m_degenerateSimplex = 6;
- checkSimplex = true;
- break;
- }
-
- btScalar previousSquaredDistance = squaredDistance;
- squaredDistance = newCachedSeparatingAxis.length2();
-#if 0
- ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
- if (squaredDistance>previousSquaredDistance)
- {
- m_degenerateSimplex = 7;
- squaredDistance = previousSquaredDistance;
- checkSimplex = false;
- break;
- }
-#endif //
-
-
- //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
-
- //are we getting any closer ?
- if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
- {
- // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
- checkSimplex = true;
- m_degenerateSimplex = 12;
-
- break;
- }
-
- m_cachedSeparatingAxis = newCachedSeparatingAxis;
-
- //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
- if (m_curIter++ > gGjkMaxIter)
- {
-#if defined(DEBUG) || defined (_DEBUG)
-
- printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
- printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n",
- m_cachedSeparatingAxis.getX(),
- m_cachedSeparatingAxis.getY(),
- m_cachedSeparatingAxis.getZ(),
- squaredDistance);
-#endif
-
- break;
-
- }
-
-
- bool check = (!simplexSolver.fullSimplex());
- //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
-
- if (!check)
- {
- //do we need this backup_closest here ?
- // m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
- m_degenerateSimplex = 13;
- break;
- }
- }
-
- if (checkSimplex)
- {
- simplexSolver.compute_points(pointOnA, pointOnB);
- normalInB = m_cachedSeparatingAxis;
-
- btScalar lenSqr =m_cachedSeparatingAxis.length2();
-
- //valid normal
- if (lenSqr < 0.0001)
- {
- m_degenerateSimplex = 5;
- }
- if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
- {
- btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
- normalInB *= rlen; //normalize
-
- btScalar s = btSqrt(squaredDistance);
-
- btAssert(s > btScalar(0.0));
- pointOnA -= m_cachedSeparatingAxis * (marginA / s);
- pointOnB += m_cachedSeparatingAxis * (marginB / s);
- distance = ((btScalar(1.)/rlen) - margin);
- isValid = true;
-
- m_lastUsedMethod = 1;
- } else
- {
- m_lastUsedMethod = 2;
- }
- }
-
- bool catchDegeneratePenetrationCase =
- (m_catchDegeneracies && m_degenerateSimplex && ((distance+margin) < 0.01));
-
- //if (checkPenetration && !isValid)
- if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
- {
- //penetration case
-
- //if there is no way to handle penetrations, bail out
-
- // Penetration depth case.
- btVector3 tmpPointOnA,tmpPointOnB;
-
- m_cachedSeparatingAxis.setZero();
-
- bool isValid2 = btGjkEpaCalcPenDepth(a,b,
- colDesc,
- m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
-
- if (isValid2)
- {
- btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
- btScalar lenSqr = tmpNormalInB.length2();
- if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
- {
- tmpNormalInB = m_cachedSeparatingAxis;
- lenSqr = m_cachedSeparatingAxis.length2();
- }
-
- if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
- {
- tmpNormalInB /= btSqrt(lenSqr);
- btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
- //only replace valid penetrations when the result is deeper (check)
- if (!isValid || (distance2 < distance))
- {
- distance = distance2;
- pointOnA = tmpPointOnA;
- pointOnB = tmpPointOnB;
- normalInB = tmpNormalInB;
-
- isValid = true;
- m_lastUsedMethod = 3;
- } else
- {
- m_lastUsedMethod = 8;
- }
- } else
- {
- m_lastUsedMethod = 9;
- }
- } else
-
- {
- ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
- ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
- ///reports a valid positive distance. Use the results of the second GJK instead of failing.
- ///thanks to Jacob.Langford for the reproduction case
- ///http://code.google.com/p/bullet/issues/detail?id=250
-
-
- if (m_cachedSeparatingAxis.length2() > btScalar(0.))
- {
- btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
- //only replace valid distances when the distance is less
- if (!isValid || (distance2 < distance))
- {
- distance = distance2;
- pointOnA = tmpPointOnA;
- pointOnB = tmpPointOnB;
- pointOnA -= m_cachedSeparatingAxis * marginA ;
- pointOnB += m_cachedSeparatingAxis * marginB ;
- normalInB = m_cachedSeparatingAxis;
- normalInB.normalize();
-
- isValid = true;
- m_lastUsedMethod = 6;
- } else
- {
- m_lastUsedMethod = 5;
- }
- }
- }
- }
- }
-
-
-
- if (isValid && ((distance < 0) || (distance*distance < colDesc.m_maximumDistanceSquared)))
- {
-
- m_cachedSeparatingAxis = normalInB;
- m_cachedSeparatingDistance = distance;
- distInfo->m_distance = distance;
- distInfo->m_normalBtoA = normalInB;
- distInfo->m_pointOnB = pointOnB;
- distInfo->m_pointOnA = pointOnB+normalInB*distance;
- return 0;
- }
- return -m_lastUsedMethod;
-}
-
-
-
-
-#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
deleted file mode 100644
index 940282f576..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
+++ /dev/null
@@ -1,242 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#include "btContinuousConvexCollision.h"
-#include "BulletCollision/CollisionShapes/btConvexShape.h"
-#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
-#include "LinearMath/btTransformUtil.h"
-#include "BulletCollision/CollisionShapes/btSphereShape.h"
-
-#include "btGjkPairDetector.h"
-#include "btPointCollector.h"
-#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
-
-
-
-btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
-:m_simplexSolver(simplexSolver),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_convexA(convexA),m_convexB1(convexB),m_planeShape(0)
-{
-}
-
-
-btContinuousConvexCollision::btContinuousConvexCollision( const btConvexShape* convexA,const btStaticPlaneShape* plane)
-:m_simplexSolver(0),
-m_penetrationDepthSolver(0),
-m_convexA(convexA),m_convexB1(0),m_planeShape(plane)
-{
-}
-
-
-/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
-/// You don't want your game ever to lock-up.
-#define MAX_ITERATIONS 64
-
-void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector)
-{
- if (m_convexB1)
- {
- m_simplexSolver->reset();
- btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver);
- btGjkPairDetector::ClosestPointInput input;
- input.m_transformA = transA;
- input.m_transformB = transB;
- gjk.getClosestPoints(input,pointCollector,0);
- } else
- {
- //convex versus plane
- const btConvexShape* convexShape = m_convexA;
- const btStaticPlaneShape* planeShape = m_planeShape;
-
- const btVector3& planeNormal = planeShape->getPlaneNormal();
- const btScalar& planeConstant = planeShape->getPlaneConstant();
-
- btTransform convexWorldTransform = transA;
- btTransform convexInPlaneTrans;
- convexInPlaneTrans= transB.inverse() * convexWorldTransform;
- btTransform planeInConvex;
- planeInConvex= convexWorldTransform.inverse() * transB;
-
- btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
-
- btVector3 vtxInPlane = convexInPlaneTrans(vtx);
- btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
-
- btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
- btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
- btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
-
- pointCollector.addContactPoint(
- normalOnSurfaceB,
- vtxInPlaneWorld,
- distance);
- }
-}
-
-bool btContinuousConvexCollision::calcTimeOfImpact(
- const btTransform& fromA,
- const btTransform& toA,
- const btTransform& fromB,
- const btTransform& toB,
- CastResult& result)
-{
-
-
- /// compute linear and angular velocity for this interval, to interpolate
- btVector3 linVelA,angVelA,linVelB,angVelB;
- btTransformUtil::calculateVelocity(fromA,toA,btScalar(1.),linVelA,angVelA);
- btTransformUtil::calculateVelocity(fromB,toB,btScalar(1.),linVelB,angVelB);
-
-
- btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
- btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f;
-
- btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
- btVector3 relLinVel = (linVelB-linVelA);
-
- btScalar relLinVelocLength = (linVelB-linVelA).length();
-
- if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
- return false;
-
-
-
- btScalar lambda = btScalar(0.);
- btVector3 v(1,0,0);
-
- int maxIter = MAX_ITERATIONS;
-
- btVector3 n;
- n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
- bool hasResult = false;
- btVector3 c;
-
- btScalar lastLambda = lambda;
- //btScalar epsilon = btScalar(0.001);
-
- int numIter = 0;
- //first solution, using GJK
-
-
- btScalar radius = 0.001f;
-// result.drawCoordSystem(sphereTr);
-
- btPointCollector pointCollector1;
-
- {
-
- computeClosestPoints(fromA,fromB,pointCollector1);
-
- hasResult = pointCollector1.m_hasResult;
- c = pointCollector1.m_pointInWorld;
- }
-
- if (hasResult)
- {
- btScalar dist;
- dist = pointCollector1.m_distance + result.m_allowedPenetration;
- n = pointCollector1.m_normalOnBInWorld;
- btScalar projectedLinearVelocity = relLinVel.dot(n);
- if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
- return false;
-
- //not close enough
- while (dist > radius)
- {
- if (result.m_debugDrawer)
- {
- result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
- }
- btScalar dLambda = btScalar(0.);
-
- projectedLinearVelocity = relLinVel.dot(n);
-
-
- //don't report time of impact for motion away from the contact normal (or causes minor penetration)
- if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
- return false;
-
- dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
-
-
-
- lambda = lambda + dLambda;
-
- if (lambda > btScalar(1.))
- return false;
-
- if (lambda < btScalar(0.))
- return false;
-
-
- //todo: next check with relative epsilon
- if (lambda <= lastLambda)
- {
- return false;
- //n.setValue(0,0,0);
- break;
- }
- lastLambda = lambda;
-
-
-
- //interpolate to next lambda
- btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
-
- btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
- btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
- relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
-
- if (result.m_debugDrawer)
- {
- result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0));
- }
-
- result.DebugDraw( lambda );
-
- btPointCollector pointCollector;
- computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector);
-
- if (pointCollector.m_hasResult)
- {
- dist = pointCollector.m_distance+result.m_allowedPenetration;
- c = pointCollector.m_pointInWorld;
- n = pointCollector.m_normalOnBInWorld;
- } else
- {
- result.reportFailure(-1, numIter);
- return false;
- }
-
- numIter++;
- if (numIter > maxIter)
- {
- result.reportFailure(-2, numIter);
- return false;
- }
- }
-
- result.m_fraction = lambda;
- result.m_normal = n;
- result.m_hitPoint = c;
- return true;
- }
-
- return false;
-
-}
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
deleted file mode 100644
index bdc0572f75..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#ifndef BT_CONTINUOUS_COLLISION_CONVEX_CAST_H
-#define BT_CONTINUOUS_COLLISION_CONVEX_CAST_H
-
-#include "btConvexCast.h"
-#include "btSimplexSolverInterface.h"
-class btConvexPenetrationDepthSolver;
-class btConvexShape;
-class btStaticPlaneShape;
-
-/// btContinuousConvexCollision implements angular and linear time of impact for convex objects.
-/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
-/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
-/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
-class btContinuousConvexCollision : public btConvexCast
-{
- btSimplexSolverInterface* m_simplexSolver;
- btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
- const btConvexShape* m_convexA;
- //second object is either a convex or a plane (code sharing)
- const btConvexShape* m_convexB1;
- const btStaticPlaneShape* m_planeShape;
-
- void computeClosestPoints( const btTransform& transA, const btTransform& transB,struct btPointCollector& pointCollector);
-
-public:
-
- btContinuousConvexCollision (const btConvexShape* shapeA,const btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
-
- btContinuousConvexCollision(const btConvexShape* shapeA,const btStaticPlaneShape* plane );
-
- virtual bool calcTimeOfImpact(
- const btTransform& fromA,
- const btTransform& toA,
- const btTransform& fromB,
- const btTransform& toB,
- CastResult& result);
-
-
-};
-
-
-#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp
deleted file mode 100644
index d2a1310b23..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include "btConvexCast.h"
-
-btConvexCast::~btConvexCast()
-{
-}
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
deleted file mode 100644
index bfd79d03be..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#ifndef BT_CONVEX_CAST_H
-#define BT_CONVEX_CAST_H
-
-#include "LinearMath/btTransform.h"
-#include "LinearMath/btVector3.h"
-#include "LinearMath/btScalar.h"
-class btMinkowskiSumShape;
-#include "LinearMath/btIDebugDraw.h"
-
-/// btConvexCast is an interface for Casting
-class btConvexCast
-{
-public:
-
-
- virtual ~btConvexCast();
-
- ///RayResult stores the closest result
- /// alternatively, add a callback method to decide about closest/all results
- struct CastResult
- {
- //virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0;
-
- virtual void DebugDraw(btScalar fraction) {(void)fraction;}
- virtual void drawCoordSystem(const btTransform& trans) {(void)trans;}
- virtual void reportFailure(int errNo, int numIterations) {(void)errNo;(void)numIterations;}
- CastResult()
- :m_fraction(btScalar(BT_LARGE_FLOAT)),
- m_debugDrawer(0),
- m_allowedPenetration(btScalar(0))
- {
- }
-
-
- virtual ~CastResult() {};
-
- btTransform m_hitTransformA;
- btTransform m_hitTransformB;
- btVector3 m_normal;
- btVector3 m_hitPoint;
- btScalar m_fraction; //input and output
- btIDebugDraw* m_debugDrawer;
- btScalar m_allowedPenetration;
-
- };
-
-
- /// cast a convex against another convex object
- virtual bool calcTimeOfImpact(
- const btTransform& fromA,
- const btTransform& toA,
- const btTransform& fromB,
- const btTransform& toB,
- CastResult& result) = 0;
-};
-
-#endif //BT_CONVEX_CAST_H
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
deleted file mode 100644
index 29620abffb..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#ifndef BT_CONVEX_PENETRATION_DEPTH_H
-#define BT_CONVEX_PENETRATION_DEPTH_H
-
-class btVector3;
-#include "btSimplexSolverInterface.h"
-class btConvexShape;
-class btTransform;
-
-///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
-class btConvexPenetrationDepthSolver
-{
-public:
-
- virtual ~btConvexPenetrationDepthSolver() {};
- virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
- const btConvexShape* convexA,const btConvexShape* convexB,
- const btTransform& transA,const btTransform& transB,
- btVector3& v, btVector3& pa, btVector3& pb,
- class btIDebugDraw* debugDraw) = 0;
-
-
-};
-#endif //BT_CONVEX_PENETRATION_DEPTH_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
deleted file mode 100644
index 0ea7b483cf..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#ifndef BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
-#define BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
-
-#include "LinearMath/btTransform.h"
-#include "LinearMath/btVector3.h"
-
-/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
-/// This interface allows to query for closest points and penetration depth between two (convex) objects
-/// the closest point is on the second object (B), and the normal points from the surface on B towards A.
-/// distance is between closest points on B and closest point on A. So you can calculate closest point on A
-/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
-struct btDiscreteCollisionDetectorInterface
-{
-
- struct Result
- {
-
- virtual ~Result(){}
-
- ///setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material combiner
- virtual void setShapeIdentifiersA(int partId0,int index0)=0;
- virtual void setShapeIdentifiersB(int partId1,int index1)=0;
- virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)=0;
- };
-
- struct ClosestPointInput
- {
- ClosestPointInput()
- :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT))
- {
- }
-
- btTransform m_transformA;
- btTransform m_transformB;
- btScalar m_maximumDistanceSquared;
- };
-
- virtual ~btDiscreteCollisionDetectorInterface() {};
-
- //
- // give either closest points (distance > 0) or penetration (distance)
- // the normal always points from B towards A
- //
- virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false) = 0;
-
-};
-
-struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
-{
- btVector3 m_normalOnSurfaceB;
- btVector3 m_closestPointInB;
- btScalar m_distance; //negative means penetration !
-
- protected:
- btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT))
- {
- }
-
- public:
- virtual ~btStorageResult() {};
-
- virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
- {
- if (depth < m_distance)
- {
- m_normalOnSurfaceB = normalOnBInWorld;
- m_closestPointInB = pointInWorld;
- m_distance = depth;
- }
- }
-};
-
-#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h
deleted file mode 100644
index 0b49b0ecc6..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#ifndef GJK_COLLISION_DESCRIPTION_H
-#define GJK_COLLISION_DESCRIPTION_H
-
-#include "LinearMath/btVector3.h"
-
-struct btGjkCollisionDescription
-{
- btVector3 m_firstDir;
- int m_maxGjkIterations;
- btScalar m_maximumDistanceSquared;
- btScalar m_gjkRelError2;
- btGjkCollisionDescription()
- :m_firstDir(0,1,0),
- m_maxGjkIterations(1000),
- m_maximumDistanceSquared(1e30f),
- m_gjkRelError2(1.0e-6)
- {
- }
- virtual ~btGjkCollisionDescription()
- {
- }
-};
-
-#endif //GJK_COLLISION_DESCRIPTION_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
deleted file mode 100644
index bef697a0a1..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
+++ /dev/null
@@ -1,176 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-
-#include "btGjkConvexCast.h"
-#include "BulletCollision/CollisionShapes/btSphereShape.h"
-#include "btGjkPairDetector.h"
-#include "btPointCollector.h"
-#include "LinearMath/btTransformUtil.h"
-
-#ifdef BT_USE_DOUBLE_PRECISION
-#define MAX_ITERATIONS 64
-#else
-#define MAX_ITERATIONS 32
-#endif
-
-btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
-:m_simplexSolver(simplexSolver),
-m_convexA(convexA),
-m_convexB(convexB)
-{
-}
-
-bool btGjkConvexCast::calcTimeOfImpact(
- const btTransform& fromA,
- const btTransform& toA,
- const btTransform& fromB,
- const btTransform& toB,
- CastResult& result)
-{
-
-
- m_simplexSolver->reset();
-
- /// compute linear velocity for this interval, to interpolate
- //assume no rotation/angular velocity, assert here?
- btVector3 linVelA,linVelB;
- linVelA = toA.getOrigin()-fromA.getOrigin();
- linVelB = toB.getOrigin()-fromB.getOrigin();
-
- btScalar radius = btScalar(0.001);
- btScalar lambda = btScalar(0.);
- btVector3 v(1,0,0);
-
- int maxIter = MAX_ITERATIONS;
-
- btVector3 n;
- n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
- bool hasResult = false;
- btVector3 c;
- btVector3 r = (linVelA-linVelB);
-
- btScalar lastLambda = lambda;
- //btScalar epsilon = btScalar(0.001);
-
- int numIter = 0;
- //first solution, using GJK
-
-
- btTransform identityTrans;
- identityTrans.setIdentity();
-
-
-// result.drawCoordSystem(sphereTr);
-
- btPointCollector pointCollector;
-
-
- btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);
- btGjkPairDetector::ClosestPointInput input;
-
- //we don't use margins during CCD
- // gjk.setIgnoreMargin(true);
-
- input.m_transformA = fromA;
- input.m_transformB = fromB;
- gjk.getClosestPoints(input,pointCollector,0);
-
- hasResult = pointCollector.m_hasResult;
- c = pointCollector.m_pointInWorld;
-
- if (hasResult)
- {
- btScalar dist;
- dist = pointCollector.m_distance;
- n = pointCollector.m_normalOnBInWorld;
-
-
-
- //not close enough
- while (dist > radius)
- {
- numIter++;
- if (numIter > maxIter)
- {
- return false; //todo: report a failure
- }
- btScalar dLambda = btScalar(0.);
-
- btScalar projectedLinearVelocity = r.dot(n);
-
- dLambda = dist / (projectedLinearVelocity);
-
- lambda = lambda - dLambda;
-
- if (lambda > btScalar(1.))
- return false;
-
- if (lambda < btScalar(0.))
- return false;
-
- //todo: next check with relative epsilon
- if (lambda <= lastLambda)
- {
- return false;
- //n.setValue(0,0,0);
- break;
- }
- lastLambda = lambda;
-
- //interpolate to next lambda
- result.DebugDraw( lambda );
- input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
- input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
-
- gjk.getClosestPoints(input,pointCollector,0);
- if (pointCollector.m_hasResult)
- {
- if (pointCollector.m_distance < btScalar(0.))
- {
- result.m_fraction = lastLambda;
- n = pointCollector.m_normalOnBInWorld;
- result.m_normal=n;
- result.m_hitPoint = pointCollector.m_pointInWorld;
- return true;
- }
- c = pointCollector.m_pointInWorld;
- n = pointCollector.m_normalOnBInWorld;
- dist = pointCollector.m_distance;
- } else
- {
- //??
- return false;
- }
-
- }
-
- //is n normalized?
- //don't report time of impact for motion away from the contact normal (or causes minor penetration)
- if (n.dot(r)>=-result.m_allowedPenetration)
- return false;
-
- result.m_fraction = lambda;
- result.m_normal = n;
- result.m_hitPoint = c;
- return true;
- }
-
- return false;
-
-
-}
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
deleted file mode 100644
index 6a42ee63b0..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-
-#ifndef BT_GJK_CONVEX_CAST_H
-#define BT_GJK_CONVEX_CAST_H
-
-#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
-
-#include "LinearMath/btVector3.h"
-#include "btConvexCast.h"
-class btConvexShape;
-class btMinkowskiSumShape;
-#include "btSimplexSolverInterface.h"
-
-///GjkConvexCast performs a raycast on a convex object using support mapping.
-class btGjkConvexCast : public btConvexCast
-{
- btSimplexSolverInterface* m_simplexSolver;
- const btConvexShape* m_convexA;
- const btConvexShape* m_convexB;
-
-public:
-
- btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);
-
- /// cast a convex against another convex object
- virtual bool calcTimeOfImpact(
- const btTransform& fromA,
- const btTransform& toA,
- const btTransform& fromB,
- const btTransform& toB,
- CastResult& result);
-
-};
-
-#endif //BT_GJK_CONVEX_CAST_H
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
deleted file mode 100644
index eefb974bbd..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
+++ /dev/null
@@ -1,1048 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the
-use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it
-freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not
-claim that you wrote the original software. If you use this software in a
-product, an acknowledgment in the product documentation would be appreciated
-but is not required.
-2. Altered source versions must be plainly marked as such, and must not be
-misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-/*
-GJK-EPA collision solver by Nathanael Presson, 2008
-*/
-#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
-#include "BulletCollision/CollisionShapes/btSphereShape.h"
-#include "btGjkEpa2.h"
-
-#if defined(DEBUG) || defined (_DEBUG)
-#include <stdio.h> //for debug printf
-#ifdef __SPU__
-#include <spu_printf.h>
-#define printf spu_printf
-#endif //__SPU__
-#endif
-
-namespace gjkepa2_impl
-{
-
- // Config
-
- /* GJK */
-#define GJK_MAX_ITERATIONS 128
-
-#ifdef BT_USE_DOUBLE_PRECISION
- #define GJK_ACCURACY ((btScalar)1e-12)
- #define GJK_MIN_DISTANCE ((btScalar)1e-12)
- #define GJK_DUPLICATED_EPS ((btScalar)1e-12)
-#else
- #define GJK_ACCURACY ((btScalar)0.0001)
- #define GJK_MIN_DISTANCE ((btScalar)0.0001)
- #define GJK_DUPLICATED_EPS ((btScalar)0.0001)
-#endif //BT_USE_DOUBLE_PRECISION
-
-
-#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
-#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
-#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
-
- /* EPA */
-#define EPA_MAX_VERTICES 128
-#define EPA_MAX_ITERATIONS 255
-
-#ifdef BT_USE_DOUBLE_PRECISION
- #define EPA_ACCURACY ((btScalar)1e-12)
- #define EPA_PLANE_EPS ((btScalar)1e-14)
- #define EPA_INSIDE_EPS ((btScalar)1e-9)
-#else
- #define EPA_ACCURACY ((btScalar)0.0001)
- #define EPA_PLANE_EPS ((btScalar)0.00001)
- #define EPA_INSIDE_EPS ((btScalar)0.01)
-#endif
-
-#define EPA_FALLBACK (10*EPA_ACCURACY)
-#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
-
-
- // Shorthands
- typedef unsigned int U;
- typedef unsigned char U1;
-
- // MinkowskiDiff
- struct MinkowskiDiff
- {
- const btConvexShape* m_shapes[2];
- btMatrix3x3 m_toshape1;
- btTransform m_toshape0;
-#ifdef __SPU__
- bool m_enableMargin;
-#else
- btVector3 (btConvexShape::*Ls)(const btVector3&) const;
-#endif//__SPU__
-
-
- MinkowskiDiff()
- {
-
- }
-#ifdef __SPU__
- void EnableMargin(bool enable)
- {
- m_enableMargin = enable;
- }
- inline btVector3 Support0(const btVector3& d) const
- {
- if (m_enableMargin)
- {
- return m_shapes[0]->localGetSupportVertexNonVirtual(d);
- } else
- {
- return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d);
- }
- }
- inline btVector3 Support1(const btVector3& d) const
- {
- if (m_enableMargin)
- {
- return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d));
- } else
- {
- return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d));
- }
- }
-#else
- void EnableMargin(bool enable)
- {
- if(enable)
- Ls=&btConvexShape::localGetSupportVertexNonVirtual;
- else
- Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
- }
- inline btVector3 Support0(const btVector3& d) const
- {
- return(((m_shapes[0])->*(Ls))(d));
- }
- inline btVector3 Support1(const btVector3& d) const
- {
- return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
- }
-#endif //__SPU__
-
- inline btVector3 Support(const btVector3& d) const
- {
- return(Support0(d)-Support1(-d));
- }
- btVector3 Support(const btVector3& d,U index) const
- {
- if(index)
- return(Support1(d));
- else
- return(Support0(d));
- }
- };
-
- typedef MinkowskiDiff tShape;
-
-
- // GJK
- struct GJK
- {
- /* Types */
- struct sSV
- {
- btVector3 d,w;
- };
- struct sSimplex
- {
- sSV* c[4];
- btScalar p[4];
- U rank;
- };
- struct eStatus { enum _ {
- Valid,
- Inside,
- Failed };};
- /* Fields */
- tShape m_shape;
- btVector3 m_ray;
- btScalar m_distance;
- sSimplex m_simplices[2];
- sSV m_store[4];
- sSV* m_free[4];
- U m_nfree;
- U m_current;
- sSimplex* m_simplex;
- eStatus::_ m_status;
- /* Methods */
- GJK()
- {
- Initialize();
- }
- void Initialize()
- {
- m_ray = btVector3(0,0,0);
- m_nfree = 0;
- m_status = eStatus::Failed;
- m_current = 0;
- m_distance = 0;
- }
- eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess)
- {
- U iterations=0;
- btScalar sqdist=0;
- btScalar alpha=0;
- btVector3 lastw[4];
- U clastw=0;
- /* Initialize solver */
- m_free[0] = &m_store[0];
- m_free[1] = &m_store[1];
- m_free[2] = &m_store[2];
- m_free[3] = &m_store[3];
- m_nfree = 4;
- m_current = 0;
- m_status = eStatus::Valid;
- m_shape = shapearg;
- m_distance = 0;
- /* Initialize simplex */
- m_simplices[0].rank = 0;
- m_ray = guess;
- const btScalar sqrl= m_ray.length2();
- appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
- m_simplices[0].p[0] = 1;
- m_ray = m_simplices[0].c[0]->w;
- sqdist = sqrl;
- lastw[0] =
- lastw[1] =
- lastw[2] =
- lastw[3] = m_ray;
- /* Loop */
- do {
- const U next=1-m_current;
- sSimplex& cs=m_simplices[m_current];
- sSimplex& ns=m_simplices[next];
- /* Check zero */
- const btScalar rl=m_ray.length();
- if(rl<GJK_MIN_DISTANCE)
- {/* Touching or inside */
- m_status=eStatus::Inside;
- break;
- }
- /* Append new vertice in -'v' direction */
- appendvertice(cs,-m_ray);
- const btVector3& w=cs.c[cs.rank-1]->w;
- bool found=false;
- for(U i=0;i<4;++i)
- {
- if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
- { found=true;break; }
- }
- if(found)
- {/* Return old simplex */
- removevertice(m_simplices[m_current]);
- break;
- }
- else
- {/* Update lastw */
- lastw[clastw=(clastw+1)&3]=w;
- }
- /* Check for termination */
- const btScalar omega=btDot(m_ray,w)/rl;
- alpha=btMax(omega,alpha);
- if(((rl-alpha)-(GJK_ACCURACY*rl))<=0)
- {/* Return old simplex */
- removevertice(m_simplices[m_current]);
- break;
- }
- /* Reduce simplex */
- btScalar weights[4];
- U mask=0;
- switch(cs.rank)
- {
- case 2: sqdist=projectorigin( cs.c[0]->w,
- cs.c[1]->w,
- weights,mask);break;
- case 3: sqdist=projectorigin( cs.c[0]->w,
- cs.c[1]->w,
- cs.c[2]->w,
- weights,mask);break;
- case 4: sqdist=projectorigin( cs.c[0]->w,
- cs.c[1]->w,
- cs.c[2]->w,
- cs.c[3]->w,
- weights,mask);break;
- }
- if(sqdist>=0)
- {/* Valid */
- ns.rank = 0;
- m_ray = btVector3(0,0,0);
- m_current = next;
- for(U i=0,ni=cs.rank;i<ni;++i)
- {
- if(mask&(1<<i))
- {
- ns.c[ns.rank] = cs.c[i];
- ns.p[ns.rank++] = weights[i];
- m_ray += cs.c[i]->w*weights[i];
- }
- else
- {
- m_free[m_nfree++] = cs.c[i];
- }
- }
- if(mask==15) m_status=eStatus::Inside;
- }
- else
- {/* Return old simplex */
- removevertice(m_simplices[m_current]);
- break;
- }
- m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
- } while(m_status==eStatus::Valid);
- m_simplex=&m_simplices[m_current];
- switch(m_status)
- {
- case eStatus::Valid: m_distance=m_ray.length();break;
- case eStatus::Inside: m_distance=0;break;
- default:
- {
- }
- }
- return(m_status);
- }
- bool EncloseOrigin()
- {
- switch(m_simplex->rank)
- {
- case 1:
- {
- for(U i=0;i<3;++i)
- {
- btVector3 axis=btVector3(0,0,0);
- axis[i]=1;
- appendvertice(*m_simplex, axis);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- appendvertice(*m_simplex,-axis);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- }
- }
- break;
- case 2:
- {
- const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
- for(U i=0;i<3;++i)
- {
- btVector3 axis=btVector3(0,0,0);
- axis[i]=1;
- const btVector3 p=btCross(d,axis);
- if(p.length2()>0)
- {
- appendvertice(*m_simplex, p);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- appendvertice(*m_simplex,-p);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- }
- }
- }
- break;
- case 3:
- {
- const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
- m_simplex->c[2]->w-m_simplex->c[0]->w);
- if(n.length2()>0)
- {
- appendvertice(*m_simplex,n);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- appendvertice(*m_simplex,-n);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- }
- }
- break;
- case 4:
- {
- if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
- m_simplex->c[1]->w-m_simplex->c[3]->w,
- m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
- return(true);
- }
- break;
- }
- return(false);
- }
- /* Internals */
- void getsupport(const btVector3& d,sSV& sv) const
- {
- sv.d = d/d.length();
- sv.w = m_shape.Support(sv.d);
- }
- void removevertice(sSimplex& simplex)
- {
- m_free[m_nfree++]=simplex.c[--simplex.rank];
- }
- void appendvertice(sSimplex& simplex,const btVector3& v)
- {
- simplex.p[simplex.rank]=0;
- simplex.c[simplex.rank]=m_free[--m_nfree];
- getsupport(v,*simplex.c[simplex.rank++]);
- }
- static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c)
- {
- return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
- a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
- a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
- }
- static btScalar projectorigin( const btVector3& a,
- const btVector3& b,
- btScalar* w,U& m)
- {
- const btVector3 d=b-a;
- const btScalar l=d.length2();
- if(l>GJK_SIMPLEX2_EPS)
- {
- const btScalar t(l>0?-btDot(a,d)/l:0);
- if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
- else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
- else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
- }
- return(-1);
- }
- static btScalar projectorigin( const btVector3& a,
- const btVector3& b,
- const btVector3& c,
- btScalar* w,U& m)
- {
- static const U imd3[]={1,2,0};
- const btVector3* vt[]={&a,&b,&c};
- const btVector3 dl[]={a-b,b-c,c-a};
- const btVector3 n=btCross(dl[0],dl[1]);
- const btScalar l=n.length2();
- if(l>GJK_SIMPLEX3_EPS)
- {
- btScalar mindist=-1;
- btScalar subw[2]={0.f,0.f};
- U subm(0);
- for(U i=0;i<3;++i)
- {
- if(btDot(*vt[i],btCross(dl[i],n))>0)
- {
- const U j=imd3[i];
- const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
- if((mindist<0)||(subd<mindist))
- {
- mindist = subd;
- m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
- w[i] = subw[0];
- w[j] = subw[1];
- w[imd3[j]] = 0;
- }
- }
- }
- if(mindist<0)
- {
- const btScalar d=btDot(a,n);
- const btScalar s=btSqrt(l);
- const btVector3 p=n*(d/l);
- mindist = p.length2();
- m = 7;
- w[0] = (btCross(dl[1],b-p)).length()/s;
- w[1] = (btCross(dl[2],c-p)).length()/s;
- w[2] = 1-(w[0]+w[1]);
- }
- return(mindist);
- }
- return(-1);
- }
- static btScalar projectorigin( const btVector3& a,
- const btVector3& b,
- const btVector3& c,
- const btVector3& d,
- btScalar* w,U& m)
- {
- static const U imd3[]={1,2,0};
- const btVector3* vt[]={&a,&b,&c,&d};
- const btVector3 dl[]={a-d,b-d,c-d};
- const btScalar vl=det(dl[0],dl[1],dl[2]);
- const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
- if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
- {
- btScalar mindist=-1;
- btScalar subw[3]={0.f,0.f,0.f};
- U subm(0);
- for(U i=0;i<3;++i)
- {
- const U j=imd3[i];
- const btScalar s=vl*btDot(d,btCross(dl[i],dl[j]));
- if(s>0)
- {
- const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
- if((mindist<0)||(subd<mindist))
- {
- mindist = subd;
- m = static_cast<U>((subm&1?1<<i:0)+
- (subm&2?1<<j:0)+
- (subm&4?8:0));
- w[i] = subw[0];
- w[j] = subw[1];
- w[imd3[j]] = 0;
- w[3] = subw[2];
- }
- }
- }
- if(mindist<0)
- {
- mindist = 0;
- m = 15;
- w[0] = det(c,b,d)/vl;
- w[1] = det(a,c,d)/vl;
- w[2] = det(b,a,d)/vl;
- w[3] = 1-(w[0]+w[1]+w[2]);
- }
- return(mindist);
- }
- return(-1);
- }
- };
-
- // EPA
- struct EPA
- {
- /* Types */
- typedef GJK::sSV sSV;
- struct sFace
- {
- btVector3 n;
- btScalar d;
- sSV* c[3];
- sFace* f[3];
- sFace* l[2];
- U1 e[3];
- U1 pass;
- };
- struct sList
- {
- sFace* root;
- U count;
- sList() : root(0),count(0) {}
- };
- struct sHorizon
- {
- sFace* cf;
- sFace* ff;
- U nf;
- sHorizon() : cf(0),ff(0),nf(0) {}
- };
- struct eStatus { enum _ {
- Valid,
- Touching,
- Degenerated,
- NonConvex,
- InvalidHull,
- OutOfFaces,
- OutOfVertices,
- AccuraryReached,
- FallBack,
- Failed };};
- /* Fields */
- eStatus::_ m_status;
- GJK::sSimplex m_result;
- btVector3 m_normal;
- btScalar m_depth;
- sSV m_sv_store[EPA_MAX_VERTICES];
- sFace m_fc_store[EPA_MAX_FACES];
- U m_nextsv;
- sList m_hull;
- sList m_stock;
- /* Methods */
- EPA()
- {
- Initialize();
- }
-
-
- static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
- {
- fa->e[ea]=(U1)eb;fa->f[ea]=fb;
- fb->e[eb]=(U1)ea;fb->f[eb]=fa;
- }
- static inline void append(sList& list,sFace* face)
- {
- face->l[0] = 0;
- face->l[1] = list.root;
- if(list.root) list.root->l[0]=face;
- list.root = face;
- ++list.count;
- }
- static inline void remove(sList& list,sFace* face)
- {
- if(face->l[1]) face->l[1]->l[0]=face->l[0];
- if(face->l[0]) face->l[0]->l[1]=face->l[1];
- if(face==list.root) list.root=face->l[1];
- --list.count;
- }
-
-
- void Initialize()
- {
- m_status = eStatus::Failed;
- m_normal = btVector3(0,0,0);
- m_depth = 0;
- m_nextsv = 0;
- for(U i=0;i<EPA_MAX_FACES;++i)
- {
- append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
- }
- }
- eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
- {
- GJK::sSimplex& simplex=*gjk.m_simplex;
- if((simplex.rank>1)&&gjk.EncloseOrigin())
- {
-
- /* Clean up */
- while(m_hull.root)
- {
- sFace* f = m_hull.root;
- remove(m_hull,f);
- append(m_stock,f);
- }
- m_status = eStatus::Valid;
- m_nextsv = 0;
- /* Orient simplex */
- if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
- simplex.c[1]->w-simplex.c[3]->w,
- simplex.c[2]->w-simplex.c[3]->w)<0)
- {
- btSwap(simplex.c[0],simplex.c[1]);
- btSwap(simplex.p[0],simplex.p[1]);
- }
- /* Build initial hull */
- sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
- newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
- newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
- newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
- if(m_hull.count==4)
- {
- sFace* best=findbest();
- sFace outer=*best;
- U pass=0;
- U iterations=0;
- bind(tetra[0],0,tetra[1],0);
- bind(tetra[0],1,tetra[2],0);
- bind(tetra[0],2,tetra[3],0);
- bind(tetra[1],1,tetra[3],2);
- bind(tetra[1],2,tetra[2],1);
- bind(tetra[2],2,tetra[3],1);
- m_status=eStatus::Valid;
- for(;iterations<EPA_MAX_ITERATIONS;++iterations)
- {
- if(m_nextsv<EPA_MAX_VERTICES)
- {
- sHorizon horizon;
- sSV* w=&m_sv_store[m_nextsv++];
- bool valid=true;
- best->pass = (U1)(++pass);
- gjk.getsupport(best->n,*w);
- const btScalar wdist=btDot(best->n,w->w)-best->d;
- if(wdist>EPA_ACCURACY)
- {
- for(U j=0;(j<3)&&valid;++j)
- {
- valid&=expand( pass,w,
- best->f[j],best->e[j],
- horizon);
- }
- if(valid&&(horizon.nf>=3))
- {
- bind(horizon.cf,1,horizon.ff,2);
- remove(m_hull,best);
- append(m_stock,best);
- best=findbest();
- outer=*best;
- } else { m_status=eStatus::InvalidHull;break; }
- } else { m_status=eStatus::AccuraryReached;break; }
- } else { m_status=eStatus::OutOfVertices;break; }
- }
- const btVector3 projection=outer.n*outer.d;
- m_normal = outer.n;
- m_depth = outer.d;
- m_result.rank = 3;
- m_result.c[0] = outer.c[0];
- m_result.c[1] = outer.c[1];
- m_result.c[2] = outer.c[2];
- m_result.p[0] = btCross( outer.c[1]->w-projection,
- outer.c[2]->w-projection).length();
- m_result.p[1] = btCross( outer.c[2]->w-projection,
- outer.c[0]->w-projection).length();
- m_result.p[2] = btCross( outer.c[0]->w-projection,
- outer.c[1]->w-projection).length();
- const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
- m_result.p[0] /= sum;
- m_result.p[1] /= sum;
- m_result.p[2] /= sum;
- return(m_status);
- }
- }
- /* Fallback */
- m_status = eStatus::FallBack;
- m_normal = -guess;
- const btScalar nl=m_normal.length();
- if(nl>0)
- m_normal = m_normal/nl;
- else
- m_normal = btVector3(1,0,0);
- m_depth = 0;
- m_result.rank=1;
- m_result.c[0]=simplex.c[0];
- m_result.p[0]=1;
- return(m_status);
- }
- bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist)
- {
- const btVector3 ba = b->w - a->w;
- const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
- const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
-
- if(a_dot_nab < 0)
- {
- // Outside of edge a->b
-
- const btScalar ba_l2 = ba.length2();
- const btScalar a_dot_ba = btDot(a->w, ba);
- const btScalar b_dot_ba = btDot(b->w, ba);
-
- if(a_dot_ba > 0)
- {
- // Pick distance vertex a
- dist = a->w.length();
- }
- else if(b_dot_ba < 0)
- {
- // Pick distance vertex b
- dist = b->w.length();
- }
- else
- {
- // Pick distance to edge a->b
- const btScalar a_dot_b = btDot(a->w, b->w);
- dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
- }
-
- return true;
- }
-
- return false;
- }
- sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
- {
- if(m_stock.root)
- {
- sFace* face=m_stock.root;
- remove(m_stock,face);
- append(m_hull,face);
- face->pass = 0;
- face->c[0] = a;
- face->c[1] = b;
- face->c[2] = c;
- face->n = btCross(b->w-a->w,c->w-a->w);
- const btScalar l=face->n.length();
- const bool v=l>EPA_ACCURACY;
-
- if(v)
- {
- if(!(getedgedist(face, a, b, face->d) ||
- getedgedist(face, b, c, face->d) ||
- getedgedist(face, c, a, face->d)))
- {
- // Origin projects to the interior of the triangle
- // Use distance to triangle plane
- face->d = btDot(a->w, face->n) / l;
- }
-
- face->n /= l;
- if(forced || (face->d >= -EPA_PLANE_EPS))
- {
- return face;
- }
- else
- m_status=eStatus::NonConvex;
- }
- else
- m_status=eStatus::Degenerated;
-
- remove(m_hull, face);
- append(m_stock, face);
- return 0;
-
- }
- m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces;
- return 0;
- }
- sFace* findbest()
- {
- sFace* minf=m_hull.root;
- btScalar mind=minf->d*minf->d;
- for(sFace* f=minf->l[1];f;f=f->l[1])
- {
- const btScalar sqd=f->d*f->d;
- if(sqd<mind)
- {
- minf=f;
- mind=sqd;
- }
- }
- return(minf);
- }
- bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
- {
- static const U i1m3[]={1,2,0};
- static const U i2m3[]={2,0,1};
- if(f->pass!=pass)
- {
- const U e1=i1m3[e];
- if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
- {
- sFace* nf=newface(f->c[e1],f->c[e],w,false);
- if(nf)
- {
- bind(nf,0,f,e);
- if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
- horizon.cf=nf;
- ++horizon.nf;
- return(true);
- }
- }
- else
- {
- const U e2=i2m3[e];
- f->pass = (U1)pass;
- if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
- expand(pass,w,f->f[e2],f->e[e2],horizon))
- {
- remove(m_hull,f);
- append(m_stock,f);
- return(true);
- }
- }
- }
- return(false);
- }
-
- };
-
- //
- static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,
- const btConvexShape* shape1,const btTransform& wtrs1,
- btGjkEpaSolver2::sResults& results,
- tShape& shape,
- bool withmargins)
- {
- /* Results */
- results.witnesses[0] =
- results.witnesses[1] = btVector3(0,0,0);
- results.status = btGjkEpaSolver2::sResults::Separated;
- /* Shape */
- shape.m_shapes[0] = shape0;
- shape.m_shapes[1] = shape1;
- shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
- shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
- shape.EnableMargin(withmargins);
- }
-
-}
-
-//
-// Api
-//
-
-using namespace gjkepa2_impl;
-
-//
-int btGjkEpaSolver2::StackSizeRequirement()
-{
- return(sizeof(GJK)+sizeof(EPA));
-}
-
-//
-bool btGjkEpaSolver2::Distance( const btConvexShape* shape0,
- const btTransform& wtrs0,
- const btConvexShape* shape1,
- const btTransform& wtrs1,
- const btVector3& guess,
- sResults& results)
-{
- tShape shape;
- Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
- GJK gjk;
- GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
- if(gjk_status==GJK::eStatus::Valid)
- {
- btVector3 w0=btVector3(0,0,0);
- btVector3 w1=btVector3(0,0,0);
- for(U i=0;i<gjk.m_simplex->rank;++i)
- {
- const btScalar p=gjk.m_simplex->p[i];
- w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
- w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
- }
- results.witnesses[0] = wtrs0*w0;
- results.witnesses[1] = wtrs0*w1;
- results.normal = w0-w1;
- results.distance = results.normal.length();
- results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
- return(true);
- }
- else
- {
- results.status = gjk_status==GJK::eStatus::Inside?
- sResults::Penetrating :
- sResults::GJK_Failed ;
- return(false);
- }
-}
-
-//
-bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0,
- const btTransform& wtrs0,
- const btConvexShape* shape1,
- const btTransform& wtrs1,
- const btVector3& guess,
- sResults& results,
- bool usemargins)
-{
- tShape shape;
- Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
- GJK gjk;
- GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
- switch(gjk_status)
- {
- case GJK::eStatus::Inside:
- {
- EPA epa;
- EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
- if(epa_status!=EPA::eStatus::Failed)
- {
- btVector3 w0=btVector3(0,0,0);
- for(U i=0;i<epa.m_result.rank;++i)
- {
- w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
- }
- results.status = sResults::Penetrating;
- results.witnesses[0] = wtrs0*w0;
- results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth);
- results.normal = -epa.m_normal;
- results.distance = -epa.m_depth;
- return(true);
- } else results.status=sResults::EPA_Failed;
- }
- break;
- case GJK::eStatus::Failed:
- results.status=sResults::GJK_Failed;
- break;
- default:
- {
- }
- }
- return(false);
-}
-
-#ifndef __SPU__
-//
-btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
- btScalar margin,
- const btConvexShape* shape0,
- const btTransform& wtrs0,
- sResults& results)
-{
- tShape shape;
- btSphereShape shape1(margin);
- btTransform wtrs1(btQuaternion(0,0,0,1),position);
- Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
- GJK gjk;
- GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
- if(gjk_status==GJK::eStatus::Valid)
- {
- btVector3 w0=btVector3(0,0,0);
- btVector3 w1=btVector3(0,0,0);
- for(U i=0;i<gjk.m_simplex->rank;++i)
- {
- const btScalar p=gjk.m_simplex->p[i];
- w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
- w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
- }
- results.witnesses[0] = wtrs0*w0;
- results.witnesses[1] = wtrs0*w1;
- const btVector3 delta= results.witnesses[1]-
- results.witnesses[0];
- const btScalar margin= shape0->getMarginNonVirtual()+
- shape1.getMarginNonVirtual();
- const btScalar length= delta.length();
- results.normal = delta/length;
- results.witnesses[0] += results.normal*margin;
- return(length-margin);
- }
- else
- {
- if(gjk_status==GJK::eStatus::Inside)
- {
- if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
- {
- const btVector3 delta= results.witnesses[0]-
- results.witnesses[1];
- const btScalar length= delta.length();
- if (length >= SIMD_EPSILON)
- results.normal = delta/length;
- return(-length);
- }
- }
- }
- return(SIMD_INFINITY);
-}
-
-//
-bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
- const btTransform& wtrs0,
- const btConvexShape* shape1,
- const btTransform& wtrs1,
- const btVector3& guess,
- sResults& results)
-{
- if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
- return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
- else
- return(true);
-}
-#endif //__SPU__
-
-/* Symbols cleanup */
-
-#undef GJK_MAX_ITERATIONS
-#undef GJK_ACCURACY
-#undef GJK_MIN_DISTANCE
-#undef GJK_DUPLICATED_EPS
-#undef GJK_SIMPLEX2_EPS
-#undef GJK_SIMPLEX3_EPS
-#undef GJK_SIMPLEX4_EPS
-
-#undef EPA_MAX_VERTICES
-#undef EPA_MAX_FACES
-#undef EPA_MAX_ITERATIONS
-#undef EPA_ACCURACY
-#undef EPA_FALLBACK
-#undef EPA_PLANE_EPS
-#undef EPA_INSIDE_EPS
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
deleted file mode 100644
index ac501d5ecf..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
+++ /dev/null
@@ -1,75 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the
-use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it
-freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not
-claim that you wrote the original software. If you use this software in a
-product, an acknowledgment in the product documentation would be appreciated
-but is not required.
-2. Altered source versions must be plainly marked as such, and must not be
-misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-/*
-GJK-EPA collision solver by Nathanael Presson, 2008
-*/
-#ifndef BT_GJK_EPA2_H
-#define BT_GJK_EPA2_H
-
-#include "BulletCollision/CollisionShapes/btConvexShape.h"
-
-///btGjkEpaSolver contributed under zlib by Nathanael Presson
-struct btGjkEpaSolver2
-{
-struct sResults
- {
- enum eStatus
- {
- Separated, /* Shapes doesnt penetrate */
- Penetrating, /* Shapes are penetrating */
- GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
- EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
- } status;
- btVector3 witnesses[2];
- btVector3 normal;
- btScalar distance;
- };
-
-static int StackSizeRequirement();
-
-static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0,
- const btConvexShape* shape1,const btTransform& wtrs1,
- const btVector3& guess,
- sResults& results);
-
-static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0,
- const btConvexShape* shape1,const btTransform& wtrs1,
- const btVector3& guess,
- sResults& results,
- bool usemargins=true);
-#ifndef __SPU__
-static btScalar SignedDistance( const btVector3& position,
- btScalar margin,
- const btConvexShape* shape,
- const btTransform& wtrs,
- sResults& results);
-
-static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0,
- const btConvexShape* shape1,const btTransform& wtrs1,
- const btVector3& guess,
- sResults& results);
-#endif //__SPU__
-
-};
-
-#endif //BT_GJK_EPA2_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h
deleted file mode 100644
index ce1f24bc50..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h
+++ /dev/null
@@ -1,1035 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2014 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the
-use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it
-freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not
-claim that you wrote the original software. If you use this software in a
-product, an acknowledgment in the product documentation would be appreciated
-but is not required.
-2. Altered source versions must be plainly marked as such, and must not be
-misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-/*
-Initial GJK-EPA collision solver by Nathanael Presson, 2008
-Improvements and refactoring by Erwin Coumans, 2008-2014
-*/
-#ifndef BT_GJK_EPA3_H
-#define BT_GJK_EPA3_H
-
-#include "LinearMath/btTransform.h"
-#include "btGjkCollisionDescription.h"
-
-
-
-struct btGjkEpaSolver3
-{
-struct sResults
- {
- enum eStatus
- {
- Separated, /* Shapes doesnt penetrate */
- Penetrating, /* Shapes are penetrating */
- GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
- EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
- } status;
- btVector3 witnesses[2];
- btVector3 normal;
- btScalar distance;
- };
-
-
-};
-
-
-
-#if defined(DEBUG) || defined (_DEBUG)
-#include <stdio.h> //for debug printf
-#ifdef __SPU__
-#include <spu_printf.h>
-#define printf spu_printf
-#endif //__SPU__
-#endif
-
-
-
- // Config
-
- /* GJK */
-#define GJK_MAX_ITERATIONS 128
-#define GJK_ACCURARY ((btScalar)0.0001)
-#define GJK_MIN_DISTANCE ((btScalar)0.0001)
-#define GJK_DUPLICATED_EPS ((btScalar)0.0001)
-#define GJK_SIMPLEX2_EPS ((btScalar)0.0)
-#define GJK_SIMPLEX3_EPS ((btScalar)0.0)
-#define GJK_SIMPLEX4_EPS ((btScalar)0.0)
-
- /* EPA */
-#define EPA_MAX_VERTICES 64
-#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
-#define EPA_MAX_ITERATIONS 255
-#define EPA_ACCURACY ((btScalar)0.0001)
-#define EPA_FALLBACK (10*EPA_ACCURACY)
-#define EPA_PLANE_EPS ((btScalar)0.00001)
-#define EPA_INSIDE_EPS ((btScalar)0.01)
-
-
- // Shorthands
- typedef unsigned int U;
- typedef unsigned char U1;
-
- // MinkowskiDiff
- template <typename btConvexTemplate>
- struct MinkowskiDiff
- {
- const btConvexTemplate* m_convexAPtr;
- const btConvexTemplate* m_convexBPtr;
-
- btMatrix3x3 m_toshape1;
- btTransform m_toshape0;
-
- bool m_enableMargin;
-
-
- MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b)
- :m_convexAPtr(&a),
- m_convexBPtr(&b)
- {
- }
-
- void EnableMargin(bool enable)
- {
- m_enableMargin = enable;
- }
- inline btVector3 Support0(const btVector3& d) const
- {
- return m_convexAPtr->getLocalSupportWithMargin(d);
- }
- inline btVector3 Support1(const btVector3& d) const
- {
- return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d);
- }
-
-
- inline btVector3 Support(const btVector3& d) const
- {
- return(Support0(d)-Support1(-d));
- }
- btVector3 Support(const btVector3& d,U index) const
- {
- if(index)
- return(Support1(d));
- else
- return(Support0(d));
- }
- };
-
-enum eGjkStatus
-{
- eGjkValid,
- eGjkInside,
- eGjkFailed
-};
-
- // GJK
- template <typename btConvexTemplate>
- struct GJK
- {
- /* Types */
- struct sSV
- {
- btVector3 d,w;
- };
- struct sSimplex
- {
- sSV* c[4];
- btScalar p[4];
- U rank;
- };
-
- /* Fields */
-
- MinkowskiDiff<btConvexTemplate> m_shape;
- btVector3 m_ray;
- btScalar m_distance;
- sSimplex m_simplices[2];
- sSV m_store[4];
- sSV* m_free[4];
- U m_nfree;
- U m_current;
- sSimplex* m_simplex;
- eGjkStatus m_status;
- /* Methods */
-
- GJK(const btConvexTemplate& a, const btConvexTemplate& b)
- :m_shape(a,b)
- {
- Initialize();
- }
- void Initialize()
- {
- m_ray = btVector3(0,0,0);
- m_nfree = 0;
- m_status = eGjkFailed;
- m_current = 0;
- m_distance = 0;
- }
- eGjkStatus Evaluate(const MinkowskiDiff<btConvexTemplate>& shapearg,const btVector3& guess)
- {
- U iterations=0;
- btScalar sqdist=0;
- btScalar alpha=0;
- btVector3 lastw[4];
- U clastw=0;
- /* Initialize solver */
- m_free[0] = &m_store[0];
- m_free[1] = &m_store[1];
- m_free[2] = &m_store[2];
- m_free[3] = &m_store[3];
- m_nfree = 4;
- m_current = 0;
- m_status = eGjkValid;
- m_shape = shapearg;
- m_distance = 0;
- /* Initialize simplex */
- m_simplices[0].rank = 0;
- m_ray = guess;
- const btScalar sqrl= m_ray.length2();
- appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
- m_simplices[0].p[0] = 1;
- m_ray = m_simplices[0].c[0]->w;
- sqdist = sqrl;
- lastw[0] =
- lastw[1] =
- lastw[2] =
- lastw[3] = m_ray;
- /* Loop */
- do {
- const U next=1-m_current;
- sSimplex& cs=m_simplices[m_current];
- sSimplex& ns=m_simplices[next];
- /* Check zero */
- const btScalar rl=m_ray.length();
- if(rl<GJK_MIN_DISTANCE)
- {/* Touching or inside */
- m_status=eGjkInside;
- break;
- }
- /* Append new vertice in -'v' direction */
- appendvertice(cs,-m_ray);
- const btVector3& w=cs.c[cs.rank-1]->w;
- bool found=false;
- for(U i=0;i<4;++i)
- {
- if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
- { found=true;break; }
- }
- if(found)
- {/* Return old simplex */
- removevertice(m_simplices[m_current]);
- break;
- }
- else
- {/* Update lastw */
- lastw[clastw=(clastw+1)&3]=w;
- }
- /* Check for termination */
- const btScalar omega=btDot(m_ray,w)/rl;
- alpha=btMax(omega,alpha);
- if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
- {/* Return old simplex */
- removevertice(m_simplices[m_current]);
- break;
- }
- /* Reduce simplex */
- btScalar weights[4];
- U mask=0;
- switch(cs.rank)
- {
- case 2: sqdist=projectorigin( cs.c[0]->w,
- cs.c[1]->w,
- weights,mask);break;
- case 3: sqdist=projectorigin( cs.c[0]->w,
- cs.c[1]->w,
- cs.c[2]->w,
- weights,mask);break;
- case 4: sqdist=projectorigin( cs.c[0]->w,
- cs.c[1]->w,
- cs.c[2]->w,
- cs.c[3]->w,
- weights,mask);break;
- }
- if(sqdist>=0)
- {/* Valid */
- ns.rank = 0;
- m_ray = btVector3(0,0,0);
- m_current = next;
- for(U i=0,ni=cs.rank;i<ni;++i)
- {
- if(mask&(1<<i))
- {
- ns.c[ns.rank] = cs.c[i];
- ns.p[ns.rank++] = weights[i];
- m_ray += cs.c[i]->w*weights[i];
- }
- else
- {
- m_free[m_nfree++] = cs.c[i];
- }
- }
- if(mask==15) m_status=eGjkInside;
- }
- else
- {/* Return old simplex */
- removevertice(m_simplices[m_current]);
- break;
- }
- m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eGjkFailed;
- } while(m_status==eGjkValid);
- m_simplex=&m_simplices[m_current];
- switch(m_status)
- {
- case eGjkValid: m_distance=m_ray.length();break;
- case eGjkInside: m_distance=0;break;
- default:
- {
- }
- }
- return(m_status);
- }
- bool EncloseOrigin()
- {
- switch(m_simplex->rank)
- {
- case 1:
- {
- for(U i=0;i<3;++i)
- {
- btVector3 axis=btVector3(0,0,0);
- axis[i]=1;
- appendvertice(*m_simplex, axis);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- appendvertice(*m_simplex,-axis);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- }
- }
- break;
- case 2:
- {
- const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
- for(U i=0;i<3;++i)
- {
- btVector3 axis=btVector3(0,0,0);
- axis[i]=1;
- const btVector3 p=btCross(d,axis);
- if(p.length2()>0)
- {
- appendvertice(*m_simplex, p);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- appendvertice(*m_simplex,-p);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- }
- }
- }
- break;
- case 3:
- {
- const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
- m_simplex->c[2]->w-m_simplex->c[0]->w);
- if(n.length2()>0)
- {
- appendvertice(*m_simplex,n);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- appendvertice(*m_simplex,-n);
- if(EncloseOrigin()) return(true);
- removevertice(*m_simplex);
- }
- }
- break;
- case 4:
- {
- if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
- m_simplex->c[1]->w-m_simplex->c[3]->w,
- m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
- return(true);
- }
- break;
- }
- return(false);
- }
- /* Internals */
- void getsupport(const btVector3& d,sSV& sv) const
- {
- sv.d = d/d.length();
- sv.w = m_shape.Support(sv.d);
- }
- void removevertice(sSimplex& simplex)
- {
- m_free[m_nfree++]=simplex.c[--simplex.rank];
- }
- void appendvertice(sSimplex& simplex,const btVector3& v)
- {
- simplex.p[simplex.rank]=0;
- simplex.c[simplex.rank]=m_free[--m_nfree];
- getsupport(v,*simplex.c[simplex.rank++]);
- }
- static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c)
- {
- return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
- a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
- a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
- }
- static btScalar projectorigin( const btVector3& a,
- const btVector3& b,
- btScalar* w,U& m)
- {
- const btVector3 d=b-a;
- const btScalar l=d.length2();
- if(l>GJK_SIMPLEX2_EPS)
- {
- const btScalar t(l>0?-btDot(a,d)/l:0);
- if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
- else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
- else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
- }
- return(-1);
- }
- static btScalar projectorigin( const btVector3& a,
- const btVector3& b,
- const btVector3& c,
- btScalar* w,U& m)
- {
- static const U imd3[]={1,2,0};
- const btVector3* vt[]={&a,&b,&c};
- const btVector3 dl[]={a-b,b-c,c-a};
- const btVector3 n=btCross(dl[0],dl[1]);
- const btScalar l=n.length2();
- if(l>GJK_SIMPLEX3_EPS)
- {
- btScalar mindist=-1;
- btScalar subw[2]={0.f,0.f};
- U subm(0);
- for(U i=0;i<3;++i)
- {
- if(btDot(*vt[i],btCross(dl[i],n))>0)
- {
- const U j=imd3[i];
- const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
- if((mindist<0)||(subd<mindist))
- {
- mindist = subd;
- m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
- w[i] = subw[0];
- w[j] = subw[1];
- w[imd3[j]] = 0;
- }
- }
- }
- if(mindist<0)
- {
- const btScalar d=btDot(a,n);
- const btScalar s=btSqrt(l);
- const btVector3 p=n*(d/l);
- mindist = p.length2();
- m = 7;
- w[0] = (btCross(dl[1],b-p)).length()/s;
- w[1] = (btCross(dl[2],c-p)).length()/s;
- w[2] = 1-(w[0]+w[1]);
- }
- return(mindist);
- }
- return(-1);
- }
- static btScalar projectorigin( const btVector3& a,
- const btVector3& b,
- const btVector3& c,
- const btVector3& d,
- btScalar* w,U& m)
- {
- static const U imd3[]={1,2,0};
- const btVector3* vt[]={&a,&b,&c,&d};
- const btVector3 dl[]={a-d,b-d,c-d};
- const btScalar vl=det(dl[0],dl[1],dl[2]);
- const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
- if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
- {
- btScalar mindist=-1;
- btScalar subw[3]={0.f,0.f,0.f};
- U subm(0);
- for(U i=0;i<3;++i)
- {
- const U j=imd3[i];
- const btScalar s=vl*btDot(d,btCross(dl[i],dl[j]));
- if(s>0)
- {
- const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
- if((mindist<0)||(subd<mindist))
- {
- mindist = subd;
- m = static_cast<U>((subm&1?1<<i:0)+
- (subm&2?1<<j:0)+
- (subm&4?8:0));
- w[i] = subw[0];
- w[j] = subw[1];
- w[imd3[j]] = 0;
- w[3] = subw[2];
- }
- }
- }
- if(mindist<0)
- {
- mindist = 0;
- m = 15;
- w[0] = det(c,b,d)/vl;
- w[1] = det(a,c,d)/vl;
- w[2] = det(b,a,d)/vl;
- w[3] = 1-(w[0]+w[1]+w[2]);
- }
- return(mindist);
- }
- return(-1);
- }
- };
-
-
-enum eEpaStatus
-{
- eEpaValid,
- eEpaTouching,
- eEpaDegenerated,
- eEpaNonConvex,
- eEpaInvalidHull,
- eEpaOutOfFaces,
- eEpaOutOfVertices,
- eEpaAccuraryReached,
- eEpaFallBack,
- eEpaFailed
-};
-
-
- // EPA
-template <typename btConvexTemplate>
- struct EPA
- {
- /* Types */
-
- struct sFace
- {
- btVector3 n;
- btScalar d;
- typename GJK<btConvexTemplate>::sSV* c[3];
- sFace* f[3];
- sFace* l[2];
- U1 e[3];
- U1 pass;
- };
- struct sList
- {
- sFace* root;
- U count;
- sList() : root(0),count(0) {}
- };
- struct sHorizon
- {
- sFace* cf;
- sFace* ff;
- U nf;
- sHorizon() : cf(0),ff(0),nf(0) {}
- };
-
- /* Fields */
- eEpaStatus m_status;
- typename GJK<btConvexTemplate>::sSimplex m_result;
- btVector3 m_normal;
- btScalar m_depth;
- typename GJK<btConvexTemplate>::sSV m_sv_store[EPA_MAX_VERTICES];
- sFace m_fc_store[EPA_MAX_FACES];
- U m_nextsv;
- sList m_hull;
- sList m_stock;
- /* Methods */
- EPA()
- {
- Initialize();
- }
-
-
- static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
- {
- fa->e[ea]=(U1)eb;fa->f[ea]=fb;
- fb->e[eb]=(U1)ea;fb->f[eb]=fa;
- }
- static inline void append(sList& list,sFace* face)
- {
- face->l[0] = 0;
- face->l[1] = list.root;
- if(list.root) list.root->l[0]=face;
- list.root = face;
- ++list.count;
- }
- static inline void remove(sList& list,sFace* face)
- {
- if(face->l[1]) face->l[1]->l[0]=face->l[0];
- if(face->l[0]) face->l[0]->l[1]=face->l[1];
- if(face==list.root) list.root=face->l[1];
- --list.count;
- }
-
-
- void Initialize()
- {
- m_status = eEpaFailed;
- m_normal = btVector3(0,0,0);
- m_depth = 0;
- m_nextsv = 0;
- for(U i=0;i<EPA_MAX_FACES;++i)
- {
- append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
- }
- }
- eEpaStatus Evaluate(GJK<btConvexTemplate>& gjk,const btVector3& guess)
- {
- typename GJK<btConvexTemplate>::sSimplex& simplex=*gjk.m_simplex;
- if((simplex.rank>1)&&gjk.EncloseOrigin())
- {
-
- /* Clean up */
- while(m_hull.root)
- {
- sFace* f = m_hull.root;
- remove(m_hull,f);
- append(m_stock,f);
- }
- m_status = eEpaValid;
- m_nextsv = 0;
- /* Orient simplex */
- if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
- simplex.c[1]->w-simplex.c[3]->w,
- simplex.c[2]->w-simplex.c[3]->w)<0)
- {
- btSwap(simplex.c[0],simplex.c[1]);
- btSwap(simplex.p[0],simplex.p[1]);
- }
- /* Build initial hull */
- sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
- newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
- newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
- newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
- if(m_hull.count==4)
- {
- sFace* best=findbest();
- sFace outer=*best;
- U pass=0;
- U iterations=0;
- bind(tetra[0],0,tetra[1],0);
- bind(tetra[0],1,tetra[2],0);
- bind(tetra[0],2,tetra[3],0);
- bind(tetra[1],1,tetra[3],2);
- bind(tetra[1],2,tetra[2],1);
- bind(tetra[2],2,tetra[3],1);
- m_status=eEpaValid;
- for(;iterations<EPA_MAX_ITERATIONS;++iterations)
- {
- if(m_nextsv<EPA_MAX_VERTICES)
- {
- sHorizon horizon;
- typename GJK<btConvexTemplate>::sSV* w=&m_sv_store[m_nextsv++];
- bool valid=true;
- best->pass = (U1)(++pass);
- gjk.getsupport(best->n,*w);
- const btScalar wdist=btDot(best->n,w->w)-best->d;
- if(wdist>EPA_ACCURACY)
- {
- for(U j=0;(j<3)&&valid;++j)
- {
- valid&=expand( pass,w,
- best->f[j],best->e[j],
- horizon);
- }
- if(valid&&(horizon.nf>=3))
- {
- bind(horizon.cf,1,horizon.ff,2);
- remove(m_hull,best);
- append(m_stock,best);
- best=findbest();
- outer=*best;
- } else { m_status=eEpaInvalidHull;break; }
- } else { m_status=eEpaAccuraryReached;break; }
- } else { m_status=eEpaOutOfVertices;break; }
- }
- const btVector3 projection=outer.n*outer.d;
- m_normal = outer.n;
- m_depth = outer.d;
- m_result.rank = 3;
- m_result.c[0] = outer.c[0];
- m_result.c[1] = outer.c[1];
- m_result.c[2] = outer.c[2];
- m_result.p[0] = btCross( outer.c[1]->w-projection,
- outer.c[2]->w-projection).length();
- m_result.p[1] = btCross( outer.c[2]->w-projection,
- outer.c[0]->w-projection).length();
- m_result.p[2] = btCross( outer.c[0]->w-projection,
- outer.c[1]->w-projection).length();
- const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
- m_result.p[0] /= sum;
- m_result.p[1] /= sum;
- m_result.p[2] /= sum;
- return(m_status);
- }
- }
- /* Fallback */
- m_status = eEpaFallBack;
- m_normal = -guess;
- const btScalar nl=m_normal.length();
- if(nl>0)
- m_normal = m_normal/nl;
- else
- m_normal = btVector3(1,0,0);
- m_depth = 0;
- m_result.rank=1;
- m_result.c[0]=simplex.c[0];
- m_result.p[0]=1;
- return(m_status);
- }
- bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist)
- {
- const btVector3 ba = b->w - a->w;
- const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
- const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
-
- if(a_dot_nab < 0)
- {
- // Outside of edge a->b
-
- const btScalar ba_l2 = ba.length2();
- const btScalar a_dot_ba = btDot(a->w, ba);
- const btScalar b_dot_ba = btDot(b->w, ba);
-
- if(a_dot_ba > 0)
- {
- // Pick distance vertex a
- dist = a->w.length();
- }
- else if(b_dot_ba < 0)
- {
- // Pick distance vertex b
- dist = b->w.length();
- }
- else
- {
- // Pick distance to edge a->b
- const btScalar a_dot_b = btDot(a->w, b->w);
- dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
- }
-
- return true;
- }
-
- return false;
- }
- sFace* newface(typename GJK<btConvexTemplate>::sSV* a,typename GJK<btConvexTemplate>::sSV* b,typename GJK<btConvexTemplate>::sSV* c,bool forced)
- {
- if(m_stock.root)
- {
- sFace* face=m_stock.root;
- remove(m_stock,face);
- append(m_hull,face);
- face->pass = 0;
- face->c[0] = a;
- face->c[1] = b;
- face->c[2] = c;
- face->n = btCross(b->w-a->w,c->w-a->w);
- const btScalar l=face->n.length();
- const bool v=l>EPA_ACCURACY;
-
- if(v)
- {
- if(!(getedgedist(face, a, b, face->d) ||
- getedgedist(face, b, c, face->d) ||
- getedgedist(face, c, a, face->d)))
- {
- // Origin projects to the interior of the triangle
- // Use distance to triangle plane
- face->d = btDot(a->w, face->n) / l;
- }
-
- face->n /= l;
- if(forced || (face->d >= -EPA_PLANE_EPS))
- {
- return face;
- }
- else
- m_status=eEpaNonConvex;
- }
- else
- m_status=eEpaDegenerated;
-
- remove(m_hull, face);
- append(m_stock, face);
- return 0;
-
- }
- m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces;
- return 0;
- }
- sFace* findbest()
- {
- sFace* minf=m_hull.root;
- btScalar mind=minf->d*minf->d;
- for(sFace* f=minf->l[1];f;f=f->l[1])
- {
- const btScalar sqd=f->d*f->d;
- if(sqd<mind)
- {
- minf=f;
- mind=sqd;
- }
- }
- return(minf);
- }
- bool expand(U pass,typename GJK<btConvexTemplate>::sSV* w,sFace* f,U e,sHorizon& horizon)
- {
- static const U i1m3[]={1,2,0};
- static const U i2m3[]={2,0,1};
- if(f->pass!=pass)
- {
- const U e1=i1m3[e];
- if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
- {
- sFace* nf=newface(f->c[e1],f->c[e],w,false);
- if(nf)
- {
- bind(nf,0,f,e);
- if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
- horizon.cf=nf;
- ++horizon.nf;
- return(true);
- }
- }
- else
- {
- const U e2=i2m3[e];
- f->pass = (U1)pass;
- if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
- expand(pass,w,f->f[e2],f->e[e2],horizon))
- {
- remove(m_hull,f);
- append(m_stock,f);
- return(true);
- }
- }
- }
- return(false);
- }
-
- };
-
- template <typename btConvexTemplate>
- static void Initialize( const btConvexTemplate& a, const btConvexTemplate& b,
- btGjkEpaSolver3::sResults& results,
- MinkowskiDiff<btConvexTemplate>& shape)
- {
- /* Results */
- results.witnesses[0] =
- results.witnesses[1] = btVector3(0,0,0);
- results.status = btGjkEpaSolver3::sResults::Separated;
- /* Shape */
-
- shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis());
- shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform());
-
- }
-
-
-//
-// Api
-//
-
-
-
-//
-template <typename btConvexTemplate>
-bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b,
- const btVector3& guess,
- btGjkEpaSolver3::sResults& results)
-{
- MinkowskiDiff<btConvexTemplate> shape(a,b);
- Initialize(a,b,results,shape);
- GJK<btConvexTemplate> gjk(a,b);
- eGjkStatus gjk_status=gjk.Evaluate(shape,guess);
- if(gjk_status==eGjkValid)
- {
- btVector3 w0=btVector3(0,0,0);
- btVector3 w1=btVector3(0,0,0);
- for(U i=0;i<gjk.m_simplex->rank;++i)
- {
- const btScalar p=gjk.m_simplex->p[i];
- w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
- w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
- }
- results.witnesses[0] = a.getWorldTransform()*w0;
- results.witnesses[1] = a.getWorldTransform()*w1;
- results.normal = w0-w1;
- results.distance = results.normal.length();
- results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
- return(true);
- }
- else
- {
- results.status = gjk_status==eGjkInside?
- btGjkEpaSolver3::sResults::Penetrating :
- btGjkEpaSolver3::sResults::GJK_Failed ;
- return(false);
- }
-}
-
-
-template <typename btConvexTemplate>
-bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a,
- const btConvexTemplate& b,
- const btVector3& guess,
- btGjkEpaSolver3::sResults& results)
-{
- MinkowskiDiff<btConvexTemplate> shape(a,b);
- Initialize(a,b,results,shape);
- GJK<btConvexTemplate> gjk(a,b);
- eGjkStatus gjk_status=gjk.Evaluate(shape,-guess);
- switch(gjk_status)
- {
- case eGjkInside:
- {
- EPA<btConvexTemplate> epa;
- eEpaStatus epa_status=epa.Evaluate(gjk,-guess);
- if(epa_status!=eEpaFailed)
- {
- btVector3 w0=btVector3(0,0,0);
- for(U i=0;i<epa.m_result.rank;++i)
- {
- w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
- }
- results.status = btGjkEpaSolver3::sResults::Penetrating;
- results.witnesses[0] = a.getWorldTransform()*w0;
- results.witnesses[1] = a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth);
- results.normal = -epa.m_normal;
- results.distance = -epa.m_depth;
- return(true);
- } else results.status=btGjkEpaSolver3::sResults::EPA_Failed;
- }
- break;
- case eGjkFailed:
- results.status=btGjkEpaSolver3::sResults::GJK_Failed;
- break;
- default:
- {
- }
- }
- return(false);
-}
-
-#if 0
-int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo)
-{
- btGjkEpaSolver3::sResults results;
- btVector3 guess = colDesc.m_firstDir;
-
- bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB,
- colDesc.m_transformA,colDesc.m_transformB,
- colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB,
- guess,
- results);
- if (res)
- {
- if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside)
- {
- //normal could be 'swapped'
-
- distInfo->m_distance = results.distance;
- distInfo->m_normalBtoA = results.normal;
- btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0];
- btScalar lenSqr = tmpNormalInB.length2();
- if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
- {
- tmpNormalInB = results.normal;
- lenSqr = results.normal.length2();
- }
-
- if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
- {
- tmpNormalInB /= btSqrt(lenSqr);
- btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length();
- //only replace valid penetrations when the result is deeper (check)
- //if ((distance2 < results.distance))
- {
- distInfo->m_distance = distance2;
- distInfo->m_pointOnA= results.witnesses[0];
- distInfo->m_pointOnB= results.witnesses[1];
- distInfo->m_normalBtoA= tmpNormalInB;
- return 0;
- }
- }
- }
-
- }
-
- return -1;
-}
-#endif
-
-template <typename btConvexTemplate, typename btDistanceInfoTemplate>
-int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b,
- const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo)
-{
- btGjkEpaSolver3::sResults results;
- btVector3 guess = colDesc.m_firstDir;
-
- bool isSeparated = btGjkEpaSolver3_Distance( a,b,
- guess,
- results);
- if (isSeparated)
- {
- distInfo->m_distance = results.distance;
- distInfo->m_pointOnA= results.witnesses[0];
- distInfo->m_pointOnB= results.witnesses[1];
- distInfo->m_normalBtoA= results.normal;
- return 0;
- }
-
- return -1;
-}
-
-/* Symbols cleanup */
-
-#undef GJK_MAX_ITERATIONS
-#undef GJK_ACCURARY
-#undef GJK_MIN_DISTANCE
-#undef GJK_DUPLICATED_EPS
-#undef GJK_SIMPLEX2_EPS
-#undef GJK_SIMPLEX3_EPS
-#undef GJK_SIMPLEX4_EPS
-
-#undef EPA_MAX_VERTICES
-#undef EPA_MAX_FACES
-#undef EPA_MAX_ITERATIONS
-#undef EPA_ACCURACY
-#undef EPA_FALLBACK
-#undef EPA_PLANE_EPS
-#undef EPA_INSIDE_EPS
-
-
-
-#endif //BT_GJK_EPA3_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
deleted file mode 100644
index 572ec36f56..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-EPA Copyright (c) Ricardo Padrela 2006
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include "BulletCollision/CollisionShapes/btConvexShape.h"
-#include "btGjkEpaPenetrationDepthSolver.h"
-
-
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
-
-bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
- const btConvexShape* pConvexA, const btConvexShape* pConvexB,
- const btTransform& transformA, const btTransform& transformB,
- btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
- class btIDebugDraw* debugDraw)
-{
-
- (void)debugDraw;
- (void)v;
- (void)simplexSolver;
-
-// const btScalar radialmargin(btScalar(0.));
-
- btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin());
- btGjkEpaSolver2::sResults results;
-
-
- if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
- pConvexB,transformB,
- guessVector,results))
-
- {
- // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
- //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
- wWitnessOnA = results.witnesses[0];
- wWitnessOnB = results.witnesses[1];
- v = results.normal;
- return true;
- } else
- {
- if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results))
- {
- wWitnessOnA = results.witnesses[0];
- wWitnessOnB = results.witnesses[1];
- v = results.normal;
- return false;
- }
- }
-
- return false;
-}
-
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
deleted file mode 100644
index 1ed6340af3..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-EPA Copyright (c) Ricardo Padrela 2006
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H
-#define BT_GJP_EPA_PENETRATION_DEPTH_H
-
-#include "btConvexPenetrationDepthSolver.h"
-
-///EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
-///calculate the penetration depth between two convex shapes.
-class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
-{
- public :
-
- btGjkEpaPenetrationDepthSolver()
- {
- }
-
- bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
- const btConvexShape* pConvexA, const btConvexShape* pConvexB,
- const btTransform& transformA, const btTransform& transformB,
- btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
- class btIDebugDraw* debugDraw);
-
- private :
-
-};
-
-#endif // BT_GJP_EPA_PENETRATION_DEPTH_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
deleted file mode 100644
index 257b026d9b..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
+++ /dev/null
@@ -1,467 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include "btGjkPairDetector.h"
-#include "BulletCollision/CollisionShapes/btConvexShape.h"
-#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
-#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
-
-
-
-#if defined(DEBUG) || defined (_DEBUG)
-//#define TEST_NON_VIRTUAL 1
-#include <stdio.h> //for debug printf
-#ifdef __SPU__
-#include <spu_printf.h>
-#define printf spu_printf
-#endif //__SPU__
-#endif
-
-//must be above the machine epsilon
-#ifdef BT_USE_DOUBLE_PRECISION
- #define REL_ERROR2 btScalar(1.0e-12)
- btScalar gGjkEpaPenetrationTolerance = 1e-7;
-#else
- #define REL_ERROR2 btScalar(1.0e-6)
- btScalar gGjkEpaPenetrationTolerance = 0.001;
-#endif
-
-//temp globals, to improve GJK/EPA/penetration calculations
-int gNumDeepPenetrationChecks = 0;
-int gNumGjkChecks = 0;
-
-
-btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
-:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_simplexSolver(simplexSolver),
-m_minkowskiA(objectA),
-m_minkowskiB(objectB),
-m_shapeTypeA(objectA->getShapeType()),
-m_shapeTypeB(objectB->getShapeType()),
-m_marginA(objectA->getMargin()),
-m_marginB(objectB->getMargin()),
-m_ignoreMargin(false),
-m_lastUsedMethod(-1),
-m_catchDegeneracies(1),
-m_fixContactNormalDirection(1)
-{
-}
-btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
-:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
-m_penetrationDepthSolver(penetrationDepthSolver),
-m_simplexSolver(simplexSolver),
-m_minkowskiA(objectA),
-m_minkowskiB(objectB),
-m_shapeTypeA(shapeTypeA),
-m_shapeTypeB(shapeTypeB),
-m_marginA(marginA),
-m_marginB(marginB),
-m_ignoreMargin(false),
-m_lastUsedMethod(-1),
-m_catchDegeneracies(1),
-m_fixContactNormalDirection(1)
-{
-}
-
-void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
-{
- (void)swapResults;
-
- getClosestPointsNonVirtual(input,output,debugDraw);
-}
-
-#ifdef __SPU__
-void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
-#else
-void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw)
-#endif
-{
- m_cachedSeparatingDistance = 0.f;
-
- btScalar distance=btScalar(0.);
- btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.));
-
- btVector3 pointOnA,pointOnB;
- btTransform localTransA = input.m_transformA;
- btTransform localTransB = input.m_transformB;
- btVector3 positionOffset=(localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5);
- localTransA.getOrigin() -= positionOffset;
- localTransB.getOrigin() -= positionOffset;
-
- bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d();
-
- btScalar marginA = m_marginA;
- btScalar marginB = m_marginB;
-
- gNumGjkChecks++;
-
- //for CCD we don't use margins
- if (m_ignoreMargin)
- {
- marginA = btScalar(0.);
- marginB = btScalar(0.);
- }
-
- m_curIter = 0;
- int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
- m_cachedSeparatingAxis.setValue(0,1,0);
-
- bool isValid = false;
- bool checkSimplex = false;
- bool checkPenetration = true;
- m_degenerateSimplex = 0;
-
- m_lastUsedMethod = -1;
-
- {
- btScalar squaredDistance = BT_LARGE_FLOAT;
- btScalar delta = btScalar(0.);
-
- btScalar margin = marginA + marginB;
-
-
-
- m_simplexSolver->reset();
-
- for ( ; ; )
- //while (true)
- {
-
- btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
- btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
-
-
- btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
- btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
-
- btVector3 pWorld = localTransA(pInA);
- btVector3 qWorld = localTransB(qInB);
-
-
- if (check2d)
- {
- pWorld[2] = 0.f;
- qWorld[2] = 0.f;
- }
-
- btVector3 w = pWorld - qWorld;
- delta = m_cachedSeparatingAxis.dot(w);
-
- // potential exit, they don't overlap
- if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
- {
- m_degenerateSimplex = 10;
- checkSimplex=true;
- //checkPenetration = false;
- break;
- }
-
- //exit 0: the new point is already in the simplex, or we didn't come any closer
- if (m_simplexSolver->inSimplex(w))
- {
- m_degenerateSimplex = 1;
- checkSimplex = true;
- break;
- }
- // are we getting any closer ?
- btScalar f0 = squaredDistance - delta;
- btScalar f1 = squaredDistance * REL_ERROR2;
-
- if (f0 <= f1)
- {
- if (f0 <= btScalar(0.))
- {
- m_degenerateSimplex = 2;
- } else
- {
- m_degenerateSimplex = 11;
- }
- checkSimplex = true;
- break;
- }
-
- //add current vertex to simplex
- m_simplexSolver->addVertex(w, pWorld, qWorld);
- btVector3 newCachedSeparatingAxis;
-
- //calculate the closest point to the origin (update vector v)
- if (!m_simplexSolver->closest(newCachedSeparatingAxis))
- {
- m_degenerateSimplex = 3;
- checkSimplex = true;
- break;
- }
-
- if(newCachedSeparatingAxis.length2()<REL_ERROR2)
- {
- m_cachedSeparatingAxis = newCachedSeparatingAxis;
- m_degenerateSimplex = 6;
- checkSimplex = true;
- break;
- }
-
- btScalar previousSquaredDistance = squaredDistance;
- squaredDistance = newCachedSeparatingAxis.length2();
-#if 0
-///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
- if (squaredDistance>previousSquaredDistance)
- {
- m_degenerateSimplex = 7;
- squaredDistance = previousSquaredDistance;
- checkSimplex = false;
- break;
- }
-#endif //
-
-
- //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
-
- //are we getting any closer ?
- if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
- {
-// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
- checkSimplex = true;
- m_degenerateSimplex = 12;
-
- break;
- }
-
- m_cachedSeparatingAxis = newCachedSeparatingAxis;
-
- //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
- if (m_curIter++ > gGjkMaxIter)
- {
- #if defined(DEBUG) || defined (_DEBUG)
-
- printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter);
- printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
- m_cachedSeparatingAxis.getX(),
- m_cachedSeparatingAxis.getY(),
- m_cachedSeparatingAxis.getZ(),
- squaredDistance,
- m_minkowskiA->getShapeType(),
- m_minkowskiB->getShapeType());
-
- #endif
- break;
-
- }
-
-
- bool check = (!m_simplexSolver->fullSimplex());
- //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
-
- if (!check)
- {
- //do we need this backup_closest here ?
-// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
- m_degenerateSimplex = 13;
- break;
- }
- }
-
- if (checkSimplex)
- {
- m_simplexSolver->compute_points(pointOnA, pointOnB);
- normalInB = m_cachedSeparatingAxis;
-
- btScalar lenSqr =m_cachedSeparatingAxis.length2();
-
- //valid normal
- if (lenSqr < REL_ERROR2)
- {
- m_degenerateSimplex = 5;
- }
- if (lenSqr > SIMD_EPSILON*SIMD_EPSILON)
- {
- btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
- normalInB *= rlen; //normalize
-
- btScalar s = btSqrt(squaredDistance);
-
- btAssert(s > btScalar(0.0));
- pointOnA -= m_cachedSeparatingAxis * (marginA / s);
- pointOnB += m_cachedSeparatingAxis * (marginB / s);
- distance = ((btScalar(1.)/rlen) - margin);
- isValid = true;
-
- m_lastUsedMethod = 1;
- } else
- {
- m_lastUsedMethod = 2;
- }
- }
-
- bool catchDegeneratePenetrationCase =
- (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
-
- //if (checkPenetration && !isValid)
- if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
- {
- //penetration case
-
- //if there is no way to handle penetrations, bail out
- if (m_penetrationDepthSolver)
- {
- // Penetration depth case.
- btVector3 tmpPointOnA,tmpPointOnB;
-
- gNumDeepPenetrationChecks++;
- m_cachedSeparatingAxis.setZero();
-
- bool isValid2 = m_penetrationDepthSolver->calcPenDepth(
- *m_simplexSolver,
- m_minkowskiA,m_minkowskiB,
- localTransA,localTransB,
- m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
- debugDraw
- );
-
-
- if (isValid2)
- {
- btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA;
- btScalar lenSqr = tmpNormalInB.length2();
- if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
- {
- tmpNormalInB = m_cachedSeparatingAxis;
- lenSqr = m_cachedSeparatingAxis.length2();
- }
-
- if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
- {
- tmpNormalInB /= btSqrt(lenSqr);
- btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length();
- m_lastUsedMethod = 3;
- //only replace valid penetrations when the result is deeper (check)
- if (!isValid || (distance2 < distance))
- {
- distance = distance2;
- pointOnA = tmpPointOnA;
- pointOnB = tmpPointOnB;
- normalInB = tmpNormalInB;
-
- isValid = true;
-
- } else
- {
- m_lastUsedMethod = 8;
- }
- } else
- {
- m_lastUsedMethod = 9;
- }
- } else
-
- {
- ///this is another degenerate case, where the initial GJK calculation reports a degenerate case
- ///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
- ///reports a valid positive distance. Use the results of the second GJK instead of failing.
- ///thanks to Jacob.Langford for the reproduction case
- ///http://code.google.com/p/bullet/issues/detail?id=250
-
-
- if (m_cachedSeparatingAxis.length2() > btScalar(0.))
- {
- btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin;
- //only replace valid distances when the distance is less
- if (!isValid || (distance2 < distance))
- {
- distance = distance2;
- pointOnA = tmpPointOnA;
- pointOnB = tmpPointOnB;
- pointOnA -= m_cachedSeparatingAxis * marginA ;
- pointOnB += m_cachedSeparatingAxis * marginB ;
- normalInB = m_cachedSeparatingAxis;
- normalInB.normalize();
-
- isValid = true;
- m_lastUsedMethod = 6;
- } else
- {
- m_lastUsedMethod = 5;
- }
- }
- }
-
- }
-
- }
- }
-
-
-
- if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared)))
- {
-
- m_cachedSeparatingAxis = normalInB;
- m_cachedSeparatingDistance = distance;
-
- {
- ///todo: need to track down this EPA penetration solver degeneracy
- ///the penetration solver reports penetration but the contact normal
- ///connecting the contact points is pointing in the opposite direction
- ///until then, detect the issue and revert the normal
-
- btScalar d1=0;
- {
- btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
- btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
-
-
- btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
- btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
-
- btVector3 pWorld = localTransA(pInA);
- btVector3 qWorld = localTransB(qInB);
- btVector3 w = pWorld - qWorld;
- d1 = (-normalInB).dot(w);
- }
- btScalar d0 = 0.f;
- {
- btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis();
- btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis();
-
-
- btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
- btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
-
- btVector3 pWorld = localTransA(pInA);
- btVector3 qWorld = localTransB(qInB);
- btVector3 w = pWorld - qWorld;
- d0 = normalInB.dot(w);
- }
- if (d1>d0)
- {
- m_lastUsedMethod = 10;
- normalInB*=-1;
- }
-
- }
- output.addContactPoint(
- normalInB,
- pointOnB+positionOffset,
- distance);
-
- }
-
-
-}
-
-
-
-
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
deleted file mode 100644
index feeae68621..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-
-
-#ifndef BT_GJK_PAIR_DETECTOR_H
-#define BT_GJK_PAIR_DETECTOR_H
-
-#include "btDiscreteCollisionDetectorInterface.h"
-#include "BulletCollision/CollisionShapes/btCollisionMargin.h"
-
-class btConvexShape;
-#include "btSimplexSolverInterface.h"
-class btConvexPenetrationDepthSolver;
-
-/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
-class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
-{
-
-
- btVector3 m_cachedSeparatingAxis;
- btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
- btSimplexSolverInterface* m_simplexSolver;
- const btConvexShape* m_minkowskiA;
- const btConvexShape* m_minkowskiB;
- int m_shapeTypeA;
- int m_shapeTypeB;
- btScalar m_marginA;
- btScalar m_marginB;
-
- bool m_ignoreMargin;
- btScalar m_cachedSeparatingDistance;
-
-
-public:
-
- //some debugging to fix degeneracy problems
- int m_lastUsedMethod;
- int m_curIter;
- int m_degenerateSimplex;
- int m_catchDegeneracies;
- int m_fixContactNormalDirection;
-
- btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
- btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
- virtual ~btGjkPairDetector() {};
-
- virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
-
- void getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);
-
-
- void setMinkowskiA(const btConvexShape* minkA)
- {
- m_minkowskiA = minkA;
- }
-
- void setMinkowskiB(const btConvexShape* minkB)
- {
- m_minkowskiB = minkB;
- }
- void setCachedSeperatingAxis(const btVector3& seperatingAxis)
- {
- m_cachedSeparatingAxis = seperatingAxis;
- }
-
- const btVector3& getCachedSeparatingAxis() const
- {
- return m_cachedSeparatingAxis;
- }
- btScalar getCachedSeparatingDistance() const
- {
- return m_cachedSeparatingDistance;
- }
-
- void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver)
- {
- m_penetrationDepthSolver = penetrationDepthSolver;
- }
-
- ///don't use setIgnoreMargin, it's for Bullet's internal use
- void setIgnoreMargin(bool ignoreMargin)
- {
- m_ignoreMargin = ignoreMargin;
- }
-
-
-};
-
-#endif //BT_GJK_PAIR_DETECTOR_H
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
deleted file mode 100644
index 571ad2c5f7..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
+++ /dev/null
@@ -1,180 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_MANIFOLD_CONTACT_POINT_H
-#define BT_MANIFOLD_CONTACT_POINT_H
-
-#include "LinearMath/btVector3.h"
-#include "LinearMath/btTransformUtil.h"
-
-#ifdef PFX_USE_FREE_VECTORMATH
- #include "physics_effects/base_level/solver/pfx_constraint_row.h"
-typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow;
-#else
- // Don't change following order of parameters
- ATTRIBUTE_ALIGNED16(struct) btConstraintRow {
- btScalar m_normal[3];
- btScalar m_rhs;
- btScalar m_jacDiagInv;
- btScalar m_lowerLimit;
- btScalar m_upperLimit;
- btScalar m_accumImpulse;
- };
- typedef btConstraintRow PfxConstraintRow;
-#endif //PFX_USE_FREE_VECTORMATH
-
-enum btContactPointFlags
-{
- BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1,
- BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
- BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
- BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
- BT_CONTACT_FLAG_FRICTION_ANCHOR = 16,
-};
-
-/// ManifoldContactPoint collects and maintains persistent contactpoints.
-/// used to improve stability and performance of rigidbody dynamics response.
-class btManifoldPoint
- {
- public:
- btManifoldPoint()
- :m_userPersistentData(0),
- m_contactPointFlags(0),
- m_appliedImpulse(0.f),
- m_appliedImpulseLateral1(0.f),
- m_appliedImpulseLateral2(0.f),
- m_contactMotion1(0.f),
- m_contactMotion2(0.f),
- m_contactCFM(0.f),
- m_contactERP(0.f),
- m_frictionCFM(0.f),
- m_lifeTime(0)
- {
- }
-
- btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB,
- const btVector3 &normal,
- btScalar distance ) :
- m_localPointA( pointA ),
- m_localPointB( pointB ),
- m_normalWorldOnB( normal ),
- m_distance1( distance ),
- m_combinedFriction(btScalar(0.)),
- m_combinedRollingFriction(btScalar(0.)),
- m_combinedSpinningFriction(btScalar(0.)),
- m_combinedRestitution(btScalar(0.)),
- m_userPersistentData(0),
- m_contactPointFlags(0),
- m_appliedImpulse(0.f),
- m_appliedImpulseLateral1(0.f),
- m_appliedImpulseLateral2(0.f),
- m_contactMotion1(0.f),
- m_contactMotion2(0.f),
- m_contactCFM(0.f),
- m_contactERP(0.f),
- m_frictionCFM(0.f),
- m_lifeTime(0)
- {
-
- }
-
-
-
- btVector3 m_localPointA;
- btVector3 m_localPointB;
- btVector3 m_positionWorldOnB;
- ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
- btVector3 m_positionWorldOnA;
- btVector3 m_normalWorldOnB;
-
- btScalar m_distance1;
- btScalar m_combinedFriction;
- btScalar m_combinedRollingFriction;//torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever
- btScalar m_combinedSpinningFriction;//torsional friction around contact normal, useful for grasping objects
- btScalar m_combinedRestitution;
-
- //BP mod, store contact triangles.
- int m_partId0;
- int m_partId1;
- int m_index0;
- int m_index1;
-
- mutable void* m_userPersistentData;
- //bool m_lateralFrictionInitialized;
- int m_contactPointFlags;
-
- btScalar m_appliedImpulse;
- btScalar m_appliedImpulseLateral1;
- btScalar m_appliedImpulseLateral2;
- btScalar m_contactMotion1;
- btScalar m_contactMotion2;
-
- union
- {
- btScalar m_contactCFM;
- btScalar m_combinedContactStiffness1;
- };
-
- union
- {
- btScalar m_contactERP;
- btScalar m_combinedContactDamping1;
- };
-
- btScalar m_frictionCFM;
-
- int m_lifeTime;//lifetime of the contactpoint in frames
-
- btVector3 m_lateralFrictionDir1;
- btVector3 m_lateralFrictionDir2;
-
-
-
-
- btScalar getDistance() const
- {
- return m_distance1;
- }
- int getLifeTime() const
- {
- return m_lifeTime;
- }
-
- const btVector3& getPositionWorldOnA() const {
- return m_positionWorldOnA;
-// return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
- }
-
- const btVector3& getPositionWorldOnB() const
- {
- return m_positionWorldOnB;
- }
-
- void setDistance(btScalar dist)
- {
- m_distance1 = dist;
- }
-
- ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver
- btScalar getAppliedImpulse() const
- {
- return m_appliedImpulse;
- }
-
-
-
- };
-
-#endif //BT_MANIFOLD_CONTACT_POINT_H
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
deleted file mode 100644
index fa45f49037..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
+++ /dev/null
@@ -1,361 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#include "btMinkowskiPenetrationDepthSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-#include "BulletCollision/CollisionShapes/btConvexShape.h"
-
-#define NUM_UNITSPHERE_POINTS 42
-
-
-bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
- const btConvexShape* convexA,const btConvexShape* convexB,
- const btTransform& transA,const btTransform& transB,
- btVector3& v, btVector3& pa, btVector3& pb,
- class btIDebugDraw* debugDraw
- )
-{
-
- (void)v;
-
- bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
-
- struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
- {
-
- btIntermediateResult():m_hasResult(false)
- {
- }
-
- btVector3 m_normalOnBInWorld;
- btVector3 m_pointInWorld;
- btScalar m_depth;
- bool m_hasResult;
-
- virtual void setShapeIdentifiersA(int partId0,int index0)
- {
- (void)partId0;
- (void)index0;
- }
- virtual void setShapeIdentifiersB(int partId1,int index1)
- {
- (void)partId1;
- (void)index1;
- }
- void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
- {
- m_normalOnBInWorld = normalOnBInWorld;
- m_pointInWorld = pointInWorld;
- m_depth = depth;
- m_hasResult = true;
- }
- };
-
- //just take fixed number of orientation, and sample the penetration depth in that direction
- btScalar minProj = btScalar(BT_LARGE_FLOAT);
- btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
- btVector3 minA,minB;
- btVector3 seperatingAxisInA,seperatingAxisInB;
- btVector3 pInA,qInB,pWorld,qWorld,w;
-
-#ifndef __SPU__
-#define USE_BATCHED_SUPPORT 1
-#endif
-#ifdef USE_BATCHED_SUPPORT
-
- btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
- btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
- btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
- btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
- int i;
-
- int numSampleDirections = NUM_UNITSPHERE_POINTS;
-
- for (i=0;i<numSampleDirections;i++)
- {
- btVector3 norm = getPenetrationDirections()[i];
- seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
- seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
- }
-
- {
- int numPDA = convexA->getNumPreferredPenetrationDirections();
- if (numPDA)
- {
- for (int i=0;i<numPDA;i++)
- {
- btVector3 norm;
- convexA->getPreferredPenetrationDirection(i,norm);
- norm = transA.getBasis() * norm;
- getPenetrationDirections()[numSampleDirections] = norm;
- seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
- seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
- numSampleDirections++;
- }
- }
- }
-
- {
- int numPDB = convexB->getNumPreferredPenetrationDirections();
- if (numPDB)
- {
- for (int i=0;i<numPDB;i++)
- {
- btVector3 norm;
- convexB->getPreferredPenetrationDirection(i,norm);
- norm = transB.getBasis() * norm;
- getPenetrationDirections()[numSampleDirections] = norm;
- seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
- seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
- numSampleDirections++;
- }
- }
- }
-
-
-
-
- convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
- convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
-
- for (i=0;i<numSampleDirections;i++)
- {
- btVector3 norm = getPenetrationDirections()[i];
- if (check2d)
- {
- norm[2] = 0.f;
- }
- if (norm.length2()>0.01)
- {
-
- seperatingAxisInA = seperatingAxisInABatch[i];
- seperatingAxisInB = seperatingAxisInBBatch[i];
-
- pInA = supportVerticesABatch[i];
- qInB = supportVerticesBBatch[i];
-
- pWorld = transA(pInA);
- qWorld = transB(qInB);
- if (check2d)
- {
- pWorld[2] = 0.f;
- qWorld[2] = 0.f;
- }
-
- w = qWorld - pWorld;
- btScalar delta = norm.dot(w);
- //find smallest delta
- if (delta < minProj)
- {
- minProj = delta;
- minNorm = norm;
- minA = pWorld;
- minB = qWorld;
- }
- }
- }
-#else
-
- int numSampleDirections = NUM_UNITSPHERE_POINTS;
-
-#ifndef __SPU__
- {
- int numPDA = convexA->getNumPreferredPenetrationDirections();
- if (numPDA)
- {
- for (int i=0;i<numPDA;i++)
- {
- btVector3 norm;
- convexA->getPreferredPenetrationDirection(i,norm);
- norm = transA.getBasis() * norm;
- getPenetrationDirections()[numSampleDirections] = norm;
- numSampleDirections++;
- }
- }
- }
-
- {
- int numPDB = convexB->getNumPreferredPenetrationDirections();
- if (numPDB)
- {
- for (int i=0;i<numPDB;i++)
- {
- btVector3 norm;
- convexB->getPreferredPenetrationDirection(i,norm);
- norm = transB.getBasis() * norm;
- getPenetrationDirections()[numSampleDirections] = norm;
- numSampleDirections++;
- }
- }
- }
-#endif // __SPU__
-
- for (int i=0;i<numSampleDirections;i++)
- {
- const btVector3& norm = getPenetrationDirections()[i];
- seperatingAxisInA = (-norm)* transA.getBasis();
- seperatingAxisInB = norm* transB.getBasis();
- pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
- qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
- pWorld = transA(pInA);
- qWorld = transB(qInB);
- w = qWorld - pWorld;
- btScalar delta = norm.dot(w);
- //find smallest delta
- if (delta < minProj)
- {
- minProj = delta;
- minNorm = norm;
- minA = pWorld;
- minB = qWorld;
- }
- }
-#endif //USE_BATCHED_SUPPORT
-
- //add the margins
-
- minA += minNorm*convexA->getMarginNonVirtual();
- minB -= minNorm*convexB->getMarginNonVirtual();
- //no penetration
- if (minProj < btScalar(0.))
- return false;
-
- btScalar extraSeparation = 0.5f;///scale dependent
- minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
-
-
-
-
-
-//#define DEBUG_DRAW 1
-#ifdef DEBUG_DRAW
- if (debugDraw)
- {
- btVector3 color(0,1,0);
- debugDraw->drawLine(minA,minB,color);
- color = btVector3 (1,1,1);
- btVector3 vec = minB-minA;
- btScalar prj2 = minNorm.dot(vec);
- debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
-
- }
-#endif //DEBUG_DRAW
-
-
-
- btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
-
- btScalar offsetDist = minProj;
- btVector3 offset = minNorm * offsetDist;
-
-
-
- btGjkPairDetector::ClosestPointInput input;
-
- btVector3 newOrg = transA.getOrigin() + offset;
-
- btTransform displacedTrans = transA;
- displacedTrans.setOrigin(newOrg);
-
- input.m_transformA = displacedTrans;
- input.m_transformB = transB;
- input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
-
- btIntermediateResult res;
- gjkdet.setCachedSeperatingAxis(-minNorm);
- gjkdet.getClosestPoints(input,res,debugDraw);
-
- btScalar correctedMinNorm = minProj - res.m_depth;
-
-
- //the penetration depth is over-estimated, relax it
- btScalar penetration_relaxation= btScalar(1.);
- minNorm*=penetration_relaxation;
-
-
- if (res.m_hasResult)
- {
-
- pa = res.m_pointInWorld - minNorm * correctedMinNorm;
- pb = res.m_pointInWorld;
- v = minNorm;
-
-#ifdef DEBUG_DRAW
- if (debugDraw)
- {
- btVector3 color(1,0,0);
- debugDraw->drawLine(pa,pb,color);
- }
-#endif//DEBUG_DRAW
-
-
- }
- return res.m_hasResult;
-}
-
-btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
-{
- static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
- {
- btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
- btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
- btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
- btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
- btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
- btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
- btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
- btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
- btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
- btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
- btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
- btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
- btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
- btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
- btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
- btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
- btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
- btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
- btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
- btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
- btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
- btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
- btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
- btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
- btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
- btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
- btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
- btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
- btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
- btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
- btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
- btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
- btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
- btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
- btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
- btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
- btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
- btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
- btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
- btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
- btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
- btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
- };
-
- return sPenetrationDirections;
-}
-
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
deleted file mode 100644
index fd533b4fc3..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
-#define BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
-
-#include "btConvexPenetrationDepthSolver.h"
-
-///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
-///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
-class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
-{
-protected:
-
- static btVector3* getPenetrationDirections();
-
-public:
-
- virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
- const btConvexShape* convexA,const btConvexShape* convexB,
- const btTransform& transA,const btTransform& transB,
- btVector3& v, btVector3& pa, btVector3& pb,
- class btIDebugDraw* debugDraw
- );
-};
-
-#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h
deleted file mode 100644
index a22a0bae66..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h
+++ /dev/null
@@ -1,908 +0,0 @@
-
-/***
- * ---------------------------------
- * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
- *
- * This file was ported from mpr.c file, part of libccd.
- * The Minkoski Portal Refinement implementation was ported
- * to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
- * The original MPR idea and implementation is by Gary Snethen
- * in XenoCollide, see http://github.com/erwincoumans/xenocollide
- *
- * Distributed under the OSI-approved BSD License (the "License");
- * see <http://www.opensource.org/licenses/bsd-license.php>.
- * This software is distributed WITHOUT ANY WARRANTY; without even the
- * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the License for more information.
- */
-
-///2014 Oct, Erwin Coumans, Use templates to avoid void* casts
-
-#ifndef BT_MPR_PENETRATION_H
-#define BT_MPR_PENETRATION_H
-
-#define BT_DEBUG_MPR1
-
-#include "LinearMath/btTransform.h"
-#include "LinearMath/btAlignedObjectArray.h"
-
-//#define MPR_AVERAGE_CONTACT_POSITIONS
-
-
-struct btMprCollisionDescription
-{
- btVector3 m_firstDir;
- int m_maxGjkIterations;
- btScalar m_maximumDistanceSquared;
- btScalar m_gjkRelError2;
-
- btMprCollisionDescription()
- : m_firstDir(0,1,0),
- m_maxGjkIterations(1000),
- m_maximumDistanceSquared(1e30f),
- m_gjkRelError2(1.0e-6)
- {
- }
- virtual ~btMprCollisionDescription()
- {
- }
-};
-
-struct btMprDistanceInfo
-{
- btVector3 m_pointOnA;
- btVector3 m_pointOnB;
- btVector3 m_normalBtoA;
- btScalar m_distance;
-};
-
-#ifdef __cplusplus
-#define BT_MPR_SQRT sqrtf
-#else
-#define BT_MPR_SQRT sqrt
-#endif
-#define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
-#define BT_MPR_FABS fabs
-
-#define BT_MPR_TOLERANCE 1E-6f
-#define BT_MPR_MAX_ITERATIONS 1000
-
-struct _btMprSupport_t
-{
- btVector3 v; //!< Support point in minkowski sum
- btVector3 v1; //!< Support point in obj1
- btVector3 v2; //!< Support point in obj2
-};
-typedef struct _btMprSupport_t btMprSupport_t;
-
-struct _btMprSimplex_t
-{
- btMprSupport_t ps[4];
- int last; //!< index of last added point
-};
-typedef struct _btMprSimplex_t btMprSimplex_t;
-
-inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx)
-{
- return &s->ps[idx];
-}
-
-inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
-{
- s->last = size - 1;
-}
-
-#ifdef DEBUG_MPR
-inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index)
-{
- printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(),portal->ps[index].v.y(),portal->ps[index].v.z(),
- portal->ps[index].v1.x(),portal->ps[index].v1.y(),portal->ps[index].v1.z(),
- portal->ps[index].v2.x(),portal->ps[index].v2.y(),portal->ps[index].v2.z());
-}
-#endif //DEBUG_MPR
-
-
-
-
-inline int btMprSimplexSize(const btMprSimplex_t *s)
-{
- return s->last + 1;
-}
-
-
-inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx)
-{
- // here is no check on boundaries
- return &s->ps[idx];
-}
-
-inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
-{
- *d = *s;
-}
-
-inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
-{
- btMprSupportCopy(s->ps + pos, a);
-}
-
-
-inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
-{
- btMprSupport_t supp;
-
- btMprSupportCopy(&supp, &s->ps[pos1]);
- btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
- btMprSupportCopy(&s->ps[pos2], &supp);
-}
-
-
-inline int btMprIsZero(float val)
-{
- return BT_MPR_FABS(val) < FLT_EPSILON;
-}
-
-
-
-inline int btMprEq(float _a, float _b)
-{
- float ab;
- float a, b;
-
- ab = BT_MPR_FABS(_a - _b);
- if (BT_MPR_FABS(ab) < FLT_EPSILON)
- return 1;
-
- a = BT_MPR_FABS(_a);
- b = BT_MPR_FABS(_b);
- if (b > a){
- return ab < FLT_EPSILON * b;
- }else{
- return ab < FLT_EPSILON * a;
- }
-}
-
-
-inline int btMprVec3Eq(const btVector3* a, const btVector3 *b)
-{
- return btMprEq((*a).x(), (*b).x())
- && btMprEq((*a).y(), (*b).y())
- && btMprEq((*a).z(), (*b).z());
-}
-
-
-
-
-
-
-
-
-
-
-
-template <typename btConvexTemplate>
-inline void btFindOrigin(const btConvexTemplate& a, const btConvexTemplate& b, const btMprCollisionDescription& colDesc,btMprSupport_t *center)
-{
-
- center->v1 = a.getObjectCenterInWorld();
- center->v2 = b.getObjectCenterInWorld();
- center->v = center->v1 - center->v2;
-}
-
-inline void btMprVec3Set(btVector3 *v, float x, float y, float z)
-{
- v->setValue(x,y,z);
-}
-
-inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
-{
- *v += *w;
-}
-
-inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
-{
- *v = *w;
-}
-
-inline void btMprVec3Scale(btVector3 *d, float k)
-{
- *d *= k;
-}
-
-inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
-{
- float dot;
-
- dot = btDot(*a,*b);
- return dot;
-}
-
-
-inline float btMprVec3Len2(const btVector3 *v)
-{
- return btMprVec3Dot(v, v);
-}
-
-inline void btMprVec3Normalize(btVector3 *d)
-{
- float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
- btMprVec3Scale(d, k);
-}
-
-inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
-{
- *d = btCross(*a,*b);
-
-}
-
-
-inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
-{
- *d = *v - *w;
-}
-
-inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
-{
- btVector3 v2v1, v3v1;
-
- btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v,
- &btMprSimplexPoint(portal, 1)->v);
- btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v,
- &btMprSimplexPoint(portal, 1)->v);
- btMprVec3Cross(dir, &v2v1, &v3v1);
- btMprVec3Normalize(dir);
-}
-
-
-inline int portalEncapsulesOrigin(const btMprSimplex_t *portal,
- const btVector3 *dir)
-{
- float dot;
- dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v);
- return btMprIsZero(dot) || dot > 0.f;
-}
-
-inline int portalReachTolerance(const btMprSimplex_t *portal,
- const btMprSupport_t *v4,
- const btVector3 *dir)
-{
- float dv1, dv2, dv3, dv4;
- float dot1, dot2, dot3;
-
- // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
-
- dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir);
- dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir);
- dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir);
- dv4 = btMprVec3Dot(&v4->v, dir);
-
- dot1 = dv4 - dv1;
- dot2 = dv4 - dv2;
- dot3 = dv4 - dv3;
-
- dot1 = BT_MPR_FMIN(dot1, dot2);
- dot1 = BT_MPR_FMIN(dot1, dot3);
-
- return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
-}
-
-inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
- const btMprSupport_t *v4,
- const btVector3 *dir)
-{
- float dot;
- dot = btMprVec3Dot(&v4->v, dir);
- return btMprIsZero(dot) || dot > 0.f;
-}
-
-inline void btExpandPortal(btMprSimplex_t *portal,
- const btMprSupport_t *v4)
-{
- float dot;
- btVector3 v4v0;
-
- btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v);
- dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0);
- if (dot > 0.f){
- dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0);
- if (dot > 0.f){
- btMprSimplexSet(portal, 1, v4);
- }else{
- btMprSimplexSet(portal, 3, v4);
- }
- }else{
- dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0);
- if (dot > 0.f){
- btMprSimplexSet(portal, 2, v4);
- }else{
- btMprSimplexSet(portal, 1, v4);
- }
- }
-}
-template <typename btConvexTemplate>
-inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b,
- const btMprCollisionDescription& colDesc,
- const btVector3& dir, btMprSupport_t *supp)
-{
- btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis();
- btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis();
-
- btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA);
- btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB);
-
- supp->v1 = a.getWorldTransform()(pInA);
- supp->v2 = b.getWorldTransform()(qInB);
- supp->v = supp->v1 - supp->v2;
-}
-
-
-template <typename btConvexTemplate>
-static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b,
- const btMprCollisionDescription& colDesc,
- btMprSimplex_t *portal)
-{
- btVector3 dir, va, vb;
- float dot;
- int cont;
-
-
-
- // vertex 0 is center of portal
- btFindOrigin(a,b,colDesc, btMprSimplexPointW(portal, 0));
-
-
- // vertex 0 is center of portal
- btMprSimplexSetSize(portal, 1);
-
-
-
- btVector3 zero = btVector3(0,0,0);
- btVector3* org = &zero;
-
- if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)){
- // Portal's center lies on origin (0,0,0) => we know that objects
- // intersect but we would need to know penetration info.
- // So move center little bit...
- btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
- btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va);
- }
-
-
- // vertex 1 = support in direction of origin
- btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v);
- btMprVec3Scale(&dir, -1.f);
- btMprVec3Normalize(&dir);
-
-
- btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 1));
-
- btMprSimplexSetSize(portal, 2);
-
- // test if origin isn't outside of v1
- dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir);
-
-
- if (btMprIsZero(dot) || dot < 0.f)
- return -1;
-
-
- // vertex 2
- btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v,
- &btMprSimplexPoint(portal, 1)->v);
- if (btMprIsZero(btMprVec3Len2(&dir))){
- if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)){
- // origin lies on v1
- return 1;
- }else{
- // origin lies on v0-v1 segment
- return 2;
- }
- }
-
- btMprVec3Normalize(&dir);
- btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 2));
-
-
-
- dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir);
- if (btMprIsZero(dot) || dot < 0.f)
- return -1;
-
- btMprSimplexSetSize(portal, 3);
-
- // vertex 3 direction
- btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
- &btMprSimplexPoint(portal, 0)->v);
- btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
- &btMprSimplexPoint(portal, 0)->v);
- btMprVec3Cross(&dir, &va, &vb);
- btMprVec3Normalize(&dir);
-
- // it is better to form portal faces to be oriented "outside" origin
- dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v);
- if (dot > 0.f){
- btMprSimplexSwap(portal, 1, 2);
- btMprVec3Scale(&dir, -1.f);
- }
-
- while (btMprSimplexSize(portal) < 4){
- btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 3));
-
- dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir);
- if (btMprIsZero(dot) || dot < 0.f)
- return -1;
-
- cont = 0;
-
- // test if origin is outside (v1, v0, v3) - set v2 as v3 and
- // continue
- btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v,
- &btMprSimplexPoint(portal, 3)->v);
- dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
- if (dot < 0.f && !btMprIsZero(dot)){
- btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3));
- cont = 1;
- }
-
- if (!cont){
- // test if origin is outside (v3, v0, v2) - set v1 as v3 and
- // continue
- btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v,
- &btMprSimplexPoint(portal, 2)->v);
- dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
- if (dot < 0.f && !btMprIsZero(dot)){
- btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3));
- cont = 1;
- }
- }
-
- if (cont){
- btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
- &btMprSimplexPoint(portal, 0)->v);
- btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
- &btMprSimplexPoint(portal, 0)->v);
- btMprVec3Cross(&dir, &va, &vb);
- btMprVec3Normalize(&dir);
- }else{
- btMprSimplexSetSize(portal, 4);
- }
- }
-
- return 0;
-}
-
-template <typename btConvexTemplate>
-static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc,
- btMprSimplex_t *portal)
-{
- btVector3 dir;
- btMprSupport_t v4;
-
- for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
- //while (1)
- {
- // compute direction outside the portal (from v0 throught v1,v2,v3
- // face)
- btPortalDir(portal, &dir);
-
- // test if origin is inside the portal
- if (portalEncapsulesOrigin(portal, &dir))
- return 0;
-
- // get next support point
-
- btMprSupport(a,b,colDesc, dir, &v4);
-
-
- // test if v4 can expand portal to contain origin and if portal
- // expanding doesn't reach given tolerance
- if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
- || portalReachTolerance(portal, &v4, &dir))
- {
- return -1;
- }
-
- // v1-v2-v3 triangle must be rearranged to face outside Minkowski
- // difference (direction from v0).
- btExpandPortal(portal, &v4);
- }
-
- return -1;
-}
-
-static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
-{
-
- btVector3 zero = btVector3(0,0,0);
- btVector3* origin = &zero;
-
- btVector3 dir;
- size_t i;
- float b[4], sum, inv;
- btVector3 vec, p1, p2;
-
- btPortalDir(portal, &dir);
-
- // use barycentric coordinates of tetrahedron to find origin
- btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
- &btMprSimplexPoint(portal, 2)->v);
- b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
-
- btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
- &btMprSimplexPoint(portal, 2)->v);
- b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
-
- btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v,
- &btMprSimplexPoint(portal, 1)->v);
- b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
-
- btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
- &btMprSimplexPoint(portal, 1)->v);
- b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
-
- sum = b[0] + b[1] + b[2] + b[3];
-
- if (btMprIsZero(sum) || sum < 0.f){
- b[0] = 0.f;
-
- btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
- &btMprSimplexPoint(portal, 3)->v);
- b[1] = btMprVec3Dot(&vec, &dir);
- btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
- &btMprSimplexPoint(portal, 1)->v);
- b[2] = btMprVec3Dot(&vec, &dir);
- btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
- &btMprSimplexPoint(portal, 2)->v);
- b[3] = btMprVec3Dot(&vec, &dir);
-
- sum = b[1] + b[2] + b[3];
- }
-
- inv = 1.f / sum;
-
- btMprVec3Copy(&p1, origin);
- btMprVec3Copy(&p2, origin);
- for (i = 0; i < 4; i++){
- btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1);
- btMprVec3Scale(&vec, b[i]);
- btMprVec3Add(&p1, &vec);
-
- btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2);
- btMprVec3Scale(&vec, b[i]);
- btMprVec3Add(&p2, &vec);
- }
- btMprVec3Scale(&p1, inv);
- btMprVec3Scale(&p2, inv);
-#ifdef MPR_AVERAGE_CONTACT_POSITIONS
- btMprVec3Copy(pos, &p1);
- btMprVec3Add(pos, &p2);
- btMprVec3Scale(pos, 0.5);
-#else
- btMprVec3Copy(pos, &p2);
-#endif//MPR_AVERAGE_CONTACT_POSITIONS
-}
-
-inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
-{
- btVector3 ab;
- btMprVec3Sub2(&ab, a, b);
- return btMprVec3Len2(&ab);
-}
-
-inline float _btMprVec3PointSegmentDist2(const btVector3 *P,
- const btVector3 *x0,
- const btVector3 *b,
- btVector3 *witness)
-{
- // The computation comes from solving equation of segment:
- // S(t) = x0 + t.d
- // where - x0 is initial point of segment
- // - d is direction of segment from x0 (|d| > 0)
- // - t belongs to <0, 1> interval
- //
- // Than, distance from a segment to some point P can be expressed:
- // D(t) = |x0 + t.d - P|^2
- // which is distance from any point on segment. Minimization
- // of this function brings distance from P to segment.
- // Minimization of D(t) leads to simple quadratic equation that's
- // solving is straightforward.
- //
- // Bonus of this method is witness point for free.
-
- float dist, t;
- btVector3 d, a;
-
- // direction of segment
- btMprVec3Sub2(&d, b, x0);
-
- // precompute vector from P to x0
- btMprVec3Sub2(&a, x0, P);
-
- t = -1.f * btMprVec3Dot(&a, &d);
- t /= btMprVec3Len2(&d);
-
- if (t < 0.f || btMprIsZero(t)){
- dist = btMprVec3Dist2(x0, P);
- if (witness)
- btMprVec3Copy(witness, x0);
- }else if (t > 1.f || btMprEq(t, 1.f)){
- dist = btMprVec3Dist2(b, P);
- if (witness)
- btMprVec3Copy(witness, b);
- }else{
- if (witness){
- btMprVec3Copy(witness, &d);
- btMprVec3Scale(witness, t);
- btMprVec3Add(witness, x0);
- dist = btMprVec3Dist2(witness, P);
- }else{
- // recycling variables
- btMprVec3Scale(&d, t);
- btMprVec3Add(&d, &a);
- dist = btMprVec3Len2(&d);
- }
- }
-
- return dist;
-}
-
-
-
-inline float btMprVec3PointTriDist2(const btVector3 *P,
- const btVector3 *x0, const btVector3 *B,
- const btVector3 *C,
- btVector3 *witness)
-{
- // Computation comes from analytic expression for triangle (x0, B, C)
- // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
- // Then equation for distance is:
- // D(s, t) = | T(s, t) - P |^2
- // This leads to minimization of quadratic function of two variables.
- // The solution from is taken only if s is between 0 and 1, t is
- // between 0 and 1 and t + s < 1, otherwise distance from segment is
- // computed.
-
- btVector3 d1, d2, a;
- float u, v, w, p, q, r;
- float s, t, dist, dist2;
- btVector3 witness2;
-
- btMprVec3Sub2(&d1, B, x0);
- btMprVec3Sub2(&d2, C, x0);
- btMprVec3Sub2(&a, x0, P);
-
- u = btMprVec3Dot(&a, &a);
- v = btMprVec3Dot(&d1, &d1);
- w = btMprVec3Dot(&d2, &d2);
- p = btMprVec3Dot(&a, &d1);
- q = btMprVec3Dot(&a, &d2);
- r = btMprVec3Dot(&d1, &d2);
-
- btScalar div = (w * v - r * r);
- if (btMprIsZero(div))
- {
- s=-1;
- } else
- {
- s = (q * r - w * p) / div;
- t = (-s * r - q) / w;
- }
-
- if ((btMprIsZero(s) || s > 0.f)
- && (btMprEq(s, 1.f) || s < 1.f)
- && (btMprIsZero(t) || t > 0.f)
- && (btMprEq(t, 1.f) || t < 1.f)
- && (btMprEq(t + s, 1.f) || t + s < 1.f)){
-
- if (witness){
- btMprVec3Scale(&d1, s);
- btMprVec3Scale(&d2, t);
- btMprVec3Copy(witness, x0);
- btMprVec3Add(witness, &d1);
- btMprVec3Add(witness, &d2);
-
- dist = btMprVec3Dist2(witness, P);
- }else{
- dist = s * s * v;
- dist += t * t * w;
- dist += 2.f * s * t * r;
- dist += 2.f * s * p;
- dist += 2.f * t * q;
- dist += u;
- }
- }else{
- dist = _btMprVec3PointSegmentDist2(P, x0, B, witness);
-
- dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2);
- if (dist2 < dist){
- dist = dist2;
- if (witness)
- btMprVec3Copy(witness, &witness2);
- }
-
- dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2);
- if (dist2 < dist){
- dist = dist2;
- if (witness)
- btMprVec3Copy(witness, &witness2);
- }
- }
-
- return dist;
-}
-
-template <typename btConvexTemplate>
-static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b,
- const btMprCollisionDescription& colDesc,
- btMprSimplex_t *portal,
- float *depth, btVector3 *pdir, btVector3 *pos)
-{
- btVector3 dir;
- btMprSupport_t v4;
- unsigned long iterations;
-
- btVector3 zero = btVector3(0,0,0);
- btVector3* origin = &zero;
-
-
- iterations = 1UL;
- for (int i=0;i<BT_MPR_MAX_ITERATIONS;i++)
- //while (1)
- {
- // compute portal direction and obtain next support point
- btPortalDir(portal, &dir);
-
- btMprSupport(a,b,colDesc, dir, &v4);
-
-
- // reached tolerance -> find penetration info
- if (portalReachTolerance(portal, &v4, &dir)
- || iterations ==BT_MPR_MAX_ITERATIONS)
- {
- *depth = btMprVec3PointTriDist2(origin,&btMprSimplexPoint(portal, 1)->v,&btMprSimplexPoint(portal, 2)->v,&btMprSimplexPoint(portal, 3)->v,pdir);
- *depth = BT_MPR_SQRT(*depth);
-
- if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z()))
- {
-
- *pdir = dir;
- }
- btMprVec3Normalize(pdir);
-
- // barycentric coordinates:
- btFindPos(portal, pos);
-
-
- return;
- }
-
- btExpandPortal(portal, &v4);
-
- iterations++;
- }
-}
-
-static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos)
-{
- // Touching contact on portal's v1 - so depth is zero and direction
- // is unimportant and pos can be guessed
- *depth = 0.f;
- btVector3 zero = btVector3(0,0,0);
- btVector3* origin = &zero;
-
-
- btMprVec3Copy(dir, origin);
-#ifdef MPR_AVERAGE_CONTACT_POSITIONS
- btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
- btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
- btMprVec3Scale(pos, 0.5);
-#else
- btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
-#endif
-}
-
-static void btFindPenetrSegment(btMprSimplex_t *portal,
- float *depth, btVector3 *dir, btVector3 *pos)
-{
-
- // Origin lies on v0-v1 segment.
- // Depth is distance to v1, direction also and position must be
- // computed
-#ifdef MPR_AVERAGE_CONTACT_POSITIONS
- btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
- btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
- btMprVec3Scale(pos, 0.5f);
-#else
- btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
-#endif//MPR_AVERAGE_CONTACT_POSITIONS
-
- btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v);
- *depth = BT_MPR_SQRT(btMprVec3Len2(dir));
- btMprVec3Normalize(dir);
-
-
-}
-
-
-template <typename btConvexTemplate>
-inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b,
- const btMprCollisionDescription& colDesc,
- float *depthOut, btVector3* dirOut, btVector3* posOut)
-{
-
- btMprSimplex_t portal;
-
-
- // Phase 1: Portal discovery
- int result = btDiscoverPortal(a,b,colDesc, &portal);
-
-
- //sepAxis[pairIndex] = *pdir;//or -dir?
-
- switch (result)
- {
- case 0:
- {
- // Phase 2: Portal refinement
-
- result = btRefinePortal(a,b,colDesc, &portal);
- if (result < 0)
- return -1;
-
- // Phase 3. Penetration info
- btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut);
-
-
- break;
- }
- case 1:
- {
- // Touching contact on portal's v1.
- btFindPenetrTouch(&portal, depthOut, dirOut, posOut);
- result=0;
- break;
- }
- case 2:
- {
-
- btFindPenetrSegment( &portal, depthOut, dirOut, posOut);
- result=0;
- break;
- }
- default:
- {
- //if (res < 0)
- //{
- // Origin isn't inside portal - no collision.
- result = -1;
- //}
- }
- };
-
- return result;
-};
-
-
-template<typename btConvexTemplate, typename btMprDistanceTemplate>
-inline int btComputeMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, const
- btMprCollisionDescription& colDesc, btMprDistanceTemplate* distInfo)
-{
- btVector3 dir,pos;
- float depth;
-
- int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos);
- if (res==0)
- {
- distInfo->m_distance = -depth;
- distInfo->m_pointOnB = pos;
- distInfo->m_normalBtoA = -dir;
- distInfo->m_pointOnA = pos-distInfo->m_distance*dir;
- return 0;
- }
-
- return -1;
-}
-
-
-
-#endif //BT_MPR_PENETRATION_H
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
deleted file mode 100644
index 23aaece22b..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
+++ /dev/null
@@ -1,308 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#include "btPersistentManifold.h"
-#include "LinearMath/btTransform.h"
-
-
-btScalar gContactBreakingThreshold = btScalar(0.02);
-ContactDestroyedCallback gContactDestroyedCallback = 0;
-ContactProcessedCallback gContactProcessedCallback = 0;
-ContactStartedCallback gContactStartedCallback = 0;
-ContactEndedCallback gContactEndedCallback = 0;
-///gContactCalcArea3Points will approximate the convex hull area using 3 points
-///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower
-bool gContactCalcArea3Points = true;
-
-
-btPersistentManifold::btPersistentManifold()
-:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
-m_body0(0),
-m_body1(0),
-m_cachedPoints (0),
-m_index1a(0)
-{
-}
-
-
-
-
-#ifdef DEBUG_PERSISTENCY
-#include <stdio.h>
-void btPersistentManifold::DebugPersistency()
-{
- int i;
- printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
- for (i=0;i<m_cachedPoints;i++)
- {
- printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
- }
-}
-#endif //DEBUG_PERSISTENCY
-
-void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
-{
-
- void* oldPtr = pt.m_userPersistentData;
- if (oldPtr)
- {
-#ifdef DEBUG_PERSISTENCY
- int i;
- int occurance = 0;
- for (i=0;i<m_cachedPoints;i++)
- {
- if (m_pointCache[i].m_userPersistentData == oldPtr)
- {
- occurance++;
- if (occurance>1)
- printf("error in clearUserCache\n");
- }
- }
- btAssert(occurance<=0);
-#endif //DEBUG_PERSISTENCY
-
- if (pt.m_userPersistentData && gContactDestroyedCallback)
- {
- (*gContactDestroyedCallback)(pt.m_userPersistentData);
- pt.m_userPersistentData = 0;
- }
-
-#ifdef DEBUG_PERSISTENCY
- DebugPersistency();
-#endif
- }
-
-
-}
-
-static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3)
-{
- // It calculates possible 3 area constructed from random 4 points and returns the biggest one.
-
- btVector3 a[3],b[3];
- a[0] = p0 - p1;
- a[1] = p0 - p2;
- a[2] = p0 - p3;
- b[0] = p2 - p3;
- b[1] = p1 - p3;
- b[2] = p1 - p2;
-
- //todo: Following 3 cross production can be easily optimized by SIMD.
- btVector3 tmp0 = a[0].cross(b[0]);
- btVector3 tmp1 = a[1].cross(b[1]);
- btVector3 tmp2 = a[2].cross(b[2]);
-
- return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2());
-}
-
-int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
-{
- //calculate 4 possible cases areas, and take biggest area
- //also need to keep 'deepest'
-
- int maxPenetrationIndex = -1;
-#define KEEP_DEEPEST_POINT 1
-#ifdef KEEP_DEEPEST_POINT
- btScalar maxPenetration = pt.getDistance();
- for (int i=0;i<4;i++)
- {
- if (m_pointCache[i].getDistance() < maxPenetration)
- {
- maxPenetrationIndex = i;
- maxPenetration = m_pointCache[i].getDistance();
- }
- }
-#endif //KEEP_DEEPEST_POINT
-
- btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
-
- if (gContactCalcArea3Points)
- {
- if (maxPenetrationIndex != 0)
- {
- btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
- btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
- btVector3 cross = a0.cross(b0);
- res0 = cross.length2();
- }
- if (maxPenetrationIndex != 1)
- {
- btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
- btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
- btVector3 cross = a1.cross(b1);
- res1 = cross.length2();
- }
-
- if (maxPenetrationIndex != 2)
- {
- btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
- btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
- btVector3 cross = a2.cross(b2);
- res2 = cross.length2();
- }
-
- if (maxPenetrationIndex != 3)
- {
- btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
- btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
- btVector3 cross = a3.cross(b3);
- res3 = cross.length2();
- }
- }
- else
- {
- if(maxPenetrationIndex != 0) {
- res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
- }
-
- if(maxPenetrationIndex != 1) {
- res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
- }
-
- if(maxPenetrationIndex != 2) {
- res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA);
- }
-
- if(maxPenetrationIndex != 3) {
- res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA);
- }
- }
- btVector4 maxvec(res0,res1,res2,res3);
- int biggestarea = maxvec.closestAxis4();
- return biggestarea;
-
-}
-
-
-int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
-{
- btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
- int size = getNumContacts();
- int nearestPoint = -1;
- for( int i = 0; i < size; i++ )
- {
- const btManifoldPoint &mp = m_pointCache[i];
-
- btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
- const btScalar distToManiPoint = diffA.dot(diffA);
- if( distToManiPoint < shortestDist )
- {
- shortestDist = distToManiPoint;
- nearestPoint = i;
- }
- }
- return nearestPoint;
-}
-
-int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive)
-{
- if (!isPredictive)
- {
- btAssert(validContactDistance(newPoint));
- }
-
- int insertIndex = getNumContacts();
- if (insertIndex == MANIFOLD_CACHE_SIZE)
- {
-#if MANIFOLD_CACHE_SIZE >= 4
- //sort cache so best points come first, based on area
- insertIndex = sortCachedPoints(newPoint);
-#else
- insertIndex = 0;
-#endif
- clearUserCache(m_pointCache[insertIndex]);
-
- } else
- {
- m_cachedPoints++;
-
-
- }
- if (insertIndex<0)
- insertIndex=0;
-
- btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
- m_pointCache[insertIndex] = newPoint;
- return insertIndex;
-}
-
-btScalar btPersistentManifold::getContactBreakingThreshold() const
-{
- return m_contactBreakingThreshold;
-}
-
-
-
-void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
-{
- int i;
-#ifdef DEBUG_PERSISTENCY
- printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
- trA.getOrigin().getX(),
- trA.getOrigin().getY(),
- trA.getOrigin().getZ(),
- trB.getOrigin().getX(),
- trB.getOrigin().getY(),
- trB.getOrigin().getZ());
-#endif //DEBUG_PERSISTENCY
- /// first refresh worldspace positions and distance
- for (i=getNumContacts()-1;i>=0;i--)
- {
- btManifoldPoint &manifoldPoint = m_pointCache[i];
- manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
- manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
- manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
- manifoldPoint.m_lifeTime++;
- }
-
- /// then
- btScalar distance2d;
- btVector3 projectedDifference,projectedPoint;
- for (i=getNumContacts()-1;i>=0;i--)
- {
-
- btManifoldPoint &manifoldPoint = m_pointCache[i];
- //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
- if (!validContactDistance(manifoldPoint))
- {
- removeContactPoint(i);
- } else
- {
- //todo: friction anchor may require the contact to be around a bit longer
- //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
- projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
- projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
- distance2d = projectedDifference.dot(projectedDifference);
- if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() )
- {
- removeContactPoint(i);
- } else
- {
- //contact point processed callback
- if (gContactProcessedCallback)
- (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
- }
- }
- }
-#ifdef DEBUG_PERSISTENCY
- DebugPersistency();
-#endif //
-}
-
-
-
-
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
deleted file mode 100644
index f872c8e1c9..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
+++ /dev/null
@@ -1,268 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_PERSISTENT_MANIFOLD_H
-#define BT_PERSISTENT_MANIFOLD_H
-
-
-#include "LinearMath/btVector3.h"
-#include "LinearMath/btTransform.h"
-#include "btManifoldPoint.h"
-class btCollisionObject;
-#include "LinearMath/btAlignedAllocator.h"
-
-struct btCollisionResult;
-
-///maximum contact breaking and merging threshold
-extern btScalar gContactBreakingThreshold;
-
-#ifndef SWIG
-class btPersistentManifold;
-
-typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
-typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1);
-typedef void (*ContactStartedCallback)(btPersistentManifold* const &manifold);
-typedef void (*ContactEndedCallback)(btPersistentManifold* const &manifold);
-extern ContactDestroyedCallback gContactDestroyedCallback;
-extern ContactProcessedCallback gContactProcessedCallback;
-extern ContactStartedCallback gContactStartedCallback;
-extern ContactEndedCallback gContactEndedCallback;
-#endif //SWIG
-
-//the enum starts at 1024 to avoid type conflicts with btTypedConstraint
-enum btContactManifoldTypes
-{
- MIN_CONTACT_MANIFOLD_TYPE = 1024,
- BT_PERSISTENT_MANIFOLD_TYPE
-};
-
-#define MANIFOLD_CACHE_SIZE 4
-
-///btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase.
-///Those contact points are created by the collision narrow phase.
-///The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time.
-///updates/refreshes old contact points, and throw them away if necessary (distance becomes too large)
-///reduces the cache to 4 points, when more then 4 points are added, using following rules:
-///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points
-///note that some pairs of objects might have more then one contact manifold.
-
-
-//ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject
-ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject
-{
-
- btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
-
- /// this two body pointers can point to the physics rigidbody class.
- const btCollisionObject* m_body0;
- const btCollisionObject* m_body1;
-
- int m_cachedPoints;
-
- btScalar m_contactBreakingThreshold;
- btScalar m_contactProcessingThreshold;
-
-
- /// sort cached points so most isolated points come first
- int sortCachedPoints(const btManifoldPoint& pt);
-
- int findContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt);
-
-public:
-
- BT_DECLARE_ALIGNED_ALLOCATOR();
-
- int m_companionIdA;
- int m_companionIdB;
-
- int m_index1a;
-
- btPersistentManifold();
-
- btPersistentManifold(const btCollisionObject* body0,const btCollisionObject* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold)
- : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
- m_body0(body0),m_body1(body1),m_cachedPoints(0),
- m_contactBreakingThreshold(contactBreakingThreshold),
- m_contactProcessingThreshold(contactProcessingThreshold)
- {
- }
-
- SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0;}
- SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1;}
-
- void setBodies(const btCollisionObject* body0,const btCollisionObject* body1)
- {
- m_body0 = body0;
- m_body1 = body1;
- }
-
- void clearUserCache(btManifoldPoint& pt);
-
-#ifdef DEBUG_PERSISTENCY
- void DebugPersistency();
-#endif //
-
- SIMD_FORCE_INLINE int getNumContacts() const { return m_cachedPoints;}
- /// the setNumContacts API is usually not used, except when you gather/fill all contacts manually
- void setNumContacts(int cachedPoints)
- {
- m_cachedPoints = cachedPoints;
- }
-
-
- SIMD_FORCE_INLINE const btManifoldPoint& getContactPoint(int index) const
- {
- btAssert(index < m_cachedPoints);
- return m_pointCache[index];
- }
-
- SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index)
- {
- btAssert(index < m_cachedPoints);
- return m_pointCache[index];
- }
-
- ///@todo: get this margin from the current physics / collision environment
- btScalar getContactBreakingThreshold() const;
-
- btScalar getContactProcessingThreshold() const
- {
- return m_contactProcessingThreshold;
- }
-
- void setContactBreakingThreshold(btScalar contactBreakingThreshold)
- {
- m_contactBreakingThreshold = contactBreakingThreshold;
- }
-
- void setContactProcessingThreshold(btScalar contactProcessingThreshold)
- {
- m_contactProcessingThreshold = contactProcessingThreshold;
- }
-
-
-
-
- int getCacheEntry(const btManifoldPoint& newPoint) const;
-
- int addManifoldPoint( const btManifoldPoint& newPoint, bool isPredictive=false);
-
- void removeContactPoint (int index)
- {
- clearUserCache(m_pointCache[index]);
-
- int lastUsedIndex = getNumContacts() - 1;
-// m_pointCache[index] = m_pointCache[lastUsedIndex];
- if(index != lastUsedIndex)
- {
- m_pointCache[index] = m_pointCache[lastUsedIndex];
- //get rid of duplicated userPersistentData pointer
- m_pointCache[lastUsedIndex].m_userPersistentData = 0;
- m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
- m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
- m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
- m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
- m_pointCache[lastUsedIndex].m_lifeTime = 0;
- }
-
- btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0);
- m_cachedPoints--;
-
- if (gContactEndedCallback && m_cachedPoints == 0)
- {
- gContactEndedCallback(this);
- }
- }
- void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex)
- {
- btAssert(validContactDistance(newPoint));
-
-#define MAINTAIN_PERSISTENCY 1
-#ifdef MAINTAIN_PERSISTENCY
- int lifeTime = m_pointCache[insertIndex].getLifeTime();
- btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
- btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
- btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
-
- bool replacePoint = true;
- ///we keep existing contact points for friction anchors
- ///if the friction force is within the Coulomb friction cone
- if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
- {
- // printf("appliedImpulse=%f\n", appliedImpulse);
- // printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1);
- // printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2);
- // printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction);
- btScalar mu = m_pointCache[insertIndex].m_combinedFriction;
- btScalar eps = 0; //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7
- btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2;
- btScalar b = eps + mu * appliedImpulse;
- b = b * b;
- replacePoint = (a) > (b);
- }
-
- if (replacePoint)
- {
- btAssert(lifeTime >= 0);
- void* cache = m_pointCache[insertIndex].m_userPersistentData;
-
- m_pointCache[insertIndex] = newPoint;
- m_pointCache[insertIndex].m_userPersistentData = cache;
- m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
- m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
- m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
- }
-
- m_pointCache[insertIndex].m_lifeTime = lifeTime;
-#else
- clearUserCache(m_pointCache[insertIndex]);
- m_pointCache[insertIndex] = newPoint;
-
-#endif
- }
-
- bool validContactDistance(const btManifoldPoint& pt) const
- {
- return pt.m_distance1 <= getContactBreakingThreshold();
- }
- /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
- void refreshContactPoints( const btTransform& trA,const btTransform& trB);
-
-
- SIMD_FORCE_INLINE void clearManifold()
- {
- int i;
- for (i=0;i<m_cachedPoints;i++)
- {
- clearUserCache(m_pointCache[i]);
- }
-
- if (gContactEndedCallback && m_cachedPoints)
- {
- gContactEndedCallback(this);
- }
- m_cachedPoints = 0;
- }
-
-
-
-}
-;
-
-
-
-
-
-#endif //BT_PERSISTENT_MANIFOLD_H
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h
deleted file mode 100644
index 18da171011..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_POINT_COLLECTOR_H
-#define BT_POINT_COLLECTOR_H
-
-#include "btDiscreteCollisionDetectorInterface.h"
-
-
-
-struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
-{
-
-
- btVector3 m_normalOnBInWorld;
- btVector3 m_pointInWorld;
- btScalar m_distance;//negative means penetration
-
- bool m_hasResult;
-
- btPointCollector ()
- : m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false)
- {
- }
-
- virtual void setShapeIdentifiersA(int partId0,int index0)
- {
- (void)partId0;
- (void)index0;
-
- }
- virtual void setShapeIdentifiersB(int partId1,int index1)
- {
- (void)partId1;
- (void)index1;
- }
-
- virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
- {
- if (depth< m_distance)
- {
- m_hasResult = true;
- m_normalOnBInWorld = normalOnBInWorld;
- m_pointInWorld = pointInWorld;
- //negative means penetration
- m_distance = depth;
- }
- }
-};
-
-#endif //BT_POINT_COLLECTOR_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
deleted file mode 100644
index ea380bc5f1..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
+++ /dev/null
@@ -1,570 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-///This file was written by Erwin Coumans
-///Separating axis rest based on work from Pierre Terdiman, see
-///And contact clipping based on work from Simon Hobbs
-
-
-#include "btPolyhedralContactClipping.h"
-#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h"
-
-#include <float.h> //for FLT_MAX
-
-int gExpectedNbTests=0;
-int gActualNbTests = 0;
-bool gUseInternalObject = true;
-
-// Clips a face to the back of a plane
-void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
-{
-
- int ve;
- btScalar ds, de;
- int numVerts = pVtxIn.size();
- if (numVerts < 2)
- return;
-
- btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
- btVector3 endVertex = pVtxIn[0];
-
- ds = planeNormalWS.dot(firstVertex)+planeEqWS;
-
- for (ve = 0; ve < numVerts; ve++)
- {
- endVertex=pVtxIn[ve];
-
- de = planeNormalWS.dot(endVertex)+planeEqWS;
-
- if (ds<0)
- {
- if (de<0)
- {
- // Start < 0, end < 0, so output endVertex
- ppVtxOut.push_back(endVertex);
- }
- else
- {
- // Start < 0, end >= 0, so output intersection
- ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
- }
- }
- else
- {
- if (de<0)
- {
- // Start >= 0, end < 0 so output intersection and end
- ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
- ppVtxOut.push_back(endVertex);
- }
- }
- firstVertex = endVertex;
- ds = de;
- }
-}
-
-
-static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB)
-{
- btScalar Min0,Max0;
- btScalar Min1,Max1;
- btVector3 witnesPtMinA,witnesPtMaxA;
- btVector3 witnesPtMinB,witnesPtMaxB;
-
- hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA);
- hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB);
-
- if(Max0<Min1 || Max1<Min0)
- return false;
-
- btScalar d0 = Max0 - Min1;
- btAssert(d0>=0.0f);
- btScalar d1 = Max1 - Min0;
- btAssert(d1>=0.0f);
- if (d0<d1)
- {
- depth = d0;
- witnessPointA = witnesPtMaxA;
- witnessPointB = witnesPtMinB;
-
- } else
- {
- depth = d1;
- witnessPointA = witnesPtMinA;
- witnessPointB = witnesPtMaxB;
- }
-
- return true;
-}
-
-
-
-static int gActualSATPairTests=0;
-
-inline bool IsAlmostZero(const btVector3& v)
-{
- if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6) return false;
- return true;
-}
-
-#ifdef TEST_INTERNAL_OBJECTS
-
-inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3])
-{
- // This version is ~11.000 cycles (4%) faster overall in one of the tests.
-// IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK);
-// IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK);
-// IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK);
- p[0] = sv[0] < 0.0f ? -extents[0] : extents[0];
- p[1] = sv[1] < 0.0f ? -extents[1] : extents[1];
- p[2] = sv[2] < 0.0f ? -extents[2] : extents[2];
-}
-
-void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
-{
- const btMatrix3x3& rot = tr.getBasis();
- const btVector3& r0 = rot[0];
- const btVector3& r1 = rot[1];
- const btVector3& r2 = rot[2];
-
- const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
- const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
- const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();
-
- out.setValue(x, y, z);
-}
-
- bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin)
-{
- const btScalar dp = delta_c.dot(axis);
-
- btVector3 localAxis0;
- InverseTransformPoint3x3(localAxis0, axis,trans0);
- btVector3 localAxis1;
- InverseTransformPoint3x3(localAxis1, axis,trans1);
-
- btScalar p0[3];
- BoxSupport(convex0.m_extents, localAxis0, p0);
- btScalar p1[3];
- BoxSupport(convex1.m_extents, localAxis1, p1);
-
- const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z();
- const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z();
-
- const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
- const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;
-
- const btScalar MinMaxRadius = MaxRadius + MinRadius;
- const btScalar d0 = MinMaxRadius + dp;
- const btScalar d1 = MinMaxRadius - dp;
-
- const btScalar depth = d0<d1 ? d0:d1;
- if(depth>dmin)
- return false;
- return true;
-}
-#endif //TEST_INTERNAL_OBJECTS
-
-
-
- SIMD_FORCE_INLINE void btSegmentsClosestPoints(
- btVector3& ptsVector,
- btVector3& offsetA,
- btVector3& offsetB,
- btScalar& tA, btScalar& tB,
- const btVector3& translation,
- const btVector3& dirA, btScalar hlenA,
- const btVector3& dirB, btScalar hlenB )
-{
- // compute the parameters of the closest points on each line segment
-
- btScalar dirA_dot_dirB = btDot(dirA,dirB);
- btScalar dirA_dot_trans = btDot(dirA,translation);
- btScalar dirB_dot_trans = btDot(dirB,translation);
-
- btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
-
- if ( denom == 0.0f ) {
- tA = 0.0f;
- } else {
- tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
- if ( tA < -hlenA )
- tA = -hlenA;
- else if ( tA > hlenA )
- tA = hlenA;
- }
-
- tB = tA * dirA_dot_dirB - dirB_dot_trans;
-
- if ( tB < -hlenB ) {
- tB = -hlenB;
- tA = tB * dirA_dot_dirB + dirA_dot_trans;
-
- if ( tA < -hlenA )
- tA = -hlenA;
- else if ( tA > hlenA )
- tA = hlenA;
- } else if ( tB > hlenB ) {
- tB = hlenB;
- tA = tB * dirA_dot_dirB + dirA_dot_trans;
-
- if ( tA < -hlenA )
- tA = -hlenA;
- else if ( tA > hlenA )
- tA = hlenA;
- }
-
- // compute the closest points relative to segment centers.
-
- offsetA = dirA * tA;
- offsetB = dirB * tB;
-
- ptsVector = translation - offsetA + offsetB;
-}
-
-
-
-bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
-{
- gActualSATPairTests++;
-
-//#ifdef TEST_INTERNAL_OBJECTS
- const btVector3 c0 = transA * hullA.m_localCenter;
- const btVector3 c1 = transB * hullB.m_localCenter;
- const btVector3 DeltaC2 = c0 - c1;
-//#endif
-
- btScalar dmin = FLT_MAX;
- int curPlaneTests=0;
-
- int numFacesA = hullA.m_faces.size();
- // Test normals from hullA
- for(int i=0;i<numFacesA;i++)
- {
- const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
- btVector3 faceANormalWS = transA.getBasis() * Normal;
- if (DeltaC2.dot(faceANormalWS)<0)
- faceANormalWS*=-1.f;
-
- curPlaneTests++;
-#ifdef TEST_INTERNAL_OBJECTS
- gExpectedNbTests++;
- if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
- continue;
- gActualNbTests++;
-#endif
-
- btScalar d;
- btVector3 wA,wB;
- if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d,wA,wB))
- return false;
-
- if(d<dmin)
- {
- dmin = d;
- sep = faceANormalWS;
- }
- }
-
- int numFacesB = hullB.m_faces.size();
- // Test normals from hullB
- for(int i=0;i<numFacesB;i++)
- {
- const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
- btVector3 WorldNormal = transB.getBasis() * Normal;
- if (DeltaC2.dot(WorldNormal)<0)
- WorldNormal *=-1.f;
-
- curPlaneTests++;
-#ifdef TEST_INTERNAL_OBJECTS
- gExpectedNbTests++;
- if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
- continue;
- gActualNbTests++;
-#endif
-
- btScalar d;
- btVector3 wA,wB;
- if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d,wA,wB))
- return false;
-
- if(d<dmin)
- {
- dmin = d;
- sep = WorldNormal;
- }
- }
-
- btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
- int edgeA=-1;
- int edgeB=-1;
- btVector3 worldEdgeA;
- btVector3 worldEdgeB;
- btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0);
-
-
- int curEdgeEdge = 0;
- // Test edges
- for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
- {
- const btVector3 edge0 = hullA.m_uniqueEdges[e0];
- const btVector3 WorldEdge0 = transA.getBasis() * edge0;
- for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
- {
- const btVector3 edge1 = hullB.m_uniqueEdges[e1];
- const btVector3 WorldEdge1 = transB.getBasis() * edge1;
-
- btVector3 Cross = WorldEdge0.cross(WorldEdge1);
- curEdgeEdge++;
- if(!IsAlmostZero(Cross))
- {
- Cross = Cross.normalize();
- if (DeltaC2.dot(Cross)<0)
- Cross *= -1.f;
-
-
-#ifdef TEST_INTERNAL_OBJECTS
- gExpectedNbTests++;
- if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
- continue;
- gActualNbTests++;
-#endif
-
- btScalar dist;
- btVector3 wA,wB;
- if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist,wA,wB))
- return false;
-
- if(dist<dmin)
- {
- dmin = dist;
- sep = Cross;
- edgeA=e0;
- edgeB=e1;
- worldEdgeA = WorldEdge0;
- worldEdgeB = WorldEdge1;
- witnessPointA=wA;
- witnessPointB=wB;
- }
- }
- }
-
- }
-
- if (edgeA>=0&&edgeB>=0)
- {
-// printf("edge-edge\n");
- //add an edge-edge contact
-
- btVector3 ptsVector;
- btVector3 offsetA;
- btVector3 offsetB;
- btScalar tA;
- btScalar tB;
-
- btVector3 translation = witnessPointB-witnessPointA;
-
- btVector3 dirA = worldEdgeA;
- btVector3 dirB = worldEdgeB;
-
- btScalar hlenB = 1e30f;
- btScalar hlenA = 1e30f;
-
- btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB,
- translation,
- dirA, hlenA,
- dirB,hlenB);
-
- btScalar nlSqrt = ptsVector.length2();
- if (nlSqrt>SIMD_EPSILON)
- {
- btScalar nl = btSqrt(nlSqrt);
- ptsVector *= 1.f/nl;
- if (ptsVector.dot(DeltaC2)<0.f)
- {
- ptsVector*=-1.f;
- }
- btVector3 ptOnB = witnessPointB + offsetB;
- btScalar distance = nl;
- resultOut.addContactPoint(ptsVector, ptOnB,-distance);
- }
-
- }
-
-
- if((DeltaC2.dot(sep))<0.0f)
- sep = -sep;
-
- return true;
-}
-
-void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
-{
- worldVertsB2.resize(0);
- btVertexArray* pVtxIn = &worldVertsB1;
- btVertexArray* pVtxOut = &worldVertsB2;
- pVtxOut->reserve(pVtxIn->size());
-
- int closestFaceA=-1;
- {
- btScalar dmin = FLT_MAX;
- for(int face=0;face<hullA.m_faces.size();face++)
- {
- const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
- const btVector3 faceANormalWS = transA.getBasis() * Normal;
-
- btScalar d = faceANormalWS.dot(separatingNormal);
- if (d < dmin)
- {
- dmin = d;
- closestFaceA = face;
- }
- }
- }
- if (closestFaceA<0)
- return;
-
- const btFace& polyA = hullA.m_faces[closestFaceA];
-
- // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
- int numVerticesA = polyA.m_indices.size();
- for(int e0=0;e0<numVerticesA;e0++)
- {
- const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
- const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
- const btVector3 edge0 = a - b;
- const btVector3 WorldEdge0 = transA.getBasis() * edge0;
- btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
-
- btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
- btVector3 worldA1 = transA*a;
- btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
-
-//int otherFace=0;
-#ifdef BLA1
- int otherFace = polyA.m_connectedFaces[e0];
- btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
- btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
-
- btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
- btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
-#else
- btVector3 planeNormalWS = planeNormalWS1;
- btScalar planeEqWS=planeEqWS1;
-
-#endif
- //clip face
-
- clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
- btSwap(pVtxIn,pVtxOut);
- pVtxOut->resize(0);
- }
-
-
-
-//#define ONLY_REPORT_DEEPEST_POINT
-
- btVector3 point;
-
-
- // only keep points that are behind the witness face
- {
- btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
- btScalar localPlaneEq = polyA.m_plane[3];
- btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
- btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
- for (int i=0;i<pVtxIn->size();i++)
- {
- btVector3 vtx = pVtxIn->at(i);
- btScalar depth = planeNormalWS.dot(vtx)+planeEqWS;
- if (depth <=minDist)
- {
-// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
- depth = minDist;
- }
-
- if (depth <=maxDist)
- {
- btVector3 point = pVtxIn->at(i);
-#ifdef ONLY_REPORT_DEEPEST_POINT
- curMaxDist = depth;
-#else
-#if 0
- if (depth<-3)
- {
- printf("error in btPolyhedralContactClipping depth = %f\n", depth);
- printf("likely wrong separatingNormal passed in\n");
- }
-#endif
- resultOut.addContactPoint(separatingNormal,point,depth);
-#endif
- }
- }
- }
-#ifdef ONLY_REPORT_DEEPEST_POINT
- if (curMaxDist<maxDist)
- {
- resultOut.addContactPoint(separatingNormal,point,curMaxDist);
- }
-#endif //ONLY_REPORT_DEEPEST_POINT
-
-}
-
-
-
-
-
-void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut)
-{
-
- btVector3 separatingNormal = separatingNormal1.normalized();
-// const btVector3 c0 = transA * hullA.m_localCenter;
-// const btVector3 c1 = transB * hullB.m_localCenter;
- //const btVector3 DeltaC2 = c0 - c1;
-
-
-
- int closestFaceB=-1;
- btScalar dmax = -FLT_MAX;
- {
- for(int face=0;face<hullB.m_faces.size();face++)
- {
- const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
- const btVector3 WorldNormal = transB.getBasis() * Normal;
- btScalar d = WorldNormal.dot(separatingNormal);
- if (d > dmax)
- {
- dmax = d;
- closestFaceB = face;
- }
- }
- }
- worldVertsB1.resize(0);
- {
- const btFace& polyB = hullB.m_faces[closestFaceB];
- const int numVertices = polyB.m_indices.size();
- for(int e0=0;e0<numVertices;e0++)
- {
- const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
- worldVertsB1.push_back(transB*b);
- }
- }
-
-
- if (closestFaceB>=0)
- clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, worldVertsB2,minDist, maxDist,resultOut);
-
-}
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
deleted file mode 100644
index 30e3db687b..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-///This file was written by Erwin Coumans
-
-
-#ifndef BT_POLYHEDRAL_CONTACT_CLIPPING_H
-#define BT_POLYHEDRAL_CONTACT_CLIPPING_H
-
-
-#include "LinearMath/btAlignedObjectArray.h"
-#include "LinearMath/btTransform.h"
-#include "btDiscreteCollisionDetectorInterface.h"
-
-class btConvexPolyhedron;
-
-typedef btAlignedObjectArray<btVector3> btVertexArray;
-
-// Clips a face to the back of a plane
-struct btPolyhedralContactClipping
-{
-
- static void clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut);
-
- static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);
-
-
- static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut);
-
- ///the clipFace method is used internally
- static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS);
-
-};
-
-#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
deleted file mode 100644
index 786efd1820..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
+++ /dev/null
@@ -1,178 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-//#include <stdio.h>
-
-#include "BulletCollision/CollisionShapes/btConvexShape.h"
-#include "BulletCollision/CollisionShapes/btTriangleShape.h"
-#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
-#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
-#include "btRaycastCallback.h"
-
-btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags)
- :
- m_from(from),
- m_to(to),
- //@BP Mod
- m_flags(flags),
- m_hitFraction(btScalar(1.))
-{
-
-}
-
-
-
-void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
-{
- const btVector3 &vert0=triangle[0];
- const btVector3 &vert1=triangle[1];
- const btVector3 &vert2=triangle[2];
-
- btVector3 v10; v10 = vert1 - vert0 ;
- btVector3 v20; v20 = vert2 - vert0 ;
-
- btVector3 triangleNormal; triangleNormal = v10.cross( v20 );
-
- const btScalar dist = vert0.dot(triangleNormal);
- btScalar dist_a = triangleNormal.dot(m_from) ;
- dist_a-= dist;
- btScalar dist_b = triangleNormal.dot(m_to);
- dist_b -= dist;
-
- if ( dist_a * dist_b >= btScalar(0.0) )
- {
- return ; // same sign
- }
-
- if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a <= btScalar(0.0)))
- {
- // Backface, skip check
- return;
- }
-
-
- const btScalar proj_length=dist_a-dist_b;
- const btScalar distance = (dist_a)/(proj_length);
- // Now we have the intersection point on the plane, we'll see if it's inside the triangle
- // Add an epsilon as a tolerance for the raycast,
- // in case the ray hits exacly on the edge of the triangle.
- // It must be scaled for the triangle size.
-
- if(distance < m_hitFraction)
- {
-
-
- btScalar edge_tolerance =triangleNormal.length2();
- edge_tolerance *= btScalar(-0.0001);
- btVector3 point; point.setInterpolate3( m_from, m_to, distance);
- {
- btVector3 v0p; v0p = vert0 - point;
- btVector3 v1p; v1p = vert1 - point;
- btVector3 cp0; cp0 = v0p.cross( v1p );
-
- if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance)
- {
-
-
- btVector3 v2p; v2p = vert2 - point;
- btVector3 cp1;
- cp1 = v1p.cross( v2p);
- if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance)
- {
- btVector3 cp2;
- cp2 = v2p.cross(v0p);
-
- if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance)
- {
- //@BP Mod
- // Triangle normal isn't normalized
- triangleNormal.normalize();
-
- //@BP Mod - Allow for unflipped normal when raycasting against backfaces
- if (((m_flags & kF_KeepUnflippedNormal) == 0) && (dist_a <= btScalar(0.0)))
- {
- m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
- }
- else
- {
- m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
- }
- }
- }
- }
- }
- }
-}
-
-
-btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin)
-{
- m_convexShape = convexShape;
- m_convexShapeFrom = convexShapeFrom;
- m_convexShapeTo = convexShapeTo;
- m_triangleToWorld = triangleToWorld;
- m_hitFraction = 1.0f;
- m_triangleCollisionMargin = triangleCollisionMargin;
- m_allowedPenetration = 0.f;
-}
-
-void
-btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex)
-{
- btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]);
- triangleShape.setMargin(m_triangleCollisionMargin);
-
- btVoronoiSimplexSolver simplexSolver;
- btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
-
-//#define USE_SUBSIMPLEX_CONVEX_CAST 1
-//if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below
-#ifdef USE_SUBSIMPLEX_CONVEX_CAST
- btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver);
-#else
- //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver);
- btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver);
-#endif //#USE_SUBSIMPLEX_CONVEX_CAST
-
- btConvexCast::CastResult castResult;
- castResult.m_fraction = btScalar(1.);
- castResult.m_allowedPenetration = m_allowedPenetration;
- if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult))
- {
- //add hit
- if (castResult.m_normal.length2() > btScalar(0.0001))
- {
- if (castResult.m_fraction < m_hitFraction)
- {
-/* btContinuousConvexCast's normal is already in world space */
-/*
-#ifdef USE_SUBSIMPLEX_CONVEX_CAST
- //rotate normal into worldspace
- castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal;
-#endif //USE_SUBSIMPLEX_CONVEX_CAST
-*/
- castResult.m_normal.normalize();
-
- reportHit (castResult.m_normal,
- castResult.m_hitPoint,
- castResult.m_fraction,
- partId,
- triangleIndex);
- }
- }
- }
-}
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
deleted file mode 100644
index f2ed0cd39c..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-#ifndef BT_RAYCAST_TRI_CALLBACK_H
-#define BT_RAYCAST_TRI_CALLBACK_H
-
-#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
-#include "LinearMath/btTransform.h"
-struct btBroadphaseProxy;
-class btConvexShape;
-
-class btTriangleRaycastCallback: public btTriangleCallback
-{
-public:
-
- //input
- btVector3 m_from;
- btVector3 m_to;
-
- //@BP Mod - allow backface filtering and unflipped normals
- enum EFlags
- {
- kF_None = 0,
- kF_FilterBackfaces = 1 << 0,
- kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle
- ///SubSimplexConvexCastRaytest is the default, even if kF_None is set.
- kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm
- kF_UseGjkConvexCastRaytest = 1 << 3,
- kF_Terminator = 0xFFFFFFFF
- };
- unsigned int m_flags;
-
- btScalar m_hitFraction;
-
- btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0);
-
- virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
-
- virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) = 0;
-
-};
-
-class btTriangleConvexcastCallback : public btTriangleCallback
-{
-public:
- const btConvexShape* m_convexShape;
- btTransform m_convexShapeFrom;
- btTransform m_convexShapeTo;
- btTransform m_triangleToWorld;
- btScalar m_hitFraction;
- btScalar m_triangleCollisionMargin;
- btScalar m_allowedPenetration;
-
- btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin);
-
- virtual void processTriangle (btVector3* triangle, int partId, int triangleIndex);
-
- virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
-};
-
-#endif //BT_RAYCAST_TRI_CALLBACK_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
deleted file mode 100644
index da8a13914c..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-
-#ifndef BT_SIMPLEX_SOLVER_INTERFACE_H
-#define BT_SIMPLEX_SOLVER_INTERFACE_H
-
-#include "LinearMath/btVector3.h"
-
-#define NO_VIRTUAL_INTERFACE 1
-#ifdef NO_VIRTUAL_INTERFACE
-#include "btVoronoiSimplexSolver.h"
-#define btSimplexSolverInterface btVoronoiSimplexSolver
-#else
-
-/// btSimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
-/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
-/// voronoi regions or barycentric coordinates
-class btSimplexSolverInterface
-{
- public:
- virtual ~btSimplexSolverInterface() {};
-
- virtual void reset() = 0;
-
- virtual void addVertex(const btVector3& w, const btVector3& p, const btVector3& q) = 0;
-
- virtual bool closest(btVector3& v) = 0;
-
- virtual btScalar maxVertex() = 0;
-
- virtual bool fullSimplex() const = 0;
-
- virtual int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const = 0;
-
- virtual bool inSimplex(const btVector3& w) = 0;
-
- virtual void backup_closest(btVector3& v) = 0;
-
- virtual bool emptySimplex() const = 0;
-
- virtual void compute_points(btVector3& p1, btVector3& p2) = 0;
-
- virtual int numVertices() const =0;
-
-
-};
-#endif
-#endif //BT_SIMPLEX_SOLVER_INTERFACE_H
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
deleted file mode 100644
index ec638f60ba..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
+++ /dev/null
@@ -1,160 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#include "btSubSimplexConvexCast.h"
-#include "BulletCollision/CollisionShapes/btConvexShape.h"
-
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
-#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
-#include "btPointCollector.h"
-#include "LinearMath/btTransformUtil.h"
-
-btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
-:m_simplexSolver(simplexSolver),
-m_convexA(convexA),m_convexB(convexB)
-{
-}
-
-///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
-///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
-#ifdef BT_USE_DOUBLE_PRECISION
-#define MAX_ITERATIONS 64
-#else
-#define MAX_ITERATIONS 32
-#endif
-bool btSubsimplexConvexCast::calcTimeOfImpact(
- const btTransform& fromA,
- const btTransform& toA,
- const btTransform& fromB,
- const btTransform& toB,
- CastResult& result)
-{
-
- m_simplexSolver->reset();
-
- btVector3 linVelA,linVelB;
- linVelA = toA.getOrigin()-fromA.getOrigin();
- linVelB = toB.getOrigin()-fromB.getOrigin();
-
- btScalar lambda = btScalar(0.);
-
- btTransform interpolatedTransA = fromA;
- btTransform interpolatedTransB = fromB;
-
- ///take relative motion
- btVector3 r = (linVelA-linVelB);
- btVector3 v;
-
- btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
- btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
- v = supVertexA-supVertexB;
- int maxIter = MAX_ITERATIONS;
-
- btVector3 n;
- n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
-
- btVector3 c;
-
-
-
-
- btScalar dist2 = v.length2();
-#ifdef BT_USE_DOUBLE_PRECISION
- btScalar epsilon = btScalar(0.0001);
-#else
- btScalar epsilon = btScalar(0.0001);
-#endif //BT_USE_DOUBLE_PRECISION
- btVector3 w,p;
- btScalar VdotR;
-
- while ( (dist2 > epsilon) && maxIter--)
- {
- supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
- supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
- w = supVertexA-supVertexB;
-
- btScalar VdotW = v.dot(w);
-
- if (lambda > btScalar(1.0))
- {
- return false;
- }
-
- if ( VdotW > btScalar(0.))
- {
- VdotR = v.dot(r);
-
- if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
- return false;
- else
- {
- lambda = lambda - VdotW / VdotR;
- //interpolate to next lambda
- // x = s + lambda * r;
- interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
- interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
- //m_simplexSolver->reset();
- //check next line
- w = supVertexA-supVertexB;
-
- n = v;
-
- }
- }
- ///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
- if (!m_simplexSolver->inSimplex(w))
- m_simplexSolver->addVertex( w, supVertexA , supVertexB);
-
- if (m_simplexSolver->closest(v))
- {
- dist2 = v.length2();
-
- //todo: check this normal for validity
- //n=v;
- //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
- //printf("DIST2=%f\n",dist2);
- //printf("numverts = %i\n",m_simplexSolver->numVertices());
- } else
- {
- dist2 = btScalar(0.);
- }
- }
-
- //int numiter = MAX_ITERATIONS - maxIter;
-// printf("number of iterations: %d", numiter);
-
- //don't report a time of impact when moving 'away' from the hitnormal
-
-
- result.m_fraction = lambda;
- if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON))
- result.m_normal = n.normalized();
- else
- result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0));
-
- //don't report time of impact for motion away from the contact normal (or causes minor penetration)
- if (result.m_normal.dot(r)>=-result.m_allowedPenetration)
- return false;
-
- btVector3 hitA,hitB;
- m_simplexSolver->compute_points(hitA,hitB);
- result.m_hitPoint=hitB;
- return true;
-}
-
-
-
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
deleted file mode 100644
index 6c8127983e..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-#ifndef BT_SUBSIMPLEX_CONVEX_CAST_H
-#define BT_SUBSIMPLEX_CONVEX_CAST_H
-
-#include "btConvexCast.h"
-#include "btSimplexSolverInterface.h"
-class btConvexShape;
-
-/// btSubsimplexConvexCast implements Gino van den Bergens' paper
-///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
-/// GJK based Ray Cast, optimized version
-/// Objects should not start in overlap, otherwise results are not defined.
-class btSubsimplexConvexCast : public btConvexCast
-{
- btSimplexSolverInterface* m_simplexSolver;
- const btConvexShape* m_convexA;
- const btConvexShape* m_convexB;
-
-public:
-
- btSubsimplexConvexCast (const btConvexShape* shapeA,const btConvexShape* shapeB,btSimplexSolverInterface* simplexSolver);
-
- //virtual ~btSubsimplexConvexCast();
- ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
- ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
- virtual bool calcTimeOfImpact(
- const btTransform& fromA,
- const btTransform& toA,
- const btTransform& fromB,
- const btTransform& toB,
- CastResult& result);
-
-};
-
-#endif //BT_SUBSIMPLEX_CONVEX_CAST_H
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
deleted file mode 100644
index 756373c9b5..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
+++ /dev/null
@@ -1,612 +0,0 @@
-
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-
- Elsevier CDROM license agreements grants nonexclusive license to use the software
- for any purpose, commercial or non-commercial as long as the following credit is included
- identifying the original source of the software:
-
- Parts of the source are "from the book Real-Time Collision Detection by
- Christer Ericson, published by Morgan Kaufmann Publishers,
- (c) 2005 Elsevier Inc."
-
-*/
-
-
-#include "btVoronoiSimplexSolver.h"
-
-#define VERTA 0
-#define VERTB 1
-#define VERTC 2
-#define VERTD 3
-
-#define CATCH_DEGENERATE_TETRAHEDRON 1
-void btVoronoiSimplexSolver::removeVertex(int index)
-{
-
- btAssert(m_numVertices>0);
- m_numVertices--;
- m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
- m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
- m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
-}
-
-void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
-{
- if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
- removeVertex(3);
-
- if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
- removeVertex(2);
-
- if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
- removeVertex(1);
-
- if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
- removeVertex(0);
-
-}
-
-
-
-
-
-//clear the simplex, remove all the vertices
-void btVoronoiSimplexSolver::reset()
-{
- m_cachedValidClosest = false;
- m_numVertices = 0;
- m_needsUpdate = true;
- m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
- m_cachedBC.reset();
-}
-
-
-
- //add a vertex
-void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q)
-{
- m_lastW = w;
- m_needsUpdate = true;
-
- m_simplexVectorW[m_numVertices] = w;
- m_simplexPointsP[m_numVertices] = p;
- m_simplexPointsQ[m_numVertices] = q;
-
- m_numVertices++;
-}
-
-bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
-{
-
- if (m_needsUpdate)
- {
- m_cachedBC.reset();
-
- m_needsUpdate = false;
-
- switch (numVertices())
- {
- case 0:
- m_cachedValidClosest = false;
- break;
- case 1:
- {
- m_cachedP1 = m_simplexPointsP[0];
- m_cachedP2 = m_simplexPointsQ[0];
- m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
- m_cachedBC.reset();
- m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
- m_cachedValidClosest = m_cachedBC.isValid();
- break;
- };
- case 2:
- {
- //closest point origin from line segment
- const btVector3& from = m_simplexVectorW[0];
- const btVector3& to = m_simplexVectorW[1];
- btVector3 nearest;
-
- btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
- btVector3 diff = p - from;
- btVector3 v = to - from;
- btScalar t = v.dot(diff);
-
- if (t > 0) {
- btScalar dotVV = v.dot(v);
- if (t < dotVV) {
- t /= dotVV;
- diff -= t*v;
- m_cachedBC.m_usedVertices.usedVertexA = true;
- m_cachedBC.m_usedVertices.usedVertexB = true;
- } else {
- t = 1;
- diff -= v;
- //reduce to 1 point
- m_cachedBC.m_usedVertices.usedVertexB = true;
- }
- } else
- {
- t = 0;
- //reduce to 1 point
- m_cachedBC.m_usedVertices.usedVertexA = true;
- }
- m_cachedBC.setBarycentricCoordinates(1-t,t);
- nearest = from + t*v;
-
- m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
- m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
- m_cachedV = m_cachedP1 - m_cachedP2;
-
- reduceVertices(m_cachedBC.m_usedVertices);
-
- m_cachedValidClosest = m_cachedBC.isValid();
- break;
- }
- case 3:
- {
- //closest point origin from triangle
- btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
-
- const btVector3& a = m_simplexVectorW[0];
- const btVector3& b = m_simplexVectorW[1];
- const btVector3& c = m_simplexVectorW[2];
-
- closestPtPointTriangle(p,a,b,c,m_cachedBC);
- m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
- m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
- m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];
-
- m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
- m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
- m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];
-
- m_cachedV = m_cachedP1-m_cachedP2;
-
- reduceVertices (m_cachedBC.m_usedVertices);
- m_cachedValidClosest = m_cachedBC.isValid();
-
- break;
- }
- case 4:
- {
-
-
- btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
-
- const btVector3& a = m_simplexVectorW[0];
- const btVector3& b = m_simplexVectorW[1];
- const btVector3& c = m_simplexVectorW[2];
- const btVector3& d = m_simplexVectorW[3];
-
- bool hasSeparation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
-
- if (hasSeparation)
- {
-
- m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
- m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
- m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
- m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
-
- m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
- m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
- m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
- m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
-
- m_cachedV = m_cachedP1-m_cachedP2;
- reduceVertices (m_cachedBC.m_usedVertices);
- } else
- {
-// printf("sub distance got penetration\n");
-
- if (m_cachedBC.m_degenerate)
- {
- m_cachedValidClosest = false;
- } else
- {
- m_cachedValidClosest = true;
- //degenerate case == false, penetration = true + zero
- m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
- }
- break;
- }
-
- m_cachedValidClosest = m_cachedBC.isValid();
-
- //closest point origin from tetrahedron
- break;
- }
- default:
- {
- m_cachedValidClosest = false;
- }
- };
- }
-
- return m_cachedValidClosest;
-
-}
-
-//return/calculate the closest vertex
-bool btVoronoiSimplexSolver::closest(btVector3& v)
-{
- bool succes = updateClosestVectorAndPoints();
- v = m_cachedV;
- return succes;
-}
-
-
-
-btScalar btVoronoiSimplexSolver::maxVertex()
-{
- int i, numverts = numVertices();
- btScalar maxV = btScalar(0.);
- for (i=0;i<numverts;i++)
- {
- btScalar curLen2 = m_simplexVectorW[i].length2();
- if (maxV < curLen2)
- maxV = curLen2;
- }
- return maxV;
-}
-
-
-
- //return the current simplex
-int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
-{
- int i;
- for (i=0;i<numVertices();i++)
- {
- yBuf[i] = m_simplexVectorW[i];
- pBuf[i] = m_simplexPointsP[i];
- qBuf[i] = m_simplexPointsQ[i];
- }
- return numVertices();
-}
-
-
-
-
-bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
-{
- bool found = false;
- int i, numverts = numVertices();
- //btScalar maxV = btScalar(0.);
-
- //w is in the current (reduced) simplex
- for (i=0;i<numverts;i++)
- {
-#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
- if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
-#else
- if (m_simplexVectorW[i] == w)
-#endif
- {
- found = true;
- break;
- }
- }
-
- //check in case lastW is already removed
- if (w == m_lastW)
- return true;
-
- return found;
-}
-
-void btVoronoiSimplexSolver::backup_closest(btVector3& v)
-{
- v = m_cachedV;
-}
-
-
-bool btVoronoiSimplexSolver::emptySimplex() const
-{
- return (numVertices() == 0);
-
-}
-
-void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2)
-{
- updateClosestVectorAndPoints();
- p1 = m_cachedP1;
- p2 = m_cachedP2;
-
-}
-
-
-
-
-bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result)
-{
- result.m_usedVertices.reset();
-
- // Check if P in vertex region outside A
- btVector3 ab = b - a;
- btVector3 ac = c - a;
- btVector3 ap = p - a;
- btScalar d1 = ab.dot(ap);
- btScalar d2 = ac.dot(ap);
- if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0))
- {
- result.m_closestPointOnSimplex = a;
- result.m_usedVertices.usedVertexA = true;
- result.setBarycentricCoordinates(1,0,0);
- return true;// a; // barycentric coordinates (1,0,0)
- }
-
- // Check if P in vertex region outside B
- btVector3 bp = p - b;
- btScalar d3 = ab.dot(bp);
- btScalar d4 = ac.dot(bp);
- if (d3 >= btScalar(0.0) && d4 <= d3)
- {
- result.m_closestPointOnSimplex = b;
- result.m_usedVertices.usedVertexB = true;
- result.setBarycentricCoordinates(0,1,0);
-
- return true; // b; // barycentric coordinates (0,1,0)
- }
- // Check if P in edge region of AB, if so return projection of P onto AB
- btScalar vc = d1*d4 - d3*d2;
- if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
- btScalar v = d1 / (d1 - d3);
- result.m_closestPointOnSimplex = a + v * ab;
- result.m_usedVertices.usedVertexA = true;
- result.m_usedVertices.usedVertexB = true;
- result.setBarycentricCoordinates(1-v,v,0);
- return true;
- //return a + v * ab; // barycentric coordinates (1-v,v,0)
- }
-
- // Check if P in vertex region outside C
- btVector3 cp = p - c;
- btScalar d5 = ab.dot(cp);
- btScalar d6 = ac.dot(cp);
- if (d6 >= btScalar(0.0) && d5 <= d6)
- {
- result.m_closestPointOnSimplex = c;
- result.m_usedVertices.usedVertexC = true;
- result.setBarycentricCoordinates(0,0,1);
- return true;//c; // barycentric coordinates (0,0,1)
- }
-
- // Check if P in edge region of AC, if so return projection of P onto AC
- btScalar vb = d5*d2 - d1*d6;
- if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
- btScalar w = d2 / (d2 - d6);
- result.m_closestPointOnSimplex = a + w * ac;
- result.m_usedVertices.usedVertexA = true;
- result.m_usedVertices.usedVertexC = true;
- result.setBarycentricCoordinates(1-w,0,w);
- return true;
- //return a + w * ac; // barycentric coordinates (1-w,0,w)
- }
-
- // Check if P in edge region of BC, if so return projection of P onto BC
- btScalar va = d3*d6 - d5*d4;
- if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
- btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
-
- result.m_closestPointOnSimplex = b + w * (c - b);
- result.m_usedVertices.usedVertexB = true;
- result.m_usedVertices.usedVertexC = true;
- result.setBarycentricCoordinates(0,1-w,w);
- return true;
- // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
- }
-
- // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
- btScalar denom = btScalar(1.0) / (va + vb + vc);
- btScalar v = vb * denom;
- btScalar w = vc * denom;
-
- result.m_closestPointOnSimplex = a + ab * v + ac * w;
- result.m_usedVertices.usedVertexA = true;
- result.m_usedVertices.usedVertexB = true;
- result.m_usedVertices.usedVertexC = true;
- result.setBarycentricCoordinates(1-v-w,v,w);
-
- return true;
-// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
-
-}
-
-
-
-
-
-/// Test if point p and d lie on opposite sides of plane through abc
-int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d)
-{
- btVector3 normal = (b-a).cross(c-a);
-
- btScalar signp = (p - a).dot(normal); // [AP AB AC]
- btScalar signd = (d - a).dot( normal); // [AD AB AC]
-
-#ifdef CATCH_DEGENERATE_TETRAHEDRON
-#ifdef BT_USE_DOUBLE_PRECISION
-if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
- {
- return -1;
- }
-#else
- if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
- {
-// printf("affine dependent/degenerate\n");//
- return -1;
- }
-#endif
-
-#endif
- // Points on opposite sides if expression signs are opposite
- return signp * signd < btScalar(0.);
-}
-
-
-bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
-{
- btSubSimplexClosestResult tempResult;
-
- // Start out assuming point inside all halfspaces, so closest to itself
- finalResult.m_closestPointOnSimplex = p;
- finalResult.m_usedVertices.reset();
- finalResult.m_usedVertices.usedVertexA = true;
- finalResult.m_usedVertices.usedVertexB = true;
- finalResult.m_usedVertices.usedVertexC = true;
- finalResult.m_usedVertices.usedVertexD = true;
-
- int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
- int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
- int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
- int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
-
- if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
- {
- finalResult.m_degenerate = true;
- return false;
- }
-
- if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
- {
- return false;
- }
-
-
- btScalar bestSqDist = FLT_MAX;
- // If point outside face abc then compute closest point on abc
- if (pointOutsideABC)
- {
- closestPtPointTriangle(p, a, b, c,tempResult);
- btVector3 q = tempResult.m_closestPointOnSimplex;
-
- btScalar sqDist = (q - p).dot( q - p);
- // Update best closest point if (squared) distance is less than current best
- if (sqDist < bestSqDist) {
- bestSqDist = sqDist;
- finalResult.m_closestPointOnSimplex = q;
- //convert result bitmask!
- finalResult.m_usedVertices.reset();
- finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
- finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
- finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
- finalResult.setBarycentricCoordinates(
- tempResult.m_barycentricCoords[VERTA],
- tempResult.m_barycentricCoords[VERTB],
- tempResult.m_barycentricCoords[VERTC],
- 0
- );
-
- }
- }
-
-
- // Repeat test for face acd
- if (pointOutsideACD)
- {
- closestPtPointTriangle(p, a, c, d,tempResult);
- btVector3 q = tempResult.m_closestPointOnSimplex;
- //convert result bitmask!
-
- btScalar sqDist = (q - p).dot( q - p);
- if (sqDist < bestSqDist)
- {
- bestSqDist = sqDist;
- finalResult.m_closestPointOnSimplex = q;
- finalResult.m_usedVertices.reset();
- finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
-
- finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
- finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
- finalResult.setBarycentricCoordinates(
- tempResult.m_barycentricCoords[VERTA],
- 0,
- tempResult.m_barycentricCoords[VERTB],
- tempResult.m_barycentricCoords[VERTC]
- );
-
- }
- }
- // Repeat test for face adb
-
-
- if (pointOutsideADB)
- {
- closestPtPointTriangle(p, a, d, b,tempResult);
- btVector3 q = tempResult.m_closestPointOnSimplex;
- //convert result bitmask!
-
- btScalar sqDist = (q - p).dot( q - p);
- if (sqDist < bestSqDist)
- {
- bestSqDist = sqDist;
- finalResult.m_closestPointOnSimplex = q;
- finalResult.m_usedVertices.reset();
- finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
- finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
-
- finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
- finalResult.setBarycentricCoordinates(
- tempResult.m_barycentricCoords[VERTA],
- tempResult.m_barycentricCoords[VERTC],
- 0,
- tempResult.m_barycentricCoords[VERTB]
- );
-
- }
- }
- // Repeat test for face bdc
-
-
- if (pointOutsideBDC)
- {
- closestPtPointTriangle(p, b, d, c,tempResult);
- btVector3 q = tempResult.m_closestPointOnSimplex;
- //convert result bitmask!
- btScalar sqDist = (q - p).dot( q - p);
- if (sqDist < bestSqDist)
- {
- bestSqDist = sqDist;
- finalResult.m_closestPointOnSimplex = q;
- finalResult.m_usedVertices.reset();
- //
- finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
- finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
- finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
-
- finalResult.setBarycentricCoordinates(
- 0,
- tempResult.m_barycentricCoords[VERTA],
- tempResult.m_barycentricCoords[VERTC],
- tempResult.m_barycentricCoords[VERTB]
- );
-
- }
- }
-
- //help! we ended up full !
-
- if (finalResult.m_usedVertices.usedVertexA &&
- finalResult.m_usedVertices.usedVertexB &&
- finalResult.m_usedVertices.usedVertexC &&
- finalResult.m_usedVertices.usedVertexD)
- {
- return true;
- }
-
- return true;
-}
-
diff --git a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
deleted file mode 100644
index 80fd490f4e..0000000000
--- a/thirdparty/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
+++ /dev/null
@@ -1,185 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
-
-This software is provided 'as-is', without any express or implied warranty.
-In no event will the authors be held liable for any damages arising from the use of this software.
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it freely,
-subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
-2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
-3. This notice may not be removed or altered from any source distribution.
-*/
-
-
-
-#ifndef BT_VORONOI_SIMPLEX_SOLVER_H
-#define BT_VORONOI_SIMPLEX_SOLVER_H
-
-#include "btSimplexSolverInterface.h"
-
-
-
-#define VORONOI_SIMPLEX_MAX_VERTS 5
-
-///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure
-#define BT_USE_EQUAL_VERTEX_THRESHOLD
-
-#ifdef BT_USE_DOUBLE_PRECISION
-#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 1e-12f
-#else
-#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f
-#endif//BT_USE_DOUBLE_PRECISION
-
-struct btUsageBitfield{
- btUsageBitfield()
- {
- reset();
- }
-
- void reset()
- {
- usedVertexA = false;
- usedVertexB = false;
- usedVertexC = false;
- usedVertexD = false;
- }
- unsigned short usedVertexA : 1;
- unsigned short usedVertexB : 1;
- unsigned short usedVertexC : 1;
- unsigned short usedVertexD : 1;
- unsigned short unused1 : 1;
- unsigned short unused2 : 1;
- unsigned short unused3 : 1;
- unsigned short unused4 : 1;
-};
-
-
-struct btSubSimplexClosestResult
-{
- btVector3 m_closestPointOnSimplex;
- //MASK for m_usedVertices
- //stores the simplex vertex-usage, using the MASK,
- // if m_usedVertices & MASK then the related vertex is used
- btUsageBitfield m_usedVertices;
- btScalar m_barycentricCoords[4];
- bool m_degenerate;
-
- void reset()
- {
- m_degenerate = false;
- setBarycentricCoordinates();
- m_usedVertices.reset();
- }
- bool isValid()
- {
- bool valid = (m_barycentricCoords[0] >= btScalar(0.)) &&
- (m_barycentricCoords[1] >= btScalar(0.)) &&
- (m_barycentricCoords[2] >= btScalar(0.)) &&
- (m_barycentricCoords[3] >= btScalar(0.));
-
-
- return valid;
- }
- void setBarycentricCoordinates(btScalar a=btScalar(0.),btScalar b=btScalar(0.),btScalar c=btScalar(0.),btScalar d=btScalar(0.))
- {
- m_barycentricCoords[0] = a;
- m_barycentricCoords[1] = b;
- m_barycentricCoords[2] = c;
- m_barycentricCoords[3] = d;
- }
-
-};
-
-/// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
-/// Can be used with GJK, as an alternative to Johnson distance algorithm.
-#ifdef NO_VIRTUAL_INTERFACE
-ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver
-#else
-ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver : public btSimplexSolverInterface
-#endif
-{
-public:
-
- BT_DECLARE_ALIGNED_ALLOCATOR();
-
- int m_numVertices;
-
- btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
- btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
- btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
-
-
-
- btVector3 m_cachedP1;
- btVector3 m_cachedP2;
- btVector3 m_cachedV;
- btVector3 m_lastW;
-
- btScalar m_equalVertexThreshold;
- bool m_cachedValidClosest;
-
-
- btSubSimplexClosestResult m_cachedBC;
-
- bool m_needsUpdate;
-
- void removeVertex(int index);
- void reduceVertices (const btUsageBitfield& usedVerts);
- bool updateClosestVectorAndPoints();
-
- bool closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult);
- int pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d);
- bool closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result);
-
-public:
-
- btVoronoiSimplexSolver()
- : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD)
- {
- }
- void reset();
-
- void addVertex(const btVector3& w, const btVector3& p, const btVector3& q);
-
- void setEqualVertexThreshold(btScalar threshold)
- {
- m_equalVertexThreshold = threshold;
- }
-
- btScalar getEqualVertexThreshold() const
- {
- return m_equalVertexThreshold;
- }
-
- bool closest(btVector3& v);
-
- btScalar maxVertex();
-
- bool fullSimplex() const
- {
- return (m_numVertices == 4);
- }
-
- int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;
-
- bool inSimplex(const btVector3& w);
-
- void backup_closest(btVector3& v) ;
-
- bool emptySimplex() const ;
-
- void compute_points(btVector3& p1, btVector3& p2) ;
-
- int numVertices() const
- {
- return m_numVertices;
- }
-
-
-};
-
-#endif //BT_VORONOI_SIMPLEX_SOLVER_H
-