public bool calcPenDepth(ISimplexSolver simplexSolver, ConvexShape pConvexA, ConvexShape pConvexB, btTransform transformA, btTransform transformB, ref btVector3 v, out btVector3 wWitnessOnA, out btVector3 wWitnessOnB, IDebugDraw debugDraw)
{
btVector3 guessVector;// = transformA.Origin - transformB.Origin;
btVector3.Subtract(ref transformA.Origin,ref transformB.Origin, out guessVector);
GjkEpaSolver2.sResults results = new GjkEpaSolver2.sResults();
if (GjkEpaSolver2.Penetration(pConvexA, transformA,
pConvexB, transformB,
guessVector, ref results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return true;
}
else
{
if (GjkEpaSolver2.Distance(pConvexA, transformA, pConvexB, transformB, guessVector, ref results))
{
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return false;
}
}
wWitnessOnA = new btVector3();
wWitnessOnB = new btVector3();
return false;
}
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 GjkPairDetector(ConvexShape objectA, ConvexShape objectB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
{
m_cachedSeparatingAxis = new btVector3(0f, 1f, 0f);
m_penetrationDepthSolver = penetrationDepthSolver;
m_simplexSolver = simplexSolver;
m_minkowskiA = objectA;
m_minkowskiB = objectB;
m_shapeTypeA = objectA.ShapeType;
m_shapeTypeB = objectB.ShapeType;
m_marginA = objectA.Margin;
m_marginB = objectB.Margin;
m_ignoreMargin = false;
m_lastUsedMethod = -1;
m_catchDegeneracies = 1;
m_cachedSeparatingDistance = 0f;
m_curIter = -1;
m_degenerateSimplex = -1;
}
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);
}
static void Initialize(ConvexShape shape0, btTransform wtrs0,
ConvexShape shape1, btTransform wtrs1,
ref GjkEpaSolver2.sResults results,
ref tShape shape,
bool withmargins)
{
/* Results */
results.witnesses0 =
results.witnesses1 = new btVector3(0, 0, 0);
results.status = GjkEpaSolver2.sResults.eStatus.Separated;
/* Shape */
shape.m_shapes0 = shape0;
shape.m_shapes1 = shape1;
//shape.m_toshape1 = wtrs1.Basis.transposeTimes(wtrs0.Basis);
wtrs1.Basis.transposeTimes(ref wtrs0.Basis, out shape.m_toshape1);
shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
shape.EnableMargin(withmargins);
}