public static void Initialize(ConvexShape shape0, ref IndexedMatrix wtrs0,
ConvexShape shape1, ref IndexedMatrix wtrs1,
ref GjkEpaSolver2Results results,
GjkEpaSolver2MinkowskiDiff shapeR,
bool withmargins)
{
/* Results */
results.witnesses0 = IndexedVector3.Zero;
results.witnesses1 = IndexedVector3.Zero;
results.status = GjkEpaSolver2Status.Separated;
/* Shape */
shapeR.m_shapes[0] = shape0;
shapeR.m_shapes[1] = shape1;
shapeR.m_toshape1 = wtrs1._basis.TransposeTimes(ref wtrs0._basis);
shapeR.m_toshape0 = wtrs0.InverseTimes(ref wtrs1);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape0", shapeR.m_toshape0);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS0", wtrs0);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS1", wtrs1);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape1", shapeR.m_toshape1);
}
#endif
shapeR.EnableMargin(withmargins);
}
} // for pool
public virtual bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
ref IndexedVector3 v, ref IndexedVector3 wWitnessOnA, ref IndexedVector3 wWitnessOnB, IDebugDraw debugDraw)
{
//float radialmargin = 0f;
IndexedVector3 guessVector = (transA._origin - transB._origin);
GjkEpaSolver2Results results = new GjkEpaSolver2Results();
if (GjkEpaSolver2.Penetration(convexA, ref transA,
convexB, ref transB,
ref guessVector, ref results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return(true);
}
else
{
if (GjkEpaSolver2.Distance(convexA, ref transA, convexB, ref transB, ref guessVector, ref results))
{
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return(false);
}
}
return(false);
}
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 bool SignedDistance(ConvexShape shape0, ref IndexedMatrix wtrs0, ConvexShape shape1, ref IndexedMatrix wtrs1, ref IndexedVector3 guess, ref GjkEpaSolver2Results results)
{
if (!Distance(shape0, ref wtrs0, shape1, ref wtrs1, ref guess, ref results))
{
return(Penetration(shape0, ref wtrs0, shape1, ref wtrs1, ref guess, ref results, false));
}
else
{
return(true);
}
}
//
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);
}
public static bool Penetration(ConvexShape shape0, ref IndexedMatrix wtrs0, ConvexShape shape1, ref IndexedMatrix wtrs1, ref IndexedVector3 guess, ref GjkEpaSolver2Results results)
{
return(Penetration(shape0, ref wtrs0, shape1, ref wtrs1, ref guess, ref results, true));
}
