public eStatus Evaluate(tShape shapearg, btVector3 guess) { U iterations = 0; float sqdist = 0; float alpha = 0; //btVector3 *lastw=stackalloc btVector3[4]; //btVector3[] lastw = new btVector3[4]; StackPtr<btVector3> lastw = StackPtr<btVector3>.Allocate(4); try { U 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 = eStatus.Valid; m_shape = shapearg; m_distance = 0; /* Initialize simplex */ m_simplices[0].rank = 0; m_ray = guess; float sqrl = m_ray.Length2; appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : new btVector3(1, 0, 0)); 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 { U next = 1 - m_current; sSimplex cs = m_simplices[m_current]; sSimplex ns = m_simplices[next]; /* Check zero */ float rl = m_ray.Length; if (rl < GJK_MIN_DISTANCE) {/* Touching or inside */ m_status = eStatus.Inside; break; } /* Append new vertice in -'v' direction */ appendvertice(cs, -m_ray); btVector3 w = cs.c[cs.rank - 1].w; bool found = false; for (U i = 0; i < 4; ++i) { if ((w - lastw[i]).Length2 < 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 = btVector3.dot(m_ray, w) / rl; alpha = (float)Math.Max(omega, alpha); if (((rl - alpha) - (GJK_ACCURARY * rl)) <= 0) {/* Return old simplex */ removevertice(m_simplices[m_current]); break; } /* Reduce simplex */ //float* weights = stackalloc float[4]; StackPtr<float> weights = StackPtr<float>.Allocate(4); try { U mask = 0; switch (cs.rank) { case 2: sqdist = projectorigin(cs.c[0].w, cs.c[1].w, weights, ref mask); break; case 3: sqdist = projectorigin(cs.c[0].w, cs.c[1].w, cs.c[2].w, weights, ref mask); break; case 4: sqdist = projectorigin(cs.c[0].w, cs.c[1].w, cs.c[2].w, cs.c[3].w, weights, ref mask); break; } if (sqdist >= 0) {/* Valid */ ns.rank = 0; m_ray = new btVector3(0, 0, 0); m_current = next; for (U i = 0, ni = cs.rank; i < ni; ++i) { if ((mask & (1 << (int)i)) != 0) { ns.c[ns.rank] = cs.c[i]; ns.p[ns.rank++] = weights[i]; #region m_ray += cs.c[i].w * weights[i]; { btVector3 temp; btVector3.Multiply(ref cs.c[i].w, weights[i], out temp); m_ray.Add(ref temp); } #endregion } else { m_free[m_nfree++] = cs.c[i]; } } if (mask == 15) m_status = eStatus.Inside; } else {/* Return old simplex */ removevertice(m_simplices[m_current]); break; } m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus.Failed; } finally { weights.Dispose(); } } while (m_status == eStatus.Valid); m_simplex = m_simplices[m_current]; switch (m_status) { case eStatus.Valid: m_distance = m_ray.Length; break; case eStatus.Inside: m_distance = 0; break; default: { break; } } return (m_status); } finally { lastw.Dispose(); } }
void removevertice(sSimplex simplex) { m_free[m_nfree++] = simplex.c[--simplex.rank]; }
void appendvertice(sSimplex simplex, btVector3 v) { simplex.p[simplex.rank] = 0; simplex.c[simplex.rank] = m_free[--m_nfree]; getsupport(v, simplex.c[simplex.rank++]); }
internal eStatus._ Evaluate( tShape shapearg, ref btVector3 guess ) { uint iterations = 0; double sqdist = 0; double alpha = 0; btVector3[] lastw = new btVector3[4]; uint clastw = 0; /* Initialize solver */ m_free[0] = new sSV(); m_free[1] = new sSV(); m_free[2] = new sSV(); m_free[3] = new sSV(); m_nfree = 4; m_current = 0; m_status = eStatus._.Valid; m_shape = shapearg; m_distance = 0; /* Initialize simplex */ m_simplices0.rank = 0; m_ray = guess; double sqrl = m_ray.length2(); btVector3 tmp; if( sqrl > 0 ) m_ray.Invert( out tmp ); else tmp = btVector3.xAxis; appendvertice( m_simplices0, ref tmp ); m_simplices0.p[0] = 1; m_ray = m_simplices0.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_current==0?m_simplices0:m_simplices1; sSimplex ns = next==0?m_simplices0:m_simplices1; /* Check zero */ double rl = m_ray.length(); if( rl < GJK_MIN_DISTANCE ) {/* Touching or inside */ m_status = eStatus._.Inside; break; } /* Append new vertice in -'v' direction */ m_ray.Invert( out tmp ); appendvertice( cs, ref tmp ); btVector3 w = cs.c[cs.rank - 1].w; bool found = false; for( uint i = 0; i < 4; ++i ) { w.Sub( ref lastw[i], out tmp ); if( tmp.length2() < GJK_DUPLICATED_EPS ) { found = true; break; } } if( found ) {/* Return old simplex */ removevertice( cs ); break; } else {/* Update lastw */ lastw[clastw = ( clastw + 1 ) & 3] = w; } /* Check for termination */ double omega = btVector3.btDot( ref m_ray, ref w ) / rl; alpha = btScalar.btMax( omega, alpha ); if( ( ( rl - alpha ) - ( GJK_ACCURARY * rl ) ) <= 0 ) {/* Return old simplex */ removevertice( cs ); break; } /* Reduce simplex */ double[] weights = new double[4]; uint mask = 0; switch( cs.rank ) { case 2: sqdist = projectorigin( ref cs.c[0].w, ref cs.c[1].w, weights, out mask ); break; case 3: sqdist = projectorigin( ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, weights, out mask ); break; case 4: sqdist = projectorigin( ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, weights, out mask ); break; } if( sqdist >= 0 ) {/* Valid */ ns.rank = 0; m_ray = btVector3.Zero; m_current = next; for( int i = 0, ni = (int)cs.rank; i < ni; ++i ) { if( ( mask & ( (uint)1 << i ) ) != 0 ) { ns.c[ns.rank] = cs.c[i]; ns.p[ns.rank++] = weights[i]; btVector3 tmp2; cs.c[i].w.Mult( weights[i], out tmp2 ); m_ray.Add( ref tmp2, out m_ray ); } else { m_free[m_nfree++] = cs.c[i]; } } if( mask == 15 ) m_status = eStatus._.Inside; } else {/* Return old simplex */ removevertice( cs ); break; } m_status = ( ( ++iterations ) < GJK_MAX_ITERATIONS ) ? m_status : eStatus._.Failed; } while( m_status == eStatus._.Valid ); m_simplex = m_current==0?m_simplices0:m_simplices1; switch( m_status ) { case eStatus._.Valid: m_distance = m_ray.length(); break; case eStatus._.Inside: m_distance = 0; break; default: break; } return ( m_status ); }