//
// 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 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 );
}
//
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 );
}
