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);
}
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 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);
}
//
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 GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref IndexedVector3 guess)
{
uint iterations = 0;
float sqdist = 0f;
float alpha = 0f;
uint clastw = 0;
/* Initialize solver */
m_free[0] = m_store[0];
m_free[1] = m_store[1];
m_free[2] = m_store[2];
m_free[3] = m_store[3];
m_nfree = 4;
m_current = 0;
m_status = GJKStatus.Valid;
m_shape = shapearg;
m_distance = 0f;
/* Initialize simplex */
m_simplices[0].rank = 0;
m_ray = guess;
float sqrl = m_ray.LengthSquared();
IndexedVector3 temp = sqrl > 0?-m_ray:new IndexedVector3(1, 0, 0);
AppendVertice(m_simplices[0], ref temp);
m_simplices[0].p[0] = 1;
m_ray = m_simplices[0].c[0].w;
sqdist = sqrl;
lastw[0] = lastw[1] = lastw[2] = lastw[3] = m_ray;
/* Loop */
do
{
uint next = 1 - m_current;
sSimplex cs = m_simplices[m_current];
sSimplex ns = m_simplices[next];
/* Check zero */
float rl = m_ray.Length();
if (rl < GjkEpaSolver2.GJK_MIN_DISTANCE)
{/* Touching or inside */
m_status = GJKStatus.Inside;
break;
}
/* Append new vertice in -'v' direction */
IndexedVector3 temp2 = -m_ray;
AppendVertice(cs, ref temp2);
IndexedVector3 w = cs.c[cs.rank - 1].w;
bool found = false;
for (int i = 0; i < 4; ++i)
{
if ((w - lastw[i]).LengthSquared() < GjkEpaSolver2.GJK_DUPLICATED_EPS)
{
found = true;
break;
}
}
if (found)
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
else
{/* Update lastw */
lastw[clastw = (clastw + 1) & 3] = w;
}
/* Check for termination */
float omega = IndexedVector3.Dot(ref m_ray, ref w) / rl;
alpha = Math.Max(omega, alpha);
if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0)
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
/* Reduce simplex */
IndexedVector4 weights = new IndexedVector4();
uint mask = 0;
switch (cs.rank)
{
case 2:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref weights, ref mask);
break;
}
case 3:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref weights, ref mask);
break;
}
case 4:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, ref weights, ref mask);
break;
}
}
if (sqdist >= 0)
{/* Valid */
ns.rank = 0;
m_ray = IndexedVector3.Zero;
m_current = next;
for (uint i = 0, ni = cs.rank; i < ni; ++i)
{
if ((mask & (1 << (int)i)) != 0)
{
ns.c[ns.rank] = cs.c[i];
float weight = weights[(int)i];
ns.p[ns.rank++] = weight;
m_ray += cs.c[i].w * weight;
}
else
{
m_free[m_nfree++] = cs.c[i];
}
}
if (mask == 15)
{
m_status = GJKStatus.Inside;
}
}
else
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
m_status = ((++iterations) < GjkEpaSolver2.GJK_MAX_ITERATIONS) ? m_status : GJKStatus.Failed;
} while(m_status == GJKStatus.Valid);
m_simplex = m_simplices[m_current];
switch (m_status)
{
case GJKStatus.Valid:
{
m_distance = m_ray.Length();
break;
}
case GJKStatus.Inside:
{
m_distance = 0;
break;
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("gjk eval dist[{0}]", m_distance));
}
#endif
return(m_status);
}
public GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref IndexedVector3 guess)
{
uint iterations=0;
float sqdist=0f;
float alpha=0f;
uint clastw=0;
/* Initialize solver */
m_free[0] = m_store[0];
m_free[1] = m_store[1];
m_free[2] = m_store[2];
m_free[3] = m_store[3];
m_nfree = 4;
m_current = 0;
m_status = GJKStatus.Valid;
m_shape = shapearg;
m_distance = 0f;
/* Initialize simplex */
m_simplices[0].rank = 0;
m_ray = guess;
float sqrl= m_ray.LengthSquared();
IndexedVector3 temp = sqrl>0?-m_ray:new IndexedVector3(1,0,0);
AppendVertice(m_simplices[0],ref temp);
m_simplices[0].p[0] = 1;
m_ray = m_simplices[0].c[0].w;
sqdist = sqrl;
lastw[0]=lastw[1]=lastw[2]=lastw[3] = m_ray;
/* Loop */
do
{
uint next = 1-m_current;
sSimplex cs = m_simplices[m_current];
sSimplex ns = m_simplices[next];
/* Check zero */
float rl=m_ray.Length();
if (rl < GjkEpaSolver2.GJK_MIN_DISTANCE)
{/* Touching or inside */
m_status=GJKStatus.Inside;
break;
}
/* Append new vertice in -'v' direction */
IndexedVector3 temp2 = -m_ray;
AppendVertice(cs,ref temp2);
IndexedVector3 w = cs.c[cs.rank-1].w;
bool found = false;
for(int i=0;i<4;++i)
{
if ((w - lastw[i]).LengthSquared() < GjkEpaSolver2.GJK_DUPLICATED_EPS)
{
found=true;
break;
}
}
if(found)
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
else
{/* Update lastw */
lastw[clastw=(clastw+1)&3]=w;
}
/* Check for termination */
float omega=IndexedVector3.Dot(ref m_ray,ref w)/rl;
alpha=Math.Max(omega,alpha);
if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0)
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
/* Reduce simplex */
IndexedVector4 weights = new IndexedVector4();
uint mask=0;
switch(cs.rank)
{
case 2 :
{
sqdist=GJK.ProjectOrigin(ref cs.c[0].w,ref cs.c[1].w,ref weights,ref mask);
break;
}
case 3:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref weights, ref mask);
break;
}
case 4:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, ref weights, ref mask);
break;
}
}
if(sqdist>=0)
{/* Valid */
ns.rank = 0;
m_ray = IndexedVector3.Zero;
m_current = next;
for(uint i=0,ni=cs.rank;i<ni;++i)
{
if((mask&(1<<(int)i)) != 0)
{
ns.c[ns.rank] = cs.c[i];
float weight = weights[(int)i];
ns.p[ns.rank++] = weight;
m_ray += cs.c[i].w * weight;
}
else
{
m_free[m_nfree++] = cs.c[i];
}
}
if(mask==15)
{
m_status=GJKStatus.Inside;
}
}
else
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
m_status = ((++iterations) < GjkEpaSolver2.GJK_MAX_ITERATIONS) ? m_status : GJKStatus.Failed;
} while(m_status==GJKStatus.Valid);
m_simplex = m_simplices[m_current];
switch(m_status)
{
case GJKStatus.Valid:
{
m_distance=m_ray.Length();
break;
}
case GJKStatus.Inside:
{
m_distance=0;
break;
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("gjk eval dist[{0}]", m_distance));
}
#endif
return(m_status);
}
