public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref Matrix transA, ref Matrix transB,
ref Vector3 v, ref Vector3 pa, ref Vector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2D() && convexB.IsConvex2D();
float minProj = float.MaxValue;
Vector3 minNorm = Vector3.Zero;
Vector3 minA = Vector3.Zero, minB = Vector3.Zero;
Vector3 seperatingAxisInA, seperatingAxisInB;
Vector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IList<Vector4> supportVerticesABatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector4> supportVerticesBBatch = new ObjectArray<Vector4>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector3> seperatingAxisInABatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
IList<Vector3> seperatingAxisInBBatch = new ObjectArray<Vector3>(NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2);
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = sPenetrationDirections[i];
seperatingAxisInABatch[i] = MathUtil.TransposeTransformNormal(-norm, transA);
seperatingAxisInBBatch[i] = MathUtil.TransposeTransformNormal(norm, transB);
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
Vector3 norm = Vector3.Up;
convexA.GetPreferredPenetrationDirection(i, ref norm);
norm = Vector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.Transform(norm, transB);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
Vector3 norm = Vector3.Up;
convexB.GetPreferredPenetrationDirection(i, ref norm);
norm = Vector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal(-norm, transA);
seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transB);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
Vector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new Vector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new Vector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
pWorld = Vector3.Transform(pInA, transA);
qWorld = Vector3.Transform(qInB, transB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
w = qWorld - pWorld;
float delta = Vector3.Dot(norm, 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++)
{
Vector3 norm = Vector3.Zero;
convexA.getPreferredPenetrationDirection(i,ref norm);
norm = Vector3.TransformNormal(norm,transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.getNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i=0;i<numPDB;i++)
{
Vector3 norm = Vector3.Zero;
convexB.getPreferredPenetrationDirection(i,ref norm);
norm = Vector3.TransformNormal(norm,transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i=0;i<numSampleDirections;i++)
{
Vector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = Vector3.TransformNormal(-norm, transA);
seperatingAxisInB = Vector3.TransformNormal(norm, transB);
pInA = convexA.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.localGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = Vector3.Transform(pInA, transA);
qWorld = Vector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = Vector3.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)
{
Vector3 color = new Vector3(0,1,0);
debugDraw.drawLine(minA,minB,color);
color = new Vector3(1,1,1);
Vector3 vec = minB-minA;
float prj2 = Vector3.Dot(minNorm,vec);
debugDraw.drawLine(minA,minA+(minNorm*minProj),color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
Vector3 offset = minNorm * offsetDist;
ClosestPointInput input = new ClosestPointInput();
Vector3 newOrg = transA.Translation + offset;
Matrix displacedTrans = transA;
displacedTrans.Translation = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
Vector3 temp = -minNorm;
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(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)
{
Vector3 color = new Vector3(1,0,0);
debugDraw.drawLine(pa,pb,color);
}
#endif//DEBUG_DRAW
}
return res.m_hasResult;
}
//
public float SignedDistance(ref Vector3 position, float margin, ConvexShape shape0, ref Matrix wtrs0, GjkEpaSolver2Results results)
{
GjkEpaSolver2MinkowskiDiff shape = new GjkEpaSolver2MinkowskiDiff();
SphereShape shape1 = new SphereShape(margin);
Matrix wtrs1 = Matrix.CreateFromQuaternion(Quaternion.Identity);
wtrs0.Translation = position;
Initialize(shape0,ref wtrs0,shape1,ref wtrs1,results,shape,false);
GJK gjk = new GJK();
Vector3 guess = new Vector3(1,1,1);
GJKStatus gjk_status=gjk.Evaluate(shape,ref guess);
if(gjk_status==GJKStatus.Valid)
{
Vector3 w0=Vector3.Zero;
Vector3 w1=Vector3.Zero;
for(int i=0;i<gjk.m_simplex.rank;++i)
{
float p=gjk.m_simplex.p[i];
w0+=shape.Support( ref gjk.m_simplex.c[i].d,0)*p;
Vector3 temp = -gjk.m_simplex.c[i].d;
w1+=shape.Support(ref temp,1)*p;
}
results.witnesses0 = Vector3.Transform(w0,wtrs0);
results.witnesses1 = Vector3.Transform(w1,wtrs0);
Vector3 delta= results.witnesses1-results.witnesses0;
float margin2 = shape0.GetMarginNonVirtual()+shape1.GetMarginNonVirtual();
float length = delta.Length();
results.normal = delta/length;
results.witnesses0 += results.normal*margin2;
return(length-margin2);
}
else
{
if(gjk_status==GJKStatus.Inside)
{
if(Penetration(shape0,ref wtrs0,shape1,ref wtrs1,ref gjk.m_ray,results))
{
Vector3 delta= results.witnesses0-results.witnesses1;
float length= delta.Length();
if (length >= MathUtil.SIMD_EPSILON)
results.normal = delta/length;
return(-length);
}
}
}
return(MathUtil.SIMD_INFINITY);
}