public static bool Penetration(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results, bool usemargins) { tShape shape=new MinkowskiDiff(); Initialize(shape0,wtrs0,shape1,wtrs1,ref results,ref shape,usemargins); using (GJK gjk = GJK.CreateFromPool()) { GJK.eStatus gjk_status = gjk.Evaluate(shape, -guess); switch (gjk_status) { case GJK.eStatus.Inside: { using(EPA epa = EPA.CreateFromPool()) { EPA.eStatus epa_status = epa.Evaluate(gjk, -guess); if (epa_status != EPA.eStatus.Failed) { btVector3 w0 = new btVector3(0, 0, 0); for (U i = 0; i < epa.m_result.rank; ++i) { #region w0 += shape.Support(epa.m_result.c[i].d, 0) * epa.m_result.p[i]; { btVector3 temp1, temp2; shape.Support(ref epa.m_result.c[i].d, 0, out temp1); btVector3.Multiply(ref temp1, epa.m_result.p[i], out temp2); w0.Add(ref temp2); } #endregion } results.status = sResults.eStatus.Penetrating; results.witnesses0 = wtrs0 * w0; results.witnesses1 = wtrs0 * (w0 - epa.m_normal * epa.m_depth); results.normal = -epa.m_normal; results.distance = -epa.m_depth; return (true); } else results.status = sResults.eStatus.EPA_Failed; } } break; case GJK.eStatus.Failed: results.status = sResults.eStatus.GJK_Failed; break; default: { break; } } return (false); } }
public static bool Distance(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results) { tShape shape = new MinkowskiDiff(); Initialize(shape0, wtrs0, shape1, wtrs1, ref results, ref shape, false); using(GJK gjk = GJK.CreateFromPool()) { GJK.eStatus gjk_status = gjk.Evaluate(shape, guess); if (gjk_status == GJK.eStatus.Valid) { btVector3 w0 = new btVector3(0, 0, 0); btVector3 w1 = new btVector3(0, 0, 0); for (U i = 0; i < gjk.m_simplex.rank; ++i) { float p = gjk.m_simplex.p[i]; btVector3 temp,temp2,temp3; #region w0 += shape.Support(gjk.m_simplex.c[i].d, 0) * p; shape.Support(ref gjk.m_simplex.c[i].d, 0, out temp); btVector3.Multiply(ref temp, p, out temp2); w0.Add(ref temp2); #endregion #region w1 += shape.Support(-gjk.m_simplex.c[i].d, 1) * p; btVector3.Minus(ref gjk.m_simplex.c[i].d, out temp3); shape.Support(ref temp3, 1, out temp); btVector3.Multiply(ref temp, p, out temp2); w1.Add(ref temp2); #endregion } results.witnesses0 = wtrs0 * w0; results.witnesses1 = wtrs0 * w1; results.normal = w0 - w1; results.distance = results.normal.Length; results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1; return (true); } else { results.status = gjk_status == GJK.eStatus.Inside ? sResults.eStatus.Penetrating : sResults.eStatus.GJK_Failed; return (false); } } }
public static bool Penetration(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results) { return Penetration(shape0, wtrs0, shape1, wtrs1, guess, ref results, true); }
// public static bool Penetration( btConvexShape shape0, ref btTransform wtrs0, btConvexShape shape1, ref btTransform wtrs1, ref btVector3 guess, out sResults results, bool usemargins = false ) { tShape shape = new tShape(); Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, usemargins ); GJK gjk = new GJK(); btVector3 tmp; guess.Invert( out tmp ); GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref tmp ); switch( gjk_status ) { case GJK.eStatus._.Inside: { EPA epa = new EPA(); EPA.eStatus._ epa_status = epa.Evaluate( gjk, ref tmp ); if( epa_status != EPA.eStatus._.Failed ) { btVector3 w0 = btVector3.Zero; for( uint i = 0; i < epa.m_result.rank; ++i ) { shape.Support( ref epa.m_result.c[i].d, 0, out tmp ); w0.AddScale( ref tmp, epa.m_result.p[i], out w0 ); } results.status = sResults.eStatus.Penetrating; wtrs0.Apply( ref w0, out results.witness0 ); w0.SubScale( ref epa.m_normal, epa.m_depth, out tmp ); wtrs0.Apply( ref tmp, out results.witness1 ); epa.m_normal.Invert( out results.normal ); results.distance = -epa.m_depth; return ( true ); } else results.status = sResults.eStatus.EPA_Failed; } break; case GJK.eStatus._.Failed: results.status = sResults.eStatus.GJK_Failed; break; default: break; } return ( false ); }
// // Api // // // public static bool Distance( btConvexShape shape0, ref btTransform wtrs0, btConvexShape shape1, ref btTransform wtrs1, ref btVector3 guess, out sResults results ) { tShape shape = new tShape(); results.witness0 = results.witness1 = btVector3.Zero; results.status = btGjkEpaSolver2.sResults.eStatus.Separated; Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, false ); GJK gjk = new GJK(); GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref guess ); if( gjk_status == GJK.eStatus._.Valid ) { btVector3 w0 = btVector3.Zero; btVector3 w1 = btVector3.Zero; for( uint i = 0; i < gjk.m_simplex.rank; ++i ) { double p = gjk.m_simplex.p[i]; btVector3 tmp; shape.Support( ref gjk.m_simplex.c[i].d, 0, out tmp ); w0.AddScale( ref tmp, p, out w0 ); gjk.m_simplex.c[i].d.Invert( out tmp ); shape.Support( ref tmp, 1, out tmp ); w1.AddScale( ref tmp, p, out w1 ); } wtrs0.Apply( ref w0, out results.witness0 ); wtrs0.Apply( ref w1, out results.witness1 ); w0.Sub( ref w1, out results.normal ); results.distance = results.normal.length(); results.normal.Div( ( results.distance > GJK_MIN_DISTANCE ) ? results.distance : 1, out results.normal ); return ( true ); } else { results.status = gjk_status == GJK.eStatus._.Inside ? sResults.eStatus.Penetrating : sResults.eStatus.GJK_Failed; return ( false ); } }
// public static bool SignedDistance( btConvexShape shape0, ref btTransform wtrs0, btConvexShape shape1, ref btTransform wtrs1, ref btVector3 guess, out sResults results ) { if( !Distance( shape0, ref wtrs0, shape1, ref wtrs1, ref guess, out results ) ) return ( Penetration( shape0, ref wtrs0, shape1, ref wtrs1, ref guess, out results, false ) ); else { return ( true ); } }
// public static double SignedDistance( ref btVector3 position, double margin, btConvexShape shape0, ref btTransform wtrs0, sResults results ) { tShape shape = new tShape(); using( btSphereShape shape1 = BulletGlobals.SphereShapePool.Get() ) { shape1.Initialize( margin ); btTransform wtrs1 = new btTransform( ref btQuaternion.Zero, ref position ); Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, false ); GJK gjk = new GJK(); GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref btVector3.One ); if( gjk_status == GJK.eStatus._.Valid ) { btVector3 w0 = btVector3.Zero; btVector3 w1 = btVector3.Zero; for( uint i = 0; i < gjk.m_simplex.rank; ++i ) { double p = gjk.m_simplex.p[i]; btVector3 tmp; shape.Support( ref gjk.m_simplex.c[i].d, 0, out tmp ); w0.AddScale( ref tmp, p, out w0 ); btVector3 tmp2; gjk.m_simplex.c[i].d.Invert( out tmp2 ); shape.Support( ref tmp2, 1, out tmp ); w1.AddScale( ref tmp, p, out w1 ); } wtrs0.Apply( ref w0, out results.witness0 ); wtrs0.Apply( ref w1, out results.witness1 ); btVector3 delta; results.witness1.Sub( ref results.witness0, out delta ); margin = shape0.getMarginNonVirtual() + shape1.getMarginNonVirtual(); double length = delta.length(); delta.Div( length, out results.normal ); results.witness0.AddScale( ref results.normal, margin, out results.witness0 ); return ( length - margin ); } else { if( gjk_status == GJK.eStatus._.Inside ) { if( Penetration( shape0, ref wtrs0, shape1, ref wtrs1, ref gjk.m_ray, out results ) ) { btVector3 delta; results.witness0.Sub( ref results.witness1, out delta ); double length = delta.length(); if( length >= btScalar.SIMD_EPSILON ) delta.Div( length, out results.normal ); return ( -length ); } } } } return ( btScalar.SIMD_INFINITY ); }