public static bool Distance(ConvexShape shape0, ref IndexedMatrix wtrs0, ConvexShape shape1, ref IndexedMatrix wtrs1, ref IndexedVector3 guess, ref GjkEpaSolver2Results results)
{
using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
using (GJK gjk = BulletGlobals.GJKPool.Get())
{
Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, false);
gjk.Initialise();
GJKStatus gjk_status = gjk.Evaluate(shape, ref guess);
if (gjk_status == GJKStatus.Valid)
{
IndexedVector3 w0 = IndexedVector3.Zero;
IndexedVector3 w1 = IndexedVector3.Zero;
for (uint 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;
IndexedVector3 temp = -gjk.m_simplex.c[i].d;
w1 += shape.Support(ref temp, 1) * p;
}
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
{
//GjkEpaSolver2Status
results.status = (gjk_status == GJKStatus.Inside) ? GjkEpaSolver2Status.Penetrating : GjkEpaSolver2Status.GJK_Failed;
return(false);
}
}
}
//
public float SignedDistance(ref IndexedVector3 position, float margin, ConvexShape shape0, ref IndexedMatrix wtrs0, ref GjkEpaSolver2Results results)
{
using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
using (GJK gjk = BulletGlobals.GJKPool.Get())
{
SphereShape shape1 = BulletGlobals.SphereShapePool.Get();
shape1.Initialize(margin);
IndexedMatrix wtrs1 = IndexedMatrix.CreateFromQuaternion(IndexedQuaternion.Identity);
wtrs0._origin = position;
Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, false);
gjk.Initialise();
IndexedVector3 guess = new IndexedVector3(1);
GJKStatus gjk_status = gjk.Evaluate(shape, ref guess);
if (gjk_status == GJKStatus.Valid)
{
IndexedVector3 w0 = IndexedVector3.Zero;
IndexedVector3 w1 = IndexedVector3.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;
IndexedVector3 temp = -gjk.m_simplex.c[i].d;
w1 += shape.Support(ref temp, 1) * p;
}
results.witnesses0 = wtrs0 * w0;
results.witnesses1 = wtrs0 * w1;
IndexedVector3 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, ref results))
{
IndexedVector3 delta = results.witnesses0 - results.witnesses1;
float length = delta.Length();
if (length >= MathUtil.SIMD_EPSILON)
{
results.normal = delta / length;
}
return(-length);
}
}
}
BulletGlobals.SphereShapePool.Free(shape1);
}
return(MathUtil.SIMD_INFINITY);
}
public static bool Penetration(ConvexShape shape0, ref IndexedMatrix wtrs0, ConvexShape shape1, ref IndexedMatrix wtrs1, ref IndexedVector3 guess, ref GjkEpaSolver2Results results, bool usemargins)
{
using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
using (GJK gjk = BulletGlobals.GJKPool.Get())
{
Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, usemargins);
gjk.Initialise();
IndexedVector3 minusGuess = -guess;
GJKStatus gjk_status = gjk.Evaluate(shape, ref minusGuess);
switch (gjk_status)
{
case GJKStatus.Inside:
{
//EPA epa = new EPA();
eStatus epa_status = epa.Evaluate(gjk, ref minusGuess);
if (epa_status != eStatus.Failed)
{
IndexedVector3 w0 = IndexedVector3.Zero;
for (uint i = 0; i < epa.m_result.rank; ++i)
{
// order of results here is 'different' , EPA.evaluate.
w0 += shape.Support(ref epa.m_result.c[i].d, 0) * epa.m_result.p[i];
}
results.status = GjkEpaSolver2Status.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 = GjkEpaSolver2Status.EPA_Failed;
}
}
break;
case GJKStatus.Failed:
results.status = GjkEpaSolver2Status.GJK_Failed;
break;
}
}
return(false);
}