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); }