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 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 eStatus Evaluate(GJK gjk,ref Vector3 guess)
{
sSimplex simplex=gjk.m_simplex;
if((simplex.rank>1)&&gjk.EncloseOrigin())
{
/* Clean up */
while(m_hull.Count > 0)
{
sFace f = m_hull[0];
Remove(m_hull,f);
Append(m_stock,f);
}
m_status = eStatus.Valid;
m_nextsv = 0;
/* Orient simplex */
if(GJK.Det( simplex.c[0].w-simplex.c[3].w,
simplex.c[1].w-simplex.c[3].w,
simplex.c[2].w-simplex.c[3].w)<0)
{
SwapSv(simplex.c,0,1);
SwapFloat(simplex.p,0,1);
}
/* Build initial hull */
sFace[] tetra ={NewFace(simplex.c[0],simplex.c[1],simplex.c[2],true),
NewFace(simplex.c[1],simplex.c[0],simplex.c[3],true),
NewFace(simplex.c[2],simplex.c[1],simplex.c[3],true),
NewFace(simplex.c[0],simplex.c[2],simplex.c[3],true)};
if(m_hull.Count==4)
{
sFace best=FindBest();
sFace outer = best;
uint pass=0;
uint iterations=0;
Bind(tetra[0],0,tetra[1],0);
Bind(tetra[0],1,tetra[2],0);
Bind(tetra[0],2,tetra[3],0);
Bind(tetra[1],1,tetra[3],2);
Bind(tetra[1],2,tetra[2],1);
Bind(tetra[2],2,tetra[3],1);
m_status=eStatus.Valid;
for (; iterations < GjkEpaSolver2.EPA_MAX_ITERATIONS; ++iterations)
{
if (m_nextsv < GjkEpaSolver2.EPA_MAX_VERTICES)
{
sHorizon horizon = new sHorizon() ;
sSV w = m_sv_store[m_nextsv++];
bool valid = true;
best.pass = (uint)(++pass);
gjk.GetSupport(ref best.n,ref w);
float wdist=Vector3.Dot(best.n,w.w)-best.d;
if (wdist > GjkEpaSolver2.EPA_ACCURACY)
{
for(int j=0;(j<3)&&valid;++j)
{
valid&=Expand( pass,w,
best.f[j],best.e[j],
horizon);
}
if(valid&&(horizon.nf>=3))
{
Bind(horizon.cf,1,horizon.ff,2);
Remove(m_hull,best);
Append(m_stock,best);
best=FindBest();
if (best.p >= outer.p)
{
outer = best;
}
}
else
{
m_status=eStatus.InvalidHull;
break;
}
}
else
{
m_status=eStatus.AccuraryReached;
break;
}
}
else
{
m_status=eStatus.OutOfVertices;
break;
}
}
Vector3 projection=outer.n*outer.d;
m_normal = outer.n;
m_depth = outer.d;
m_result.rank = 3;
m_result.c[0] = outer.c[0];
m_result.c[1] = outer.c[1];
m_result.c[2] = outer.c[2];
m_result.p[0] = Vector3.Cross( outer.c[1].w-projection,
outer.c[2].w-projection).Length();
m_result.p[1] = Vector3.Cross(outer.c[2].w - projection,
outer.c[0].w-projection).Length();
m_result.p[2] = Vector3.Cross(outer.c[0].w - projection,
outer.c[1].w-projection).Length();
float sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
m_result.p[0] /= sum;
m_result.p[1] /= sum;
m_result.p[2] /= sum;
return(m_status);
}
}
/* Fallback */
m_status = eStatus.FallBack;
m_normal = -guess;
float nl=m_normal.LengthSquared();
if(nl>0)
{
m_normal.Normalize();
}
else
{
m_normal = new Vector3(1,0,0);
}
m_depth = 0;
m_result.rank=1;
m_result.c[0]=simplex.c[0];
m_result.p[0]=1;
return(m_status);
}
