public bool Expand(uint pass, sSV w, sFace f, uint e, ref sHorizon horizon) { if (f.pass != pass) { uint e1 = i1m3[e]; if ((IndexedVector3.Dot(ref f.n, ref w.w) - f.d) < -GjkEpaSolver2.EPA_PLANE_EPS) { sFace nf = NewFace(f.c[e1], f.c[e], w, false); if (nf != null) { Bind(nf, 0, f, e); if (horizon.cf != null) { Bind(horizon.cf, 1, nf, 2); } else { horizon.ff = nf; } horizon.cf = nf; ++horizon.nf; return(true); } } else { uint e2 = i2m3[e]; f.pass = (uint)pass; if (Expand(pass, w, f.f[e1], f.e[e1], ref horizon) && Expand(pass, w, f.f[e2], f.e[e2], ref horizon)) { Remove(m_hull, f); Append(m_stock, f); return(true); } } } return(false); }
public eStatus Evaluate(GJK gjk, ref IndexedVector3 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 */ tetra[0] = NewFace(simplex.c[0], simplex.c[1], simplex.c[2], true); tetra[1] = NewFace(simplex.c[1], simplex.c[0], simplex.c[3], true); tetra[2] = NewFace(simplex.c[2], simplex.c[1], simplex.c[3], true); tetra[3] = 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 = IndexedVector3.Dot(ref best.n, ref 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], ref 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; } } IndexedVector3 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] = IndexedVector3.Cross(outer.c[1].w - projection, outer.c[2].w - projection).Length(); m_result.p[1] = IndexedVector3.Cross(outer.c[2].w - projection, outer.c[0].w - projection).Length(); m_result.p[2] = IndexedVector3.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 IndexedVector3(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); }
public eStatus Evaluate(GJK gjk,ref IndexedVector3 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 */ tetra[0] = NewFace(simplex.c[0], simplex.c[1], simplex.c[2], true); tetra[1] = NewFace(simplex.c[1], simplex.c[0], simplex.c[3], true); tetra[2] = NewFace(simplex.c[2], simplex.c[1], simplex.c[3], true); tetra[3] = 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=IndexedVector3.Dot(ref best.n,ref 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], ref 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; } } IndexedVector3 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] = IndexedVector3.Cross( outer.c[1].w-projection, outer.c[2].w-projection).Length(); m_result.p[1] = IndexedVector3.Cross(outer.c[2].w - projection, outer.c[0].w-projection).Length(); m_result.p[2] = IndexedVector3.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 IndexedVector3(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); }
public bool Expand(uint pass, sSV w, sFace f, uint e, ref sHorizon horizon) { if(f.pass!=pass) { uint e1 = i1m3[e]; if ((IndexedVector3.Dot(ref f.n, ref w.w) - f.d) < -GjkEpaSolver2.EPA_PLANE_EPS) { sFace nf = NewFace(f.c[e1],f.c[e],w,false); if(nf != null) { Bind(nf,0,f,e); if(horizon.cf != null) { Bind(horizon.cf,1,nf,2); } else { horizon.ff=nf; } horizon.cf=nf; ++horizon.nf; return(true); } } else { uint e2=i2m3[e]; f.pass = (uint)pass; if( Expand(pass,w,f.f[e1],f.e[e1],ref horizon)&& Expand(pass,w,f.f[e2],f.e[e2],ref horizon)) { Remove(m_hull,f); Append(m_stock,f); return(true); } } } return(false); }