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