public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d();
float minProj = float.MaxValue;
IndexedVector3 minNorm = IndexedVector3.Zero;
IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero;
IndexedVector3 seperatingAxisInA, seperatingAxisInB;
IndexedVector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IndexedVector4[] supportVerticesABatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector4[] supportVerticesBBatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis);
//seperatingAxisInABatch[i] = (-norm) * transA._basis;
//seperatingAxisInBBatch[i] = norm * transB._basis;
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transA._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA);
IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
IndexedVector3.Subtract(out w, ref qWorld, ref pWorld);
float delta = IndexedVector3.Dot(ref norm, ref w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm = IndexedVector3.Zero;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA);
seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB);
pInA = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = IndexedVector3.Transform(pInA, transA);
qWorld = IndexedVector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = IndexedVector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm * convexA.GetMarginNonVirtual();
minB -= minNorm * convexB.GetMarginNonVirtual();
//no penetration
if (minProj < 0f)
{
return(false);
}
float extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());
#if DEBUG_DRAW
if (debugDraw)
{
IndexedVector3 color = new IndexedVector3(0, 1, 0);
debugDraw.drawLine(minA, minB, color);
color = new IndexedVector3(1, 1, 1);
IndexedVector3 vec = minB - minA;
float prj2 = IndexedVector3.Dot(minNorm, vec);
debugDraw.drawLine(minA, minA + (minNorm * minProj), color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get();
gjkdet.Initialize(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
IndexedVector3 offset = minNorm * offsetDist;
ClosestPointInput input = ClosestPointInput.Default();
IndexedVector3 newOrg = transA._origin + offset;
IndexedMatrix displacedTrans = transA;
displacedTrans._origin = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(ref input, res, debugDraw, false);
float correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1f;
minNorm *= penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#if DEBUG_DRAW
if (debugDraw != null)
{
IndexedVector3 color = new IndexedVector3(1, 0, 0);
debugDraw.drawLine(pa, pb, color);
}
#endif//DEBUG_DRAW
}
BulletGlobals.GjkPairDetectorPool.Free(gjkdet);
return(res.m_hasResult);
}
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d();
float minProj = float.MaxValue;
IndexedVector3 minNorm = IndexedVector3.Zero;
IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero;
IndexedVector3 seperatingAxisInA, seperatingAxisInB;
IndexedVector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IndexedVector4[] supportVerticesABatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector4[] supportVerticesBBatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis);
//seperatingAxisInABatch[i] = (-norm) * transA._basis;
//seperatingAxisInBBatch[i] = norm * transB._basis;
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm ,ref transA._basis ,ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm,ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections] ,ref norm ,ref transB._basis);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections],ref negNorm,ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections],ref norm ,ref transB._basis);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA);
IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
IndexedVector3.Subtract(out w, ref qWorld, ref pWorld);
float delta = IndexedVector3.Dot(ref norm, ref w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i=0;i<numPDA;i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm,transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i=0;i<numPDB;i++)
{
IndexedVector3 norm = IndexedVector3.Zero;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm,transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i=0;i<numSampleDirections;i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA);
seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB);
pInA = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = IndexedVector3.Transform(pInA, transA);
qWorld = IndexedVector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = IndexedVector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm * convexA.GetMarginNonVirtual();
minB -= minNorm * convexB.GetMarginNonVirtual();
//no penetration
if (minProj < 0f)
{
return false;
}
float extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());
#if DEBUG_DRAW
if (debugDraw)
{
IndexedVector3 color = new IndexedVector3(0,1,0);
debugDraw.drawLine(minA,minB,color);
color = new IndexedVector3(1,1,1);
IndexedVector3 vec = minB-minA;
float prj2 = IndexedVector3.Dot(minNorm,vec);
debugDraw.drawLine(minA,minA+(minNorm*minProj),color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get();
gjkdet.Initialize(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
IndexedVector3 offset = minNorm * offsetDist;
ClosestPointInput input = ClosestPointInput.Default();
IndexedVector3 newOrg = transA._origin + offset;
IndexedMatrix displacedTrans = transA;
displacedTrans._origin = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(ref input, res, debugDraw, false);
float correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1f;
minNorm *= penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#if DEBUG_DRAW
if (debugDraw != null)
{
IndexedVector3 color = new IndexedVector3(1,0,0);
debugDraw.drawLine(pa,pb,color);
}
#endif//DEBUG_DRAW
}
BulletGlobals.GjkPairDetectorPool.Free(gjkdet);
return res.m_hasResult;
}
