public void Initialise() { m_ray = IndexedVector3.Zero; m_nfree = 0; m_status = GJKStatus.Failed; m_current = 0; m_distance = 0f; for (int i = 0; i < m_simplices.Length; ++i) { if (m_simplices[i] == null) { m_simplices[i] = new sSimplex(); } } for (int i = 0; i < m_store.Length; ++i) { if (m_store[i] == null) { m_store[i] = new sSV(); } } }
public void Initialize() { m_status = eStatus.Failed; m_normal = IndexedVector3.Zero; m_depth = 0; m_nextsv = 0; m_result = new sSimplex(); for (int i = 0; i < m_sv_store.Length; ++i) { m_sv_store[i] = new sSV(); } for (int i = 0; i < m_fc_store.Length; ++i) { m_fc_store[i] = new sFace(); } for (int i = 0; i < GjkEpaSolver2.EPA_MAX_FACES; ++i) { Append(m_stock, m_fc_store[GjkEpaSolver2.EPA_MAX_FACES - i - 1]); } }
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 void AppendVertice(sSimplex simplex, ref IndexedVector3 v) { simplex.p[simplex.rank] = 0; simplex.c[simplex.rank] = m_free[--m_nfree]; GetSupport(ref v, ref simplex.c[simplex.rank++]); }
public void RemoveVertice(sSimplex simplex) { m_free[m_nfree++] = simplex.c[--simplex.rank]; }
public GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref IndexedVector3 guess) { uint iterations = 0; float sqdist = 0f; float alpha = 0f; 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(); IndexedVector3 temp = sqrl > 0?-m_ray:new IndexedVector3(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 */ IndexedVector3 temp2 = -m_ray; AppendVertice(cs, ref temp2); IndexedVector3 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 = IndexedVector3.Dot(ref m_ray, ref w) / rl; alpha = Math.Max(omega, alpha); if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0) {/* Return old simplex */ RemoveVertice(m_simplices[m_current]); break; } /* Reduce simplex */ IndexedVector4 weights = new IndexedVector4(); 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 = IndexedVector3.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 = 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 DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK) { BulletGlobals.g_streamWriter.WriteLine(String.Format("gjk eval dist[{0}]", m_distance)); } #endif return(m_status); }
public void Initialize() { m_status = eStatus.Failed; m_normal = IndexedVector3.Zero; m_depth = 0; m_nextsv = 0; m_result = new sSimplex(); for (int i = 0; i < m_sv_store.Length; ++i) { m_sv_store[i] = new sSV(); } for (int i = 0; i < m_fc_store.Length; ++i) { m_fc_store[i] = new sFace(); } for (int i = 0; i < GjkEpaSolver2.EPA_MAX_FACES; ++i) { Append(m_stock, m_fc_store[GjkEpaSolver2.EPA_MAX_FACES - i - 1]); } }
public void AppendVertice(sSimplex simplex,ref IndexedVector3 v) { simplex.p[simplex.rank]=0; simplex.c[simplex.rank]=m_free[--m_nfree]; GetSupport(ref v,ref simplex.c[simplex.rank++]); }
public void RemoveVertice(sSimplex simplex) { m_free[m_nfree++]=simplex.c[--simplex.rank]; }
public void Initialise() { m_ray = IndexedVector3.Zero; m_nfree = 0; m_status = GJKStatus.Failed; m_current = 0; m_distance = 0f; for (int i = 0; i < m_simplices.Length; ++i) { if (m_simplices[i] == null) { m_simplices[i] = new sSimplex(); } } for (int i = 0; i < m_store.Length; ++i) { if (m_store[i] == null) { m_store[i] = new sSV(); } } }