bool expand( uint pass, sSV w, sFace f, uint e, sHorizon horizon ) { int[] i1m3 = { 1, 2, 0 }; int[] i2m3 = { 2, 0, 1 }; if( f.pass != pass ) { int e1 = i1m3[e]; if( ( btVector3.btDot( ref f.n, ref w.w ) - f.d ) < -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 { int e2 = i2m3[e]; f.pass = (byte)pass; if( expand( pass, w, f.f[e1], f.e[e1], horizon ) && expand( pass, w, f.f[e2], f.e[e2], horizon ) ) { remove( m_hull, f ); append( m_stock, f ); return ( true ); } } } return ( false ); }
internal eStatus._ Evaluate( GJK gjk, ref btVector3 guess ) { GJK.sSimplex simplex = gjk.m_simplex; if( ( simplex.rank > 1 ) && gjk.EncloseOrigin() ) { /* Clean up */ while( m_hull.root != null ) { sFace f = m_hull.root; remove( m_hull, f ); append( m_stock, f ); } m_status = eStatus._.Valid; m_nextsv = 0; /* Orient simplex */ if( btVector3.det( ref simplex.c[0].w, ref simplex.c[3].w, ref simplex.c[1].w, ref simplex.c[3].w, ref simplex.c[2].w, ref simplex.c[3].w ) < 0 ) { btScalar.btSwap( ref simplex.c[0], ref simplex.c[1] ); btScalar.btSwap( ref simplex.p[0], ref simplex.p[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 = new sFace( 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 < EPA_MAX_ITERATIONS; ++iterations ) { if( m_nextsv < EPA_MAX_VERTICES ) { sHorizon horizon = new sHorizon(); sSV w = m_sv_store[m_nextsv++]; bool valid = true; best.pass = (byte)( ++pass ); gjk.getsupport( ref best.n, w ); double wdist = best.n.dot( ref w.w ) - best.d; if( wdist > EPA_ACCURACY ) { for( uint 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(); outer = new sFace( best ); } else { m_status = eStatus._.InvalidHull; break; } } else { m_status = eStatus._.AccuraryReached; break; } } else { m_status = eStatus._.OutOfVertices; break; } } btVector3 projection; outer.n.Mult( outer.d, out projection ); 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]; btVector3 tmp; btVector3.btCross2Del( ref outer.c[1].w, ref projection, ref outer.c[2].w, ref projection, out tmp ); m_result.p[0] = tmp.length(); btVector3.btCross2Del( ref outer.c[2].w, ref projection, ref outer.c[0].w, ref projection, out tmp ); m_result.p[1] = tmp.length(); btVector3.btCross2Del( ref outer.c[0].w, ref projection, ref outer.c[1].w, ref projection, out tmp ); m_result.p[2] = tmp.length(); double 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; guess.Invert( out m_normal ); //m_normal = -guess; double nl = m_normal.length(); if( nl > 0 ) m_normal.Div( nl, out m_normal ); else m_normal = btVector3.xAxis; m_depth = 0; m_result.rank = 1; m_result.c[0] = simplex.c[0]; m_result.p[0] = 1; return ( m_status ); }