private void DrawShape(tShape shape, Point point1, Point point2, Pen pen)
{
using (var g = this.CreateGraphics())
{
this.GetDrawAction(shape, g)(pen,
Math.Min(point1.X, point2.X),
Math.Min(point1.Y, point2.Y),
Math.Abs(point1.X - point2.X),
Math.Abs(point1.Y - point2.Y));
}
}
public static IShape MakeShape(string shapeType)
{
IShape shape = null;
switch (shapeType.ToLower())
{
case "i":
shape = new iShape();
break;
case "j":
shape = new jShape();
break;
case "l":
shape = new lShape();
break;
case "o":
shape = new oShape();
break;
case "s":
shape = new sShape();
break;
case "t":
shape = new tShape();
break;
case "z":
shape = new zShape();
break;
case "test":
shape = new testShape();
break;
default:
throw new ArgumentException("Invalid shape name");
}
return(shape);
}
private void EraseShape(tShape shape, Point point1, Point point2)
{
this.DrawShape(shape, point1, point2, Pens.White);
}
private void DrawShape(tShape shape, Point point1, Point point2)
{
this.DrawShape(shape, point1, point2, Pens.MidnightBlue);
}
private Action<Pen, float, float, float, float> GetDrawAction(tShape shape, Graphics g)
{
switch (shape)
{
case tShape.Rect:
return g.DrawRectangle;
case tShape.Ellipse:
return g.DrawEllipse;
default:
throw new Exception("");
}
}
//
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 );
}
}
//
static void Initialize( btConvexShape shape0, ref btTransform wtrs0,
btConvexShape shape1, ref btTransform wtrs1,
out btGjkEpaSolver2.sResults results,
tShape shape,
bool withmargins )
{
/* Results */
results.normal = btVector3.xAxis;
results.witness0 =
results.witness1 = btVector3.Zero;
results.status = btGjkEpaSolver2.sResults.eStatus.Separated;
results.distance = 0;
/* Shape */
shape.m_shape0 = shape0;
shape.m_shape1 = shape1;
wtrs1.m_basis.transposeTimes( ref wtrs0.m_basis, out shape.m_toshape1 );
wtrs0.inverseTimes( ref wtrs1, out shape.m_toshape0 );
shape.EnableMargin( withmargins );
}
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 );
}
//
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 );
}
