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