Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

639
src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h
View File

@@ -16,163 +16,153 @@ subject to the following restrictions:
#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 "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)
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;
(void)v;
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;
// 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)
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();
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)
@@ -182,188 +172,183 @@ int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate
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 //
//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;
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
#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H

125
src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
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"
@@ -24,59 +23,60 @@ subject to the following restrictions:
#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 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)
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)
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 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
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;
convexInPlaneTrans = transB.inverse() * convexWorldTransform;
btTransform planeInConvex;
planeInConvex= convexWorldTransform.inverse() * transB;
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
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 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
@@ -87,36 +87,33 @@ void btContinuousConvexCollision::computeClosestPoints( const btTransform& trans
}
}
bool btContinuousConvexCollision::calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
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);
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 boundingRadiusB = m_convexB1 ? m_convexB1->getAngularMotionDisc() : 0.f;
btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
btVector3 relLinVel = (linVelB-linVelA);
btVector3 relLinVel = (linVelB - linVelA);
btScalar relLinVelocLength = (linVelB-linVelA).length();
if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f)
btScalar relLinVelocLength = (linVelB - linVelA).length();
if ((relLinVelocLength + maxAngularProjectedVelocity) == 0.f)
return false;
btScalar lambda = btScalar(0.);
btVector3 n;
n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
bool hasResult = false;
btVector3 c;
@@ -126,14 +123,13 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
int numIter = 0;
//first solution, using GJK
btScalar radius = 0.001f;
// result.drawCoordSystem(sphereTr);
// result.drawCoordSystem(sphereTr);
btPointCollector pointCollector1;
btPointCollector pointCollector1;
{
computeClosestPoints(fromA,fromB,pointCollector1);
{
computeClosestPoints(fromA, fromB, pointCollector1);
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
@@ -145,7 +141,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
dist = pointCollector1.m_distance + result.m_allowedPenetration;
n = pointCollector1.m_normalOnBInWorld;
btScalar projectedLinearVelocity = relLinVel.dot(n);
if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON)
return false;
//not close enough
@@ -153,18 +149,17 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
{
if (result.m_debugDrawer)
{
result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1));
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)
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= SIMD_EPSILON)
return false;
dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
lambda += dLambda;
@@ -181,28 +176,29 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
lastLambda = lambda;
//interpolate to next lambda
btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
btTransform interpolatedTransA, interpolatedTransB, relativeTrans;
btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
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.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(), 0.2f, btVector3(1, 0, 0));
}
result.DebugDraw( lambda );
result.DebugDraw(lambda);
btPointCollector pointCollector;
computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector);
btPointCollector pointCollector;
computeClosestPoints(interpolatedTransA, interpolatedTransB, pointCollector);
if (pointCollector.m_hasResult)
{
dist = pointCollector.m_distance+result.m_allowedPenetration;
c = pointCollector.m_pointInWorld;
dist = pointCollector.m_distance + result.m_allowedPenetration;
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
} else
}
else
{
result.reportFailure(-1, numIter);
return false;
@@ -215,7 +211,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
return false;
}
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
@@ -224,4 +220,3 @@ bool btContinuousConvexCollision::calcTimeOfImpact(
return false;
}

34
src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
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
@@ -30,30 +29,25 @@ class btStaticPlaneShape;
class btContinuousConvexCollision : public btConvexCast
{
btSimplexSolverInterface* m_simplexSolver;
btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
const btConvexShape* m_convexA;
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;
const btConvexShape* m_convexB1;
const btStaticPlaneShape* m_planeShape;
void computeClosestPoints( const btTransform& transA, const btTransform& transB,struct btPointCollector& pointCollector);
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 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);
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
#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H

54
src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_CONVEX_CAST_H
#define BT_CONVEX_CAST_H
@@ -27,47 +26,46 @@ class btMinkowskiSumShape;
class btConvexCast
{
public:
virtual ~btConvexCast();
///RayResult stores the closest result
/// alternatively, add a callback method to decide about closest/all results
struct CastResult
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;}
virtual void DebugDraw(btScalar fraction) { (void)fraction; }
virtual void drawCoordSystem(const btTransform& trans) { (void)trans; }
virtual void reportFailure(int errNo, int numIterations)
{
(void)errNo;
(void)numIterations;
}
CastResult()
:m_fraction(btScalar(BT_LARGE_FLOAT)),
m_debugDrawer(0),
m_allowedPenetration(btScalar(0))
: m_fraction(btScalar(BT_LARGE_FLOAT)),
m_debugDrawer(0),
m_allowedPenetration(btScalar(0))
{
}
virtual ~CastResult(){};
virtual ~CastResult() {};
btTransform m_hitTransformA;
btTransform m_hitTransformB;
btVector3 m_normal;
btVector3 m_hitPoint;
btScalar m_fraction; //input and output
btTransform m_hitTransformA;
btTransform m_hitTransformB;
btVector3 m_normal;
btVector3 m_hitPoint;
btScalar m_fraction; //input and output
btIDebugDraw* m_debugDrawer;
btScalar m_allowedPenetration;
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;
virtual bool calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result) = 0;
};
#endif //BT_CONVEX_CAST_H
#endif //BT_CONVEX_CAST_H

21
src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
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
@@ -25,16 +24,12 @@ 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;
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
#endif //BT_CONVEX_PENETRATION_DEPTH_H

57
src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
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
@@ -27,64 +26,60 @@ subject to the following restrictions:
/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
struct btDiscreteCollisionDetectorInterface
{
struct Result
{
virtual ~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;
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))
: m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT))
{
}
btTransform m_transformA;
btTransform m_transformB;
btScalar m_maximumDistanceSquared;
btScalar m_maximumDistanceSquared;
};
virtual ~btDiscreteCollisionDetectorInterface() {};
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;
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 !
btVector3 m_normalOnSurfaceB;
btVector3 m_closestPointInB;
btScalar m_distance; //negative means penetration !
protected:
btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT))
{
}
public:
virtual ~btStorageResult() {};
protected:
btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT))
{
}
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
public:
virtual ~btStorageResult(){};
virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
{
if (depth < m_distance)
{
if (depth < m_distance)
{
m_normalOnSurfaceB = normalOnBInWorld;
m_closestPointInB = pointInWorld;
m_distance = depth;
}
m_normalOnSurfaceB = normalOnBInWorld;
m_closestPointInB = pointInWorld;
m_distance = depth;
}
}
};
#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H

32
src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef GJK_COLLISION_DESCRIPTION_H
#define GJK_COLLISION_DESCRIPTION_H
@@ -21,21 +20,20 @@ subject to the following restrictions:
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()
{
}
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
#endif //GJK_COLLISION_DESCRIPTION_H

76
src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
View File

@@ -13,8 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btGjkConvexCast.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "btGjkPairDetector.h"
@@ -27,41 +25,39 @@ subject to the following restrictions:
#define MAX_ITERATIONS 32
#endif
btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
:m_simplexSolver(simplexSolver),
m_convexA(convexA),
m_convexB(convexB)
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)
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();
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);
btVector3 v(1, 0, 0);
int maxIter = MAX_ITERATIONS;
btVector3 n;
n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
bool hasResult = false;
btVector3 c;
btVector3 r = (linVelA-linVelB);
btVector3 r = (linVelA - linVelB);
btScalar lastLambda = lambda;
//btScalar epsilon = btScalar(0.001);
@@ -69,17 +65,14 @@ bool btGjkConvexCast::calcTimeOfImpact(
int numIter = 0;
//first solution, using GJK
btTransform identityTrans;
identityTrans.setIdentity();
// result.drawCoordSystem(sphereTr);
// result.drawCoordSystem(sphereTr);
btPointCollector pointCollector;
btPointCollector pointCollector;
btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);
btGjkPairDetector gjk(m_convexA, m_convexB, m_simplexSolver, 0); //m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
//we don't use margins during CCD
@@ -87,7 +80,7 @@ bool btGjkConvexCast::calcTimeOfImpact(
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.getClosestPoints(input,pointCollector,0);
gjk.getClosestPoints(input, pointCollector, 0);
hasResult = pointCollector.m_hasResult;
c = pointCollector.m_pointInWorld;
@@ -98,20 +91,18 @@ bool btGjkConvexCast::calcTimeOfImpact(
dist = pointCollector.m_distance;
n = pointCollector.m_normalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return false; //todo: report a failure
return false; //todo: report a failure
}
btScalar dLambda = btScalar(0.);
btScalar projectedLinearVelocity = r.dot(n);
dLambda = dist / (projectedLinearVelocity);
lambda = lambda - dLambda;
@@ -132,35 +123,35 @@ bool btGjkConvexCast::calcTimeOfImpact(
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);
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_normal = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return true;
}
c = pointCollector.m_pointInWorld;
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
} else
}
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)
if (n.dot(r) >= -result.m_allowedPenetration)
return false;
result.m_fraction = lambda;
@@ -170,7 +161,4 @@ bool btGjkConvexCast::calcTimeOfImpact(
}
return false;
}

26
src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
View File

@@ -13,8 +13,6 @@ subject to the following restrictions:
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
@@ -29,22 +27,20 @@ class btMinkowskiSumShape;
///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;
btSimplexSolverInterface* m_simplexSolver;
const btConvexShape* m_convexA;
const btConvexShape* m_convexB;
public:
btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);
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);
virtual bool calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result);
};
#endif //BT_GJK_CONVEX_CAST_H
#endif //BT_GJK_CONVEX_CAST_H

1853
src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
View File

File diff suppressed because it is too large Load Diff

62
src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
View File

@@ -28,48 +28,46 @@ GJK-EPA collision solver by Nathanael Presson, 2008
#include "BulletCollision/CollisionShapes/btConvexShape.h"
///btGjkEpaSolver contributed under zlib by Nathanael Presson
struct btGjkEpaSolver2
struct btGjkEpaSolver2
{
struct sResults
struct sResults
{
enum eStatus
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;
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 int StackSizeRequirement();
static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0,
const btConvexShape* shape1,const btTransform& wtrs1,
const btVector3& guess,
sResults& results);
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,
static bool Penetration(const btConvexShape* shape0, const btTransform& wtrs0,
const btConvexShape* shape1, const btTransform& wtrs1,
const btVector3& guess,
sResults& results,
bool usemargins=true);
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__
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
#endif //BT_GJK_EPA2_H

1836
src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h
View File

File diff suppressed because it is too large Load Diff

19
src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
View File

@@ -18,16 +18,14 @@ subject to the following restrictions:
#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)
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;
@@ -49,13 +47,13 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simp
for (int i = 0; i < numVectors; i++)
{
simplexSolver.reset();
btVector3 guessVector = guessVectors[i];
btVector3 guessVector = guessVectors[i];
btGjkEpaSolver2::sResults results;
btGjkEpaSolver2::sResults results;
if (btGjkEpaSolver2::Penetration(pConvexA, transformA,
pConvexB, transformB,
guessVector, results))
pConvexB, transformB,
guessVector, results))
{
wWitnessOnA = results.witnesses[0];
@@ -81,4 +79,3 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simp
v.setValue(0, 0, 0);
return false;
}

25
src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h
View File

@@ -23,21 +23,18 @@ subject to the following restrictions:
///calculate the penetration depth between two convex shapes.
class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
{
public :
public:
btGjkEpaPenetrationDepthSolver()
{
}
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 :
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
#endif // BT_GJP_EPA_PENETRATION_DEPTH_H

485
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
View File

File diff suppressed because it is too large Load Diff

52
src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
View File

@@ -13,9 +13,6 @@ subject to the following restrictions:
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
@@ -29,39 +26,34 @@ class btConvexPenetrationDepthSolver;
/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
{
btVector3 m_cachedSeparatingAxis;
btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
btVector3 m_cachedSeparatingAxis;
btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
btSimplexSolverInterface* m_simplexSolver;
const btConvexShape* m_minkowskiA;
const btConvexShape* m_minkowskiB;
int m_shapeTypeA;
int m_shapeTypeA;
int m_shapeTypeB;
btScalar m_marginA;
btScalar m_marginB;
btScalar m_marginA;
btScalar m_marginB;
bool m_ignoreMargin;
btScalar m_cachedSeparatingDistance;
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;
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() {};
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);
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 getClosestPointsNonVirtual(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw);
void setMinkowskiA(const btConvexShape* minkA)
{
@@ -81,23 +73,21 @@ public:
{
return m_cachedSeparatingAxis;
}
btScalar getCachedSeparatingDistance() const
btScalar getCachedSeparatingDistance() const
{
return m_cachedSeparatingDistance;
}
void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver)
void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver)
{
m_penetrationDepthSolver = penetrationDepthSolver;
}
///don't use setIgnoreMargin, it's for Bullet's internal use
void setIgnoreMargin(bool ignoreMargin)
void setIgnoreMargin(bool ignoreMargin)
{
m_ignoreMargin = ignoreMargin;
}
};
#endif //BT_GJK_PAIR_DETECTOR_H
#endif //BT_GJK_PAIR_DETECTOR_H

265
src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
View File

@@ -20,161 +20,152 @@ subject to the following restrictions:
#include "LinearMath/btTransformUtil.h"
#ifdef PFX_USE_FREE_VECTORMATH
#include "physics_effects/base_level/solver/pfx_constraint_row.h"
#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
// 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_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)
{
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)
{
}
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;
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;
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;
};
//BP mod, store contact triangles.
int m_partId0;
int m_partId1;
int m_index0;
int m_index1;
btScalar m_frictionCFM;
mutable void* m_userPersistentData;
//bool m_lateralFrictionInitialized;
int m_contactPointFlags;
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;
}
btScalar m_appliedImpulse;
btScalar m_appliedImpulseLateral1;
btScalar m_appliedImpulseLateral2;
btScalar m_contactMotion1;
btScalar m_contactMotion2;
union {
btScalar m_contactCFM;
btScalar m_combinedContactStiffness1;
};
#endif //BT_MANIFOLD_CONTACT_POINT_H
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

253
src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
View File

@@ -21,42 +21,38 @@ subject to the following restrictions:
#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
)
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();
bool check2d = convexA->isConvex2d() && convexB->isConvex2d();
struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
{
btIntermediateResult():m_hasResult(false)
btIntermediateResult() : m_hasResult(false)
{
}
btVector3 m_normalOnBInWorld;
btVector3 m_pointInWorld;
btScalar m_depth;
bool m_hasResult;
bool m_hasResult;
virtual void setShapeIdentifiersA(int partId0,int index0)
virtual void setShapeIdentifiersA(int partId0, int index0)
{
(void)partId0;
(void)index0;
}
virtual void setShapeIdentifiersB(int partId1,int index1)
virtual void setShapeIdentifiersB(int partId1, int index1)
{
(void)partId1;
(void)index1;
}
void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
{
m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld;
@@ -68,39 +64,39 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
//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;
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];
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++)
for (i = 0; i < numSampleDirections; i++)
{
btVector3 norm = getPenetrationDirections()[i];
seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
seperatingAxisInABatch[i] = (-norm) * transA.getBasis();
seperatingAxisInBBatch[i] = norm * transB.getBasis();
}
{
int numPDA = convexA->getNumPreferredPenetrationDirections();
if (numPDA)
{
for (int i=0;i<numPDA;i++)
for (int i = 0; i < numPDA; i++)
{
btVector3 norm;
convexA->getPreferredPenetrationDirection(i,norm);
norm = transA.getBasis() * norm;
convexA->getPreferredPenetrationDirection(i, norm);
norm = transA.getBasis() * norm;
getPenetrationDirections()[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
@@ -113,11 +109,11 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
int numPDB = convexB->getNumPreferredPenetrationDirections();
if (numPDB)
{
for (int i=0;i<numPDB;i++)
for (int i = 0; i < numPDB; i++)
{
btVector3 norm;
convexB->getPreferredPenetrationDirection(i,norm);
norm = transB.getBasis() * norm;
convexB->getPreferredPenetrationDirection(i, norm);
norm = transB.getBasis() * norm;
getPenetrationDirections()[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
@@ -126,29 +122,25 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
}
}
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
for (i=0;i<numSampleDirections;i++)
for (i = 0; i < numSampleDirections; i++)
{
btVector3 norm = getPenetrationDirections()[i];
if (check2d)
{
norm[2] = 0.f;
}
if (norm.length2()>0.01)
if (norm.length2() > 0.01)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = transA(pInA);
pWorld = transA(pInA);
qWorld = transB(qInB);
if (check2d)
{
@@ -156,7 +148,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
qWorld[2] = 0.f;
}
w = qWorld - pWorld;
w = qWorld - pWorld;
btScalar delta = norm.dot(w);
//find smallest delta
if (delta < minProj)
@@ -167,7 +159,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
minB = qWorld;
}
}
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
@@ -177,11 +169,11 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
int numPDA = convexA->getNumPreferredPenetrationDirections();
if (numPDA)
{
for (int i=0;i<numPDA;i++)
for (int i = 0; i < numPDA; i++)
{
btVector3 norm;
convexA->getPreferredPenetrationDirection(i,norm);
norm = transA.getBasis() * norm;
convexA->getPreferredPenetrationDirection(i, norm);
norm = transA.getBasis() * norm;
getPenetrationDirections()[numSampleDirections] = norm;
numSampleDirections++;
}
@@ -192,28 +184,28 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
int numPDB = convexB->getNumPreferredPenetrationDirections();
if (numPDB)
{
for (int i=0;i<numPDB;i++)
for (int i = 0; i < numPDB; i++)
{
btVector3 norm;
convexB->getPreferredPenetrationDirection(i,norm);
norm = transB.getBasis() * norm;
convexB->getPreferredPenetrationDirection(i, norm);
norm = transB.getBasis() * norm;
getPenetrationDirections()[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
#endif // __SPU__
#endif // __SPU__
for (int i=0;i<numSampleDirections;i++)
for (int i = 0; i < numSampleDirections; i++)
{
const btVector3& norm = getPenetrationDirections()[i];
seperatingAxisInA = (-norm)* transA.getBasis();
seperatingAxisInB = norm* transB.getBasis();
seperatingAxisInA = (-norm) * transA.getBasis();
seperatingAxisInB = norm * transB.getBasis();
pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
pWorld = transA(pInA);
pWorld = transA(pInA);
qWorld = transB(qInB);
w = qWorld - pWorld;
w = qWorld - pWorld;
btScalar delta = norm.dot(w);
//find smallest delta
if (delta < minProj)
@@ -224,48 +216,39 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
minB = qWorld;
}
}
#endif //USE_BATCHED_SUPPORT
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm*convexA->getMarginNonVirtual();
minB -= minNorm*convexB->getMarginNonVirtual();
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());
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;
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);
debugDraw->drawLine(minA, minA + (minNorm * minProj), color);
}
#endif //DEBUG_DRAW
#endif //DEBUG_DRAW
btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
btGjkPairDetector gjkdet(convexA, convexB, &simplexSolver, 0);
btScalar offsetDist = minProj;
btVector3 offset = minNorm * offsetDist;
btGjkPairDetector::ClosestPointInput input;
btVector3 newOrg = transA.getOrigin() + offset;
btTransform displacedTrans = transA;
@@ -273,89 +256,81 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); //minProj;
btIntermediateResult res;
gjkdet.setCachedSeperatingAxis(-minNorm);
gjkdet.getClosestPoints(input,res,debugDraw);
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;
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);
btVector3 color(1, 0, 0);
debugDraw->drawLine(pa, pb, color);
}
#endif//DEBUG_DRAW
#endif //DEBUG_DRAW
}
return res.m_hasResult;
}
btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
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))
};
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;
}

18
src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
View File

@@ -23,18 +23,14 @@ subject to the following restrictions:
class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
{
protected:
static btVector3* getPenetrationDirections();
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
);
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
#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H

1078
src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h
View File

File diff suppressed because it is too large Load Diff

218
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btPersistentManifold.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btSerializer.h"
@@ -24,83 +23,76 @@ subject to the following restrictions:
#define btCollisionObjectData btCollisionObjectFloatData
#endif
btScalar gContactBreakingThreshold = btScalar(0.02);
ContactDestroyedCallback gContactDestroyedCallback = 0;
ContactProcessedCallback gContactProcessedCallback = 0;
ContactStartedCallback gContactStartedCallback = 0;
ContactEndedCallback gContactEndedCallback = 0;
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;
bool gContactCalcArea3Points = true;
btPersistentManifold::btPersistentManifold()
:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
m_body0(0),
m_body1(0),
m_cachedPoints (0),
m_companionIdA(0),
m_companionIdB(0),
m_index1a(0)
: btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
m_body0(0),
m_body1(0),
m_cachedPoints(0),
m_companionIdA(0),
m_companionIdB(0),
m_index1a(0)
{
}
#ifdef DEBUG_PERSISTENCY
#include <stdio.h>
void btPersistentManifold::DebugPersistency()
void btPersistentManifold::DebugPersistency()
{
int i;
printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
for (i=0;i<m_cachedPoints;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);
printf("m_pointCache[%d].m_userPersistentData = %x\n", i, m_pointCache[i].m_userPersistentData);
}
}
#endif //DEBUG_PERSISTENCY
#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++)
for (i = 0; i < m_cachedPoints; i++)
{
if (m_pointCache[i].m_userPersistentData == oldPtr)
{
occurance++;
if (occurance>1)
if (occurance > 1)
printf("error in clearUserCache\n");
}
}
btAssert(occurance<=0);
#endif //DEBUG_PERSISTENCY
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)
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];
btVector3 a[3], b[3];
a[0] = p0 - p1;
a[1] = p0 - p2;
a[2] = p0 - p3;
@@ -113,100 +105,102 @@ static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,c
btVector3 tmp1 = a[1].cross(b[1]);
btVector3 tmp2 = a[2].cross(b[2]);
return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2());
return btMax(btMax(tmp0.length2(), tmp1.length2()), tmp2.length2());
}
int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
//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++)
btScalar maxPenetration = pt.getDistance();
for (int i = 0; i < 4; i++)
{
if (m_pointCache[i].getDistance() < maxPenetration)
{
if (m_pointCache[i].getDistance() < maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = m_pointCache[i].getDistance();
}
maxPenetrationIndex = i;
maxPenetration = m_pointCache[i].getDistance();
}
#endif //KEEP_DEEPEST_POINT
btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
}
#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 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 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 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 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 != 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 != 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 != 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);
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);
btVector4 maxvec(res0, res1, res2, res3);
int biggestarea = maxvec.closestAxis4();
return biggestarea;
}
int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
{
btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
int size = getNumContacts();
int nearestPoint = -1;
for( int i = 0; i < size; i++ )
for (int i = 0; i < size; i++)
{
const btManifoldPoint &mp = m_pointCache[i];
const btManifoldPoint& mp = m_pointCache[i];
btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
btVector3 diffA = mp.m_localPointA - newPoint.m_localPointA;
const btScalar distToManiPoint = diffA.dot(diffA);
if( distToManiPoint < shortestDist )
if (distToManiPoint < shortestDist)
{
shortestDist = distToManiPoint;
nearestPoint = i;
@@ -221,7 +215,7 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool
{
btAssert(validContactDistance(newPoint));
}
int insertIndex = getNumContacts();
if (insertIndex == MANIFOLD_CACHE_SIZE)
{
@@ -232,91 +226,87 @@ int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool
insertIndex = 0;
#endif
clearUserCache(m_pointCache[insertIndex]);
} else
}
else
{
m_cachedPoints++;
}
if (insertIndex<0)
insertIndex=0;
if (insertIndex < 0)
insertIndex = 0;
btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
btAssert(m_pointCache[insertIndex].m_userPersistentData == 0);
m_pointCache[insertIndex] = newPoint;
return insertIndex;
}
btScalar btPersistentManifold::getContactBreakingThreshold() const
btScalar btPersistentManifold::getContactBreakingThreshold() const
{
return m_contactBreakingThreshold;
}
void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
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
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--)
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);
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
/// then
btScalar distance2d;
btVector3 projectedDifference,projectedPoint;
for (i=getNumContacts()-1;i>=0;i--)
btVector3 projectedDifference, projectedPoint;
for (i = getNumContacts() - 1; i >= 0; i--)
{
btManifoldPoint &manifoldPoint = m_pointCache[i];
btManifoldPoint& manifoldPoint = m_pointCache[i];
//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
if (!validContactDistance(manifoldPoint))
{
removeContactPoint(i);
} else
}
else
{
//todo: friction anchor may require the contact to be around a bit longer
//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() )
if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold())
{
removeContactPoint(i);
} else
}
else
{
//contact point processed callback
if (gContactProcessedCallback)
(*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
(*gContactProcessedCallback)(manifoldPoint, (void*)m_body0, (void*)m_body1);
}
}
}
#ifdef DEBUG_PERSISTENCY
DebugPersistency();
#endif //
#endif //
}
int btPersistentManifold::calculateSerializeBufferSize() const
int btPersistentManifold::calculateSerializeBufferSize() const
{
return sizeof(btPersistentManifoldData);
}
const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
{
btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
memset(dataOut, 0, sizeof(btPersistentManifoldData));
@@ -379,7 +369,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleDa
for (int i = 0; i < this->getNumContacts(); i++)
{
btManifoldPoint& pt = m_pointCache[i];
pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
@@ -409,7 +399,6 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleDa
pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
}
}
void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr)
@@ -455,5 +444,4 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatDat
pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
}
}

125
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef BT_PERSISTENT_MANIFOLD_H
#define BT_PERSISTENT_MANIFOLD_H
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
#include "btManifoldPoint.h"
@@ -34,14 +33,14 @@ extern btScalar gContactBreakingThreshold;
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;
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
#endif //SWIG
//the enum starts at 1024 to avoid type conflicts with btTypedConstraint
enum btContactManifoldTypes
@@ -60,73 +59,74 @@ enum btContactManifoldTypes
///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
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;
int m_cachedPoints;
btScalar m_contactBreakingThreshold;
btScalar m_contactProcessingThreshold;
btScalar m_contactBreakingThreshold;
btScalar m_contactProcessingThreshold;
/// sort cached points so most isolated points come first
int sortCachedPoints(const btManifoldPoint& pt);
int sortCachedPoints(const btManifoldPoint& pt);
int findContactPoint(const btManifoldPoint* unUsed, int numUnused,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_companionIdA;
int m_companionIdB;
int m_index1a;
btPersistentManifold();
btPersistentManifold(const btCollisionObject* body0,const btCollisionObject* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold)
btPersistentManifold(const btCollisionObject* body0, const btCollisionObject* body1, int, btScalar contactBreakingThreshold, btScalar contactProcessingThreshold)
: btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
m_body0(body0),m_body1(body1),m_cachedPoints(0),
m_contactBreakingThreshold(contactBreakingThreshold),
m_contactProcessingThreshold(contactProcessingThreshold),
m_companionIdA(0),
m_companionIdB(0),
m_index1a(0)
m_body0(body0),
m_body1(body1),
m_cachedPoints(0),
m_contactBreakingThreshold(contactBreakingThreshold),
m_contactProcessingThreshold(contactProcessingThreshold),
m_companionIdA(0),
m_companionIdB(0),
m_index1a(0)
{
}
SIMD_FORCE_INLINE const btCollisionObject* getBody0() const { return m_body0;}
SIMD_FORCE_INLINE const btCollisionObject* getBody1() const { return m_body1;}
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)
void setBodies(const btCollisionObject* body0, const btCollisionObject* body1)
{
m_body0 = body0;
m_body1 = body1;
}
void clearUserCache(btManifoldPoint& pt);
void clearUserCache(btManifoldPoint & pt);
#ifdef DEBUG_PERSISTENCY
void DebugPersistency();
#endif //
SIMD_FORCE_INLINE int getNumContacts() const { return m_cachedPoints;}
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);
@@ -140,39 +140,36 @@ public:
}
///@todo: get this margin from the current physics / collision environment
btScalar getContactBreakingThreshold() const;
btScalar getContactBreakingThreshold() const;
btScalar getContactProcessingThreshold() const
btScalar getContactProcessingThreshold() const
{
return m_contactProcessingThreshold;
}
void setContactBreakingThreshold(btScalar contactBreakingThreshold)
{
m_contactBreakingThreshold = contactBreakingThreshold;
}
void setContactProcessingThreshold(btScalar contactProcessingThreshold)
void setContactProcessingThreshold(btScalar contactProcessingThreshold)
{
m_contactProcessingThreshold = contactProcessingThreshold;
}
int getCacheEntry(const btManifoldPoint& newPoint) const;
int addManifoldPoint( const btManifoldPoint& newPoint, bool isPredictive=false);
int addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive = false);
void removeContactPoint (int index)
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];
if (index != lastUsedIndex)
{
m_pointCache[index] = m_pointCache[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;
@@ -182,7 +179,7 @@ public:
m_pointCache[lastUsedIndex].m_lifeTime = 0;
}
btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0);
btAssert(m_pointCache[lastUsedIndex].m_userPersistentData == 0);
m_cachedPoints--;
if (gContactEndedCallback && m_cachedPoints == 0)
@@ -243,13 +240,12 @@ public:
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);
void refreshContactPoints(const btTransform& trA, const btTransform& trB);
SIMD_FORCE_INLINE void clearManifold()
SIMD_FORCE_INLINE void clearManifold()
{
int i;
for (i=0;i<m_cachedPoints;i++)
for (i = 0; i < m_cachedPoints; i++)
{
clearUserCache(m_pointCache[i]);
}
@@ -261,15 +257,13 @@ public:
m_cachedPoints = 0;
}
int calculateSerializeBufferSize() const;
const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
int calculateSerializeBufferSize() const;
const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr);
};
// clang-format off
struct btPersistentManifoldDoubleData
{
@@ -362,17 +356,14 @@ struct btPersistentManifoldFloatData
btCollisionObjectFloatData *m_body1;
};
// clang-format on
#ifdef BT_USE_DOUBLE_PRECISION
#define btPersistentManifoldData btPersistentManifoldDoubleData
#define btPersistentManifoldDataName "btPersistentManifoldDoubleData"
#define btPersistentManifoldData btPersistentManifoldDoubleData
#define btPersistentManifoldDataName "btPersistentManifoldDoubleData"
#else
#define btPersistentManifoldData btPersistentManifoldFloatData
#define btPersistentManifoldDataName "btPersistentManifoldFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#define btPersistentManifoldData btPersistentManifoldFloatData
#define btPersistentManifoldDataName "btPersistentManifoldFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#endif //BT_PERSISTENT_MANIFOLD_H
#endif //BT_PERSISTENT_MANIFOLD_H

24
src/BulletCollision/NarrowPhaseCollision/btPointCollector.h
View File

@@ -18,38 +18,33 @@ subject to the following restrictions:
#include "btDiscreteCollisionDetectorInterface.h"
struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
{
btVector3 m_normalOnBInWorld;
btVector3 m_pointInWorld;
btScalar m_distance;//negative means penetration
btScalar m_distance; //negative means penetration
bool m_hasResult;
bool m_hasResult;
btPointCollector ()
: m_distance(btScalar(BT_LARGE_FLOAT)),m_hasResult(false)
btPointCollector()
: m_distance(btScalar(BT_LARGE_FLOAT)), m_hasResult(false)
{
}
virtual void setShapeIdentifiersA(int partId0,int index0)
virtual void setShapeIdentifiersA(int partId0, int index0)
{
(void)partId0;
(void)index0;
}
virtual void setShapeIdentifiersB(int partId1,int index1)
virtual void setShapeIdentifiersB(int partId1, int index1)
{
(void)partId1;
(void)index1;
}
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
{
if (depth< m_distance)
if (depth < m_distance)
{
m_hasResult = true;
m_normalOnBInWorld = normalOnBInWorld;
@@ -60,5 +55,4 @@ struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
}
};
#endif //BT_POINT_COLLECTOR_H
#endif //BT_POINT_COLLECTOR_H

354
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
View File

@@ -13,45 +13,42 @@ subject to the following restrictions:
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
#include <float.h> //for FLT_MAX
int gExpectedNbTests=0;
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)
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 firstVertex = pVtxIn[pVtxIn.size() - 1];
btVector3 endVertex = pVtxIn[0];
ds = planeNormalWS.dot(firstVertex)+planeEqWS;
ds = planeNormalWS.dot(firstVertex) + planeEqWS;
for (ve = 0; ve < numVerts; ve++)
{
endVertex=pVtxIn[ve];
endVertex = pVtxIn[ve];
de = planeNormalWS.dot(endVertex)+planeEqWS;
de = planeNormalWS.dot(endVertex) + planeEqWS;
if (ds<0)
if (ds < 0)
{
if (de<0)
if (de < 0)
{
// Start < 0, end < 0, so output endVertex
ppVtxOut.push_back(endVertex);
@@ -59,15 +56,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
else
{
// Start < 0, end >= 0, so output intersection
ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
ppVtxOut.push_back(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de))));
}
}
else
{
if (de<0)
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(firstVertex.lerp(endVertex, btScalar(ds * 1.f / (ds - de))));
ppVtxOut.push_back(endVertex);
}
}
@@ -76,47 +73,44 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex
}
}
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)
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;
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);
hullA.project(transA, sep_axis, Min0, Max0, witnesPtMinA, witnesPtMaxA);
hullB.project(transB, sep_axis, Min1, Max1, witnesPtMinB, witnesPtMaxB);
if(Max0<Min1 || Max1<Min0)
if (Max0 < Min1 || Max1 < Min0)
return false;
btScalar d0 = Max0 - Min1;
btAssert(d0>=0.0f);
btAssert(d0 >= 0.0f);
btScalar d1 = Max1 - Min0;
btAssert(d1>=0.0f);
if (d0<d1)
btAssert(d1 >= 0.0f);
if (d0 < d1)
{
depth = d0;
witnessPointA = witnesPtMaxA;
witnessPointB = witnesPtMinB;
} else
}
else
{
depth = d1;
witnessPointA = witnesPtMinA;
witnessPointB = witnesPtMaxB;
}
return true;
}
static int gActualSATPairTests=0;
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;
if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false;
return true;
}
@@ -125,9 +119,9 @@ inline bool IsAlmostZero(const btVector3& v)
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);
// 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];
@@ -140,90 +134,94 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
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();
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)
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);
InverseTransformPoint3x3(localAxis0, axis, trans0);
btVector3 localAxis1;
InverseTransformPoint3x3(localAxis1, axis,trans1);
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 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 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)
const btScalar depth = d0 < d1 ? d0 : d1;
if (depth > dmin)
return false;
return true;
}
#endif //TEST_INTERNAL_OBJECTS
#endif //TEST_INTERNAL_OBJECTS
SIMD_FORCE_INLINE void btSegmentsClosestPoints(
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 )
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 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 ) {
if (denom == 0.0f)
{
tA = 0.0f;
} else {
tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
if ( tA < -hlenA )
}
else
{
tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom;
if (tA < -hlenA)
tA = -hlenA;
else if ( tA > hlenA )
else if (tA > hlenA)
tA = hlenA;
}
tB = tA * dirA_dot_dirB - dirB_dot_trans;
if ( tB < -hlenB ) {
if (tB < -hlenB)
{
tB = -hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if ( tA < -hlenA )
if (tA < -hlenA)
tA = -hlenA;
else if ( tA > hlenA )
else if (tA > hlenA)
tA = hlenA;
} else if ( tB > hlenB ) {
}
else if (tB > hlenB)
{
tB = hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if ( tA < -hlenA )
if (tA < -hlenA)
tA = -hlenA;
else if ( tA > hlenA )
else if (tA > hlenA)
tA = hlenA;
}
@@ -235,44 +233,42 @@ void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTrans
ptsVector = translation - offsetA + offsetB;
}
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
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
//#ifdef TEST_INTERNAL_OBJECTS
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
//#endif
//#endif
btScalar dmin = FLT_MAX;
int curPlaneTests=0;
int curPlaneTests = 0;
int numFacesA = hullA.m_faces.size();
// Test normals from hullA
for(int i=0;i<numFacesA;i++)
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;
if (DeltaC2.dot(faceANormalWS) < 0)
faceANormalWS *= -1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
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))
btVector3 wA, wB;
if (!TestSepAxis(hullA, hullB, transA, transB, faceANormalWS, d, wA, wB))
return false;
if(d<dmin)
if (d < dmin)
{
dmin = d;
sep = faceANormalWS;
@@ -281,92 +277,89 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
int numFacesB = hullB.m_faces.size();
// Test normals from hullB
for(int i=0;i<numFacesB;i++)
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;
if (DeltaC2.dot(WorldNormal) < 0)
WorldNormal *= -1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
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))
btVector3 wA, wB;
if (!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d, wA, wB))
return false;
if(d<dmin)
if (d < dmin)
{
dmin = d;
sep = WorldNormal;
}
}
btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
int edgeA=-1;
int edgeB=-1;
btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
int edgeA = -1;
int edgeB = -1;
btVector3 worldEdgeA;
btVector3 worldEdgeB;
btVector3 witnessPointA(0,0,0),witnessPointB(0,0,0);
btVector3 witnessPointA(0, 0, 0), witnessPointB(0, 0, 0);
int curEdgeEdge = 0;
// Test edges
for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
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++)
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))
if (!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
if (DeltaC2.dot(Cross) < 0)
Cross *= -1.f;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
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))
btVector3 wA, wB;
if (!TestSepAxis(hullA, hullB, transA, transB, Cross, dist, wA, wB))
return false;
if(dist<dmin)
if (dist < dmin)
{
dmin = dist;
sep = Cross;
edgeA=e0;
edgeB=e1;
edgeA = e0;
edgeB = e1;
worldEdgeA = WorldEdge0;
worldEdgeB = WorldEdge1;
witnessPointA=wA;
witnessPointB=wB;
witnessPointA = wA;
witnessPointB = wB;
}
}
}
}
if (edgeA>=0&&edgeB>=0)
if (edgeA >= 0 && edgeB >= 0)
{
// printf("edge-edge\n");
// printf("edge-edge\n");
//add an edge-edge contact
btVector3 ptsVector;
@@ -375,57 +368,55 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
btScalar tA;
btScalar tB;
btVector3 translation = witnessPointB-witnessPointA;
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);
btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB,
translation,
dirA, hlenA,
dirB, hlenB);
btScalar nlSqrt = ptsVector.length2();
if (nlSqrt>SIMD_EPSILON)
if (nlSqrt > SIMD_EPSILON)
{
btScalar nl = btSqrt(nlSqrt);
ptsVector *= 1.f/nl;
if (ptsVector.dot(DeltaC2)<0.f)
ptsVector *= 1.f / nl;
if (ptsVector.dot(DeltaC2) < 0.f)
{
ptsVector*=-1.f;
ptsVector *= -1.f;
}
btVector3 ptOnB = witnessPointB + offsetB;
btScalar distance = nl;
resultOut.addContactPoint(ptsVector, ptOnB,-distance);
resultOut.addContactPoint(ptsVector, ptOnB, -distance);
}
}
if((DeltaC2.dot(sep))<0.0f)
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)
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;
int closestFaceA = -1;
{
btScalar dmin = FLT_MAX;
for(int face=0;face<hullA.m_faces.size();face++)
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)
{
@@ -434,69 +425,66 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
}
}
}
if (closestFaceA<0)
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
// 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++)
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& 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 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;
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]);
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 = transA.getBasis() * localPlaneNormal;
btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
#else
btVector3 planeNormalWS = planeNormalWS1;
btScalar planeEqWS=planeEqWS1;
btScalar planeEqWS = planeEqWS1;
#endif
//clip face
clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
btSwap(pVtxIn,pVtxOut);
clipFace(*pVtxIn, *pVtxOut, planeNormalWS, planeEqWS);
btSwap(pVtxIn, pVtxOut);
pVtxOut->resize(0);
}
//#define ONLY_REPORT_DEEPEST_POINT
//#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]);
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 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)
btScalar depth = planeNormalWS.dot(vtx) + planeEqWS;
if (depth <= minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <=maxDist)
if (depth <= maxDist)
{
btVector3 point = pVtxIn->at(i);
#ifdef ONLY_REPORT_DEEPEST_POINT
@@ -507,40 +495,32 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.addContactPoint(separatingNormal,point,depth);
}
#endif
resultOut.addContactPoint(separatingNormal, point, depth);
#endif
}
}
}
#ifdef ONLY_REPORT_DEEPEST_POINT
if (curMaxDist<maxDist)
if (curMaxDist < maxDist)
{
resultOut.addContactPoint(separatingNormal,point,curMaxDist);
resultOut.addContactPoint(separatingNormal, point, curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
#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)
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 c0 = transA * hullA.m_localCenter;
// const btVector3 c1 = transB * hullB.m_localCenter;
//const btVector3 DeltaC2 = c0 - c1;
int closestFaceB=-1;
int closestFaceB = -1;
btScalar dmax = -FLT_MAX;
{
for(int face=0;face<hullB.m_faces.size();face++)
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;
@@ -553,18 +533,16 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
}
}
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);
{
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);
}

17
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
View File

@@ -13,14 +13,11 @@ subject to the following restrictions:
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"
@@ -32,18 +29,14 @@ 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 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 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);
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);
static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS, btScalar planeEqWS);
};
#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H
#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H

135
src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
View File

@@ -23,39 +23,38 @@ subject to the following restrictions:
#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.))
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)
void btTriangleRaycastCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
const btVector3 &vert0=triangle[0];
const btVector3 &vert1=triangle[1];
const btVector3 &vert2=triangle[2];
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 v10;
v10 = vert1 - vert0;
btVector3 v20;
v20 = vert2 - vert0;
btVector3 triangleNormal;
triangleNormal = v10.cross(v20);
btVector3 triangleNormal; triangleNormal = v10.cross( v20 );
const btScalar dist = vert0.dot(triangleNormal);
btScalar dist_a = triangleNormal.dot(m_from) ;
dist_a-= dist;
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) )
if (dist_a * dist_b >= btScalar(0.0))
{
return ; // same sign
return; // same sign
}
if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a <= btScalar(0.0)))
@@ -64,52 +63,52 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId,
return;
}
const btScalar proj_length=dist_a-dist_b;
const btScalar distance = (dist_a)/(proj_length);
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)
if (distance < m_hitFraction)
{
btScalar edge_tolerance =triangleNormal.length2();
btScalar edge_tolerance = triangleNormal.length2();
edge_tolerance *= btScalar(-0.0001);
btVector3 point; point.setInterpolate3( m_from, m_to, distance);
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 );
btVector3 v0p;
v0p = vert0 - point;
btVector3 v1p;
v1p = vert1 - point;
btVector3 cp0;
cp0 = v0p.cross(v1p);
if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance)
if ((btScalar)(cp0.dot(triangleNormal)) >= edge_tolerance)
{
btVector3 v2p; v2p = vert2 - point;
btVector3 v2p;
v2p = vert2 - point;
btVector3 cp1;
cp1 = v1p.cross( v2p);
if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance)
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 ((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);
m_hitFraction = reportHit(-triangleNormal, distance, partId, triangleIndex);
}
else
{
m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
m_hitFraction = reportHit(triangleNormal, distance, partId, triangleIndex);
}
}
}
@@ -118,8 +117,7 @@ void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId,
}
}
btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin)
btTriangleConvexcastCallback::btTriangleConvexcastCallback(const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin)
{
m_convexShape = convexShape;
m_convexShapeFrom = convexShapeFrom;
@@ -130,14 +128,13 @@ btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape*
m_allowedPenetration = 0.f;
}
void
btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex)
void btTriangleConvexcastCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]);
triangleShape.setMargin(m_triangleCollisionMargin);
btTriangleShape triangleShape(triangle[0], triangle[1], triangle[2]);
triangleShape.setMargin(m_triangleCollisionMargin);
btVoronoiSimplexSolver simplexSolver;
btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
btVoronoiSimplexSolver simplexSolver;
btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
//#define USE_SUBSIMPLEX_CONVEX_CAST 1
//if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below
@@ -145,21 +142,21 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId,
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
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))
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 */
/*
/* 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;
@@ -167,11 +164,11 @@ btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId,
*/
castResult.m_normal.normalize();
reportHit (castResult.m_normal,
castResult.m_hitPoint,
castResult.m_fraction,
partId,
triangleIndex);
reportHit(castResult.m_normal,
castResult.m_hitPoint,
castResult.m_fraction,
partId,
triangleIndex);
}
}
}

45
src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
View File

@@ -21,35 +21,33 @@ subject to the following restrictions:
struct btBroadphaseProxy;
class btConvexShape;
class btTriangleRaycastCallback: public btTriangleCallback
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;
//@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;
btScalar m_hitFraction;
btTriangleRaycastCallback(const btVector3& from, const btVector3& to, unsigned int flags = 0);
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;
virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
};
class btTriangleConvexcastCallback : public btTriangleCallback
@@ -63,12 +61,11 @@ public:
btScalar m_triangleCollisionMargin;
btScalar m_allowedPenetration;
btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin);
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 void processTriangle(btVector3* triangle, int partId, int triangleIndex);
virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0;
};
#endif //BT_RAYCAST_TRI_CALLBACK_H
#endif //BT_RAYCAST_TRI_CALLBACK_H

19
src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
View File

@@ -13,8 +13,6 @@ subject to the following restrictions:
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
@@ -31,33 +29,30 @@ subject to the following restrictions:
/// voronoi regions or barycentric coordinates
class btSimplexSolverInterface
{
public:
virtual ~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 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;
virtual int numVertices() const = 0;
};
#endif
#endif //BT_SIMPLEX_SOLVER_INTERFACE_H
#endif //BT_SIMPLEX_SOLVER_INTERFACE_H

110
src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btSubSimplexConvexCast.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
@@ -22,9 +21,10 @@ subject to the following restrictions:
#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)
btSubsimplexConvexCast::btSubsimplexConvexCast(const btConvexShape* convexA, const btConvexShape* convexB, btSimplexSolverInterface* simplexSolver)
: m_simplexSolver(simplexSolver),
m_convexA(convexA),
m_convexB(convexB)
{
}
@@ -35,19 +35,18 @@ m_convexA(convexA),m_convexB(convexB)
#else
#define MAX_ITERATIONS 32
#endif
bool btSubsimplexConvexCast::calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
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();
btVector3 linVelA, linVelB;
linVelA = toA.getOrigin() - fromA.getOrigin();
linVelB = toB.getOrigin() - fromB.getOrigin();
btScalar lambda = btScalar(0.);
@@ -55,43 +54,39 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
btTransform interpolatedTransB = fromB;
///take relative motion
btVector3 r = (linVelA-linVelB);
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;
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.));
n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
btVector3 c;
btScalar dist2 = v.length2();
#ifdef BT_USE_DOUBLE_PRECISION
btScalar epsilon = SIMD_EPSILON * 10;
#else
//todo: epsilon kept for backward compatibility of unit tests.
//will need to digg deeper to make the algorithm more robust
//since, a large epsilon can cause an early termination with false
//positive results (ray intersections that shouldn't be there)
//todo: epsilon kept for backward compatibility of unit tests.
//will need to digg deeper to make the algorithm more robust
//since, a large epsilon can cause an early termination with false
//positive results (ray intersections that shouldn't be there)
btScalar epsilon = btScalar(0.0001);
#endif //BT_USE_DOUBLE_PRECISION
#endif //BT_USE_DOUBLE_PRECISION
btVector3 w,p;
btVector3 w, p;
btScalar VdotR;
while ( (dist2 > epsilon) && maxIter--)
while ((dist2 > epsilon) && maxIter--)
{
supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
w = supVertexA-supVertexB;
supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v * interpolatedTransA.getBasis()));
supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v * interpolatedTransB.getBasis()));
w = supVertexA - supVertexB;
btScalar VdotW = v.dot(w);
@@ -100,68 +95,63 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
return false;
}
if ( VdotW > btScalar(0.))
if (VdotW > btScalar(0.))
{
VdotR = v.dot(r);
if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
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);
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;
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);
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
}
else
{
dist2 = btScalar(0.);
}
}
}
//int numiter = MAX_ITERATIONS - maxIter;
// printf("number of iterations: %d", numiter);
// 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))
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)
if (result.m_normal.dot(r) >= -result.m_allowedPenetration)
return false;
btVector3 hitA,hitB;
m_simplexSolver->compute_points(hitA,hitB);
result.m_hitPoint=hitB;
btVector3 hitA, hitB;
m_simplexSolver->compute_points(hitA, hitB);
result.m_hitPoint = hitB;
return true;
}

23
src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
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
@@ -28,23 +27,21 @@ class btConvexShape;
class btSubsimplexConvexCast : public btConvexCast
{
btSimplexSolverInterface* m_simplexSolver;
const btConvexShape* m_convexA;
const btConvexShape* m_convexB;
const btConvexShape* m_convexA;
const btConvexShape* m_convexB;
public:
btSubsimplexConvexCast (const btConvexShape* shapeA,const btConvexShape* shapeB,btSimplexSolverInterface* simplexSolver);
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);
virtual bool calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result);
};
#endif //BT_SUBSIMPLEX_CONVEX_CAST_H
#endif //BT_SUBSIMPLEX_CONVEX_CAST_H

505
src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
View File

@@ -23,26 +23,24 @@ subject to the following restrictions:
*/
#include "btVoronoiSimplexSolver.h"
#define VERTA 0
#define VERTB 1
#define VERTC 2
#define VERTD 3
#define VERTA 0
#define VERTB 1
#define VERTC 2
#define VERTD 3
#define CATCH_DEGENERATE_TETRAHEDRON 1
void btVoronoiSimplexSolver::removeVertex(int index)
void btVoronoiSimplexSolver::removeVertex(int index)
{
btAssert(m_numVertices>0);
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)
void btVoronoiSimplexSolver::reduceVertices(const btUsageBitfield& usedVerts)
{
if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
removeVertex(3);
@@ -52,29 +50,22 @@ void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
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_lastW = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
m_cachedBC.reset();
}
//add a vertex
//add a vertex
void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q)
{
m_lastW = w;
@@ -87,9 +78,8 @@ void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, c
m_numVertices++;
}
bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
{
if (m_needsUpdate)
{
m_cachedBC.reset();
@@ -98,127 +88,131 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
switch (numVertices())
{
case 0:
case 0:
m_cachedValidClosest = false;
break;
case 1:
case 1:
{
m_cachedP1 = m_simplexPointsP[0];
m_cachedP2 = m_simplexPointsQ[0];
m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[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_cachedBC.setBarycentricCoordinates(btScalar(1.), btScalar(0.), btScalar(0.), btScalar(0.));
m_cachedValidClosest = m_cachedBC.isValid();
break;
};
case 2:
case 2:
{
//closest point origin from line segment
const btVector3& from = m_simplexVectorW[0];
const btVector3& to = m_simplexVectorW[1];
btVector3 nearest;
//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
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 = 0;
//reduce to 1 point
t /= dotVV;
diff -= t * v;
m_cachedBC.m_usedVertices.usedVertexA = true;
m_cachedBC.m_usedVertices.usedVertexB = true;
}
m_cachedBC.setBarycentricCoordinates(1-t,t);
nearest = from + t*v;
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_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;
m_cachedValidClosest = m_cachedBC.isValid();
break;
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:
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.));
btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
const btVector3& a = m_simplexVectorW[0];
const btVector3& b = m_simplexVectorW[1];
const btVector3& c = m_simplexVectorW[2];
const btVector3& d = m_simplexVectorW[3];
bool hasSeparation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
bool hasSeparation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC);
if (hasSeparation)
{
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
m_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_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
m_cachedV = m_cachedP1 - m_cachedP2;
reduceVertices(m_cachedBC.m_usedVertices);
}
else
{
// printf("sub distance got penetration\n");
// printf("sub distance got penetration\n");
if (m_cachedBC.m_degenerate)
{
m_cachedValidClosest = false;
} else
}
else
{
m_cachedValidClosest = true;
//degenerate case == false, penetration = true + zero
m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
m_cachedV.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
}
break;
}
@@ -228,7 +222,7 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
//closest point origin from tetrahedron
break;
}
default:
default:
{
m_cachedValidClosest = false;
}
@@ -236,7 +230,6 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
}
return m_cachedValidClosest;
}
//return/calculate the closest vertex
@@ -247,13 +240,11 @@ bool btVoronoiSimplexSolver::closest(btVector3& v)
return succes;
}
btScalar btVoronoiSimplexSolver::maxVertex()
{
int i, numverts = numVertices();
btScalar maxV = btScalar(0.);
for (i=0;i<numverts;i++)
for (i = 0; i < numverts; i++)
{
btScalar curLen2 = m_simplexVectorW[i].length2();
if (maxV < curLen2)
@@ -262,13 +253,11 @@ btScalar btVoronoiSimplexSolver::maxVertex()
return maxV;
}
//return the current simplex
int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
//return the current simplex
int btVoronoiSimplexSolver::getSimplex(btVector3* pBuf, btVector3* qBuf, btVector3* yBuf) const
{
int i;
for (i=0;i<numVertices();i++)
for (i = 0; i < numVertices(); i++)
{
yBuf[i] = m_simplexVectorW[i];
pBuf[i] = m_simplexPointsP[i];
@@ -277,20 +266,17 @@ int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVecto
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++)
for (i = 0; i < numverts; i++)
{
#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
if (m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
#else
if (m_simplexVectorW[i] == w)
#endif
@@ -303,199 +289,190 @@ bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
//check in case lastW is already removed
if (w == m_lastW)
return true;
return found;
}
void btVoronoiSimplexSolver::backup_closest(btVector3& v)
void btVoronoiSimplexSolver::backup_closest(btVector3& v)
{
v = m_cachedV;
}
bool btVoronoiSimplexSolver::emptySimplex() const
bool btVoronoiSimplexSolver::emptySimplex() const
{
return (numVertices() == 0);
}
void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2)
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)
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))
// 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)
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)
// 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);
result.setBarycentricCoordinates(0, 1, 0);
return true; // b; // barycentric coordinates (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);
// 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);
result.setBarycentricCoordinates(1 - v, v, 0);
return true;
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
//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)
// 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)
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);
// 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);
result.setBarycentricCoordinates(1 - w, 0, w);
return true;
//return a + w * ac; // barycentric coordinates (1-w,0,w)
}
//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));
// 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)
}
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;
// 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);
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
// 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);
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]
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)))
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");//
// printf("affine dependent/degenerate\n");//
return -1;
}
#endif
#endif
// Points on opposite sides if expression signs are opposite
return signp * signd < btScalar(0.);
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)
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
// 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.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 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);
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)
if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
{
closestPtPointTriangle(p, a, b, c,tempResult);
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) {
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!
@@ -504,25 +481,22 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const
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
);
tempResult.m_barycentricCoords[VERTA],
tempResult.m_barycentricCoords[VERTB],
tempResult.m_barycentricCoords[VERTC],
0);
}
}
}
// Repeat test for face acd
if (pointOutsideACD)
if (pointOutsideACD)
{
closestPtPointTriangle(p, a, c, d,tempResult);
closestPtPointTriangle(p, a, c, d, tempResult);
btVector3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
btScalar sqDist = (q - p).dot( q - p);
if (sqDist < bestSqDist)
btScalar sqDist = (q - p).dot(q - p);
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.m_closestPointOnSimplex = q;
@@ -532,52 +506,46 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const
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]
);
tempResult.m_barycentricCoords[VERTA],
0,
tempResult.m_barycentricCoords[VERTB],
tempResult.m_barycentricCoords[VERTC]);
}
}
// Repeat test for face adb
}
// Repeat test for face adb
if (pointOutsideADB)
{
closestPtPointTriangle(p, a, d, b,tempResult);
closestPtPointTriangle(p, a, d, b, tempResult);
btVector3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
btScalar sqDist = (q - p).dot( q - p);
if (sqDist < bestSqDist)
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]
);
tempResult.m_barycentricCoords[VERTA],
tempResult.m_barycentricCoords[VERTC],
0,
tempResult.m_barycentricCoords[VERTB]);
}
}
// Repeat test for face bdc
}
// Repeat test for face bdc
if (pointOutsideBDC)
{
closestPtPointTriangle(p, b, d, c,tempResult);
closestPtPointTriangle(p, b, d, c, tempResult);
btVector3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
btScalar sqDist = (q - p).dot( q - p);
if (sqDist < bestSqDist)
btScalar sqDist = (q - p).dot(q - p);
if (sqDist < bestSqDist)
{
bestSqDist = sqDist;
finalResult.m_closestPointOnSimplex = q;
@@ -588,25 +556,22 @@ bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
finalResult.setBarycentricCoordinates(
0,
tempResult.m_barycentricCoords[VERTA],
tempResult.m_barycentricCoords[VERTC],
tempResult.m_barycentricCoords[VERTB]
);
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)
finalResult.m_usedVertices.usedVertexD)
{
return true;
}
return true;
return true;
}

154
src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
View File

@@ -13,15 +13,11 @@ subject to the following restrictions:
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
@@ -31,9 +27,10 @@ subject to the following restrictions:
#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 1e-12f
#else
#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f
#endif//BT_USE_DOUBLE_PRECISION
#endif //BT_USE_DOUBLE_PRECISION
struct btUsageBitfield{
struct btUsageBitfield
{
btUsageBitfield()
{
reset();
@@ -46,140 +43,131 @@ struct btUsageBitfield{
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;
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
struct btSubSimplexClosestResult
{
btVector3 m_closestPointOnSimplex;
btVector3 m_closestPointOnSimplex;
//MASK for m_usedVertices
//stores the simplex vertex-usage, using the MASK,
//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];
btUsageBitfield m_usedVertices;
btScalar m_barycentricCoords[4];
bool m_degenerate;
void reset()
void reset()
{
m_degenerate = false;
setBarycentricCoordinates();
m_usedVertices.reset();
}
bool isValid()
bool isValid()
{
bool valid = (m_barycentricCoords[0] >= btScalar(0.)) &&
(m_barycentricCoords[1] >= btScalar(0.)) &&
(m_barycentricCoords[2] >= btScalar(0.)) &&
(m_barycentricCoords[3] >= 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.))
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
ATTRIBUTE_ALIGNED16(class)
btVoronoiSimplexSolver
#else
ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver : public btSimplexSolverInterface
ATTRIBUTE_ALIGNED16(class)
btVoronoiSimplexSolver : public btSimplexSolverInterface
#endif
{
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
int m_numVertices;
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_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;
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 m_needsUpdate;
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);
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)
: m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD)
{
}
void reset();
void reset();
void addVertex(const btVector3& w, const btVector3& p, const btVector3& q);
void addVertex(const btVector3& w, const btVector3& p, const btVector3& q);
void setEqualVertexThreshold(btScalar threshold)
{
m_equalVertexThreshold = threshold;
}
void setEqualVertexThreshold(btScalar threshold)
{
m_equalVertexThreshold = threshold;
}
btScalar getEqualVertexThreshold() const
{
return m_equalVertexThreshold;
}
btScalar getEqualVertexThreshold() const
{
return m_equalVertexThreshold;
}
bool closest(btVector3& v);
bool closest(btVector3 & v);
btScalar maxVertex();
btScalar maxVertex();
bool fullSimplex() const
{
return (m_numVertices == 4);
}
bool fullSimplex() const
{
return (m_numVertices == 4);
}
int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const;
int getSimplex(btVector3 * pBuf, btVector3 * qBuf, btVector3 * yBuf) const;
bool inSimplex(const btVector3& w);
void backup_closest(btVector3& v) ;
bool inSimplex(const btVector3& w);
bool emptySimplex() const ;
void backup_closest(btVector3 & v);
void compute_points(btVector3& p1, btVector3& p2) ;
int numVertices() const
{
return m_numVertices;
}
bool emptySimplex() const;
void compute_points(btVector3 & p1, btVector3 & p2);
int numVertices() const
{
return m_numVertices;
}
};
#endif //BT_VORONOI_SIMPLEX_SOLVER_H
#endif //BT_VORONOI_SIMPLEX_SOLVER_H