public static bool Penetration(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results, bool usemargins)
{
tShape shape=new MinkowskiDiff();
Initialize(shape0,wtrs0,shape1,wtrs1,ref results,ref shape,usemargins);
using (GJK gjk = GJK.CreateFromPool())
{
GJK.eStatus gjk_status = gjk.Evaluate(shape, -guess);
switch (gjk_status)
{
case GJK.eStatus.Inside:
{
using(EPA epa = EPA.CreateFromPool())
{
EPA.eStatus epa_status = epa.Evaluate(gjk, -guess);
if (epa_status != EPA.eStatus.Failed)
{
btVector3 w0 = new btVector3(0, 0, 0);
for (U i = 0; i < epa.m_result.rank; ++i)
{
#region w0 += shape.Support(epa.m_result.c[i].d, 0) * epa.m_result.p[i];
{
btVector3 temp1, temp2;
shape.Support(ref epa.m_result.c[i].d, 0, out temp1);
btVector3.Multiply(ref temp1, epa.m_result.p[i], out temp2);
w0.Add(ref temp2);
}
#endregion
}
results.status = sResults.eStatus.Penetrating;
results.witnesses0 = wtrs0 * w0;
results.witnesses1 = wtrs0 * (w0 - epa.m_normal * epa.m_depth);
results.normal = -epa.m_normal;
results.distance = -epa.m_depth;
return (true);
}
else results.status = sResults.eStatus.EPA_Failed;
}
}
break;
case GJK.eStatus.Failed:
results.status = sResults.eStatus.GJK_Failed;
break;
default:
{
break;
}
}
return (false);
}
}
public static bool Distance(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results)
{
tShape shape = new MinkowskiDiff();
Initialize(shape0, wtrs0, shape1, wtrs1, ref results, ref shape, false);
using(GJK gjk = GJK.CreateFromPool())
{
GJK.eStatus gjk_status = gjk.Evaluate(shape, guess);
if (gjk_status == GJK.eStatus.Valid)
{
btVector3 w0 = new btVector3(0, 0, 0);
btVector3 w1 = new btVector3(0, 0, 0);
for (U i = 0; i < gjk.m_simplex.rank; ++i)
{
float p = gjk.m_simplex.p[i];
btVector3 temp,temp2,temp3;
#region w0 += shape.Support(gjk.m_simplex.c[i].d, 0) * p;
shape.Support(ref gjk.m_simplex.c[i].d, 0, out temp);
btVector3.Multiply(ref temp, p, out temp2);
w0.Add(ref temp2);
#endregion
#region w1 += shape.Support(-gjk.m_simplex.c[i].d, 1) * p;
btVector3.Minus(ref gjk.m_simplex.c[i].d, out temp3);
shape.Support(ref temp3, 1, out temp);
btVector3.Multiply(ref temp, p, out temp2);
w1.Add(ref temp2);
#endregion
}
results.witnesses0 = wtrs0 * w0;
results.witnesses1 = wtrs0 * w1;
results.normal = w0 - w1;
results.distance = results.normal.Length;
results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
return (true);
}
else
{
results.status = gjk_status == GJK.eStatus.Inside ?
sResults.eStatus.Penetrating :
sResults.eStatus.GJK_Failed;
return (false);
}
}
}