public static bool Distance(ConvexShape shape0,ref Matrix wtrs0,ConvexShape shape1,ref Matrix wtrs1,ref Vector3 guess,GjkEpaSolver2Results results)
{
GjkEpaSolver2MinkowskiDiff shape = new GjkEpaSolver2MinkowskiDiff();
Initialize(shape0,ref wtrs0,shape1,ref wtrs1,results,shape,false);
GJK gjk = new GJK();
GJKStatus gjk_status= gjk.Evaluate(shape,ref guess);
if(gjk_status == GJKStatus.Valid)
{
Vector3 w0 = Vector3.Zero;
Vector3 w1 = Vector3.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;
Vector3 temp = -gjk.m_simplex.c[i].d;
w1+=shape.Support(ref temp,1)*p;
}
results.witnesses0 = Vector3.Transform(w0,wtrs0);
results.witnesses1 = Vector3.Transform(w1,wtrs0);
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 Matrix wtrs0,
ConvexShape shape1,ref Matrix wtrs1,
GjkEpaSolver2Results results,
GjkEpaSolver2MinkowskiDiff shapeR,
bool withmargins)
{
/* Results */
results.witnesses0 = Vector3.Zero;
results.witnesses1 = Vector3.Zero;
results.status = GjkEpaSolver2Status.Separated;
/* Shape */
shapeR.m_shapes[0] = shape0;
shapeR.m_shapes[1] = shape1;
shapeR.m_toshape1 = MathUtil.TransposeTimesBasis(ref wtrs1,ref wtrs0);
shapeR.m_toshape0 = MathUtil.InverseTimes(ref wtrs0, ref wtrs1);
if (BulletGlobals.g_streamWriter != null && debugGJK)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape0", shapeR.m_toshape0);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape1", shapeR.m_toshape1);
}
shapeR.EnableMargin(withmargins);
}
public static bool Penetration(ConvexShape shape0,ref Matrix wtrs0,ConvexShape shape1,ref Matrix wtrs1,ref Vector3 guess,GjkEpaSolver2Results results,bool usemargins)
{
GjkEpaSolver2MinkowskiDiff shape = new GjkEpaSolver2MinkowskiDiff();
Initialize(shape0,ref wtrs0,shape1,ref wtrs1,results, shape,usemargins);
GJK gjk = new GJK();
Vector3 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)
{
Vector3 w0 = Vector3.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 = Vector3.Transform(w0,wtrs0);
results.witnesses1 = Vector3.Transform((w0-epa.m_normal*epa.m_depth),wtrs0);
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 float SignedDistance(ref Vector3 position, float margin, ConvexShape shape0, ref Matrix wtrs0, GjkEpaSolver2Results results)
{
GjkEpaSolver2MinkowskiDiff shape = new GjkEpaSolver2MinkowskiDiff();
SphereShape shape1 = new SphereShape(margin);
Matrix wtrs1 = Matrix.CreateFromQuaternion(Quaternion.Identity);
wtrs0.Translation = position;
Initialize(shape0,ref wtrs0,shape1,ref wtrs1,results,shape,false);
GJK gjk = new GJK();
Vector3 guess = new Vector3(1,1,1);
GJKStatus gjk_status=gjk.Evaluate(shape,ref guess);
if(gjk_status==GJKStatus.Valid)
{
Vector3 w0=Vector3.Zero;
Vector3 w1=Vector3.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;
Vector3 temp = -gjk.m_simplex.c[i].d;
w1+=shape.Support(ref temp,1)*p;
}
results.witnesses0 = Vector3.Transform(w0,wtrs0);
results.witnesses1 = Vector3.Transform(w1,wtrs0);
Vector3 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,results))
{
Vector3 delta= results.witnesses0-results.witnesses1;
float length= delta.Length();
if (length >= MathUtil.SIMD_EPSILON)
results.normal = delta/length;
return(-length);
}
}
}
return(MathUtil.SIMD_INFINITY);
}
public GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref Vector3 guess)
{
uint iterations=0;
float sqdist=0f;
float alpha=0f;
Vector3[] lastw = new Vector3[4];
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();
Vector3 temp = sqrl>0?-m_ray:new Vector3(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 */
Vector3 temp2 = -m_ray;
AppendVertice(cs,ref temp2);
Vector3 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=Vector3.Dot(m_ray,w)/rl;
alpha=System.Math.Max(omega,alpha);
if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0)
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
/* Reduce simplex */
Vector4 weights = new Vector4();
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 = Vector3.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 = MathUtil.VectorComponent(ref 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 (BulletGlobals.g_streamWriter != null && GjkEpaSolver2.debugGJK)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("gjk eval dist[{0}]", m_distance));
}
return(m_status);
}