public virtual bool russianCalcTimeOfImpact(ref Matrix fromA, ref Matrix toA, ref Matrix fromB, ref Matrix toB, CastResult result)
{
MinkowskiSumShape convex = new MinkowskiSumShape(m_convexA, m_convexB);
Matrix rayFromLocalA;
Matrix rayToLocalA;
rayFromLocalA = Matrix.Invert(fromA) * fromB;
rayToLocalA = Matrix.Invert(toA) * toB;
m_simplexSolver.Reset();
//convex.TransformB = rayFromLocalA;
Matrix temp = Matrix.CreateFromQuaternion(Quaternion.CreateFromRotationMatrix(rayFromLocalA));
convex.SetTransformB(ref temp);
float lambda = 0;
//todo: need to verify this:
//because of minkowski difference, we need the inverse direction
Vector3 s = -rayFromLocalA.Translation;
Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation);
Vector3 x = s;
Vector3 v;
Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(ref r);
v = x - arbitraryPoint;
int maxIter = MAX_ITERATIONS;
Vector3 n = new Vector3();
float lastLambda = lambda;
float dist2 = v.LengthSquared();
float epsilon = 0.0001f;
Vector3 w = Vector3.Zero, p = Vector3.Zero;
float VdotR;
while ((dist2 > epsilon) && (maxIter-- != 0))
{
p = convex.LocalGetSupportingVertex(ref v);
w = x - p;
float VdotW = Vector3.Dot(v, w);
if (VdotW > 0)
{
VdotR = Vector3.Dot(v, r);
if (VdotR >= -(MathUtil.SIMD_EPSILON * MathUtil.SIMD_EPSILON))
return false;
else
{
lambda = lambda - VdotW / VdotR;
x = s + lambda * r;
m_simplexSolver.Reset();
//check next line
w = x - p;
lastLambda = lambda;
n = v;
}
}
m_simplexSolver.AddVertex(ref w, ref x, ref p);
if (m_simplexSolver.Closest(ref v))
{
dist2 = v.LengthSquared();
}
else
{
dist2 = 0f;
}
}
result.m_fraction = lambda;
result.m_normal = n;
return true;
}
