summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp')
-rw-r--r--thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp253
1 files changed, 114 insertions, 139 deletions
diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
index fa45f49037..a77449812f 100644
--- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
+++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
@@ -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;
}
-
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-21 15:04:05 +0000