public static float ProjectOrigin(ref IndexedVector3 a,
ref IndexedVector3 b,
ref IndexedVector3 c,
ref IndexedVector3 d,
ref IndexedVector4 w, ref uint m)
{
//uint[] imd3 ={1,2,0};
//IndexedVector3[] vt = {a,b,c,d};
//IndexedVector3[] dl= {a-d,b-d,c-d};
Debug.Assert(inhere1 == false);
inhere1 = true;
vta[0] = a; vta[1] = b; vta[2] = c; vta[3] = d;
dla[0] = a - d; dla[1] = b - d; dla[2] = c - d;
float vl = Det(dl[0], dl[1], dl[2]);
bool ng = (vl * IndexedVector3.Dot(a, IndexedVector3.Cross(b - c, a - b))) <= 0;
if (ng && (Math.Abs(vl) > GjkEpaSolver2.GJK_SIMPLEX4_EPS))
{
float mindist = -1;
IndexedVector4 subw = new IndexedVector4();
uint subm = 0;
for (int i = 0; i < 3; ++i)
{
uint j = imd3[i];
float s = vl * IndexedVector3.Dot(d, IndexedVector3.Cross(dl[i], dl[j]));
if (s > 0)
{
float subd = GJK.ProjectOrigin(ref vt[i], ref vt[j], ref d, ref subw, ref subm);
if ((mindist < 0) || (subd < mindist))
{
mindist = subd;
m = (uint)((((subm & 1) != 0)?1 << i:0) +
(((subm & 2) != 0)?1 << (int)j:0) +
(((subm & 4) != 0)?8:0));
w[(int)i] = subw.X;
w[(int)j] = subw.Y;
w[(int)imd3[j]] = 0f;
w.W = subw.Z;
}
}
}
if (mindist < 0)
{
mindist = 0;
m = 15;
w.X = Det(c, b, d) / vl;
w.Y = Det(a, c, d) / vl;
w.Z = Det(b, a, d) / vl;
w.W = 1 - (w.X + w.Y + w.Z);
}
inhere1 = false;
return(mindist);
}
inhere1 = false;
return(-1);
}
public static float ProjectOrigin(ref IndexedVector3 a,
ref IndexedVector3 b,
ref IndexedVector3 c,
ref IndexedVector4 w, ref uint m)
{
Debug.Assert(inhere2 == false);
inhere2 = true;
vt[0] = a; vt[1] = b; vt[2] = c;
dl[0] = a - b; dl[1] = b - c; dl[2] = c - a;
IndexedVector3 n = IndexedVector3.Cross(dl[0], dl[1]);
float l = n.LengthSquared();
if (l > GjkEpaSolver2.GJK_SIMPLEX3_EPS)
{
float mindist = -1f;
IndexedVector4 subw = new IndexedVector4();
uint subm = 0;
for (int i = 0; i < 3; ++i)
{
if (IndexedVector3.Dot(vt[i], IndexedVector3.Cross(dl[i], n)) > 0)
{
uint j = imd3[i];
float subd = GJK.ProjectOrigin(ref vt[i], ref vt[j], ref subw, ref subm);
if ((mindist < 0) || (subd < mindist))
{
mindist = subd;
m = (uint)((((subm & 1) != 0)?1 << i:0) + (((subm & 2) != 0)?1 << (int)j:0));
w[(int)i] = subw.X;
w[(int)j] = subw.Y;
w[(int)imd3[j]] = 0f;
}
}
}
if (mindist < 0)
{
float d = IndexedVector3.Dot(ref a, ref n);
float s = (float)Math.Sqrt(l);
IndexedVector3 p = n * (d / l);
mindist = p.LengthSquared();
m = 7;
w.X = (IndexedVector3.Cross(dl[1], b - p)).Length() / s;
w.Y = (IndexedVector3.Cross(dl[2], c - p)).Length() / s;
w.Z = 1 - (w.X + w.Y);
}
inhere2 = false;
return(mindist);
}
inhere2 = false;
return(-1);
}
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);
}