private static bool XenoDetection(
XenoScan <T> a,
XenoScan <T> b,
int maxIterations,
out Vector <T> point,
out Vector <T> normal,
out T penetration)
{
point = null;
normal = null;
penetration = default(T);
Vector <T> minkowskiDifference = b.Position - a.Position;
normal = -minkowskiDifference;
if (NearlyZero(minkowskiDifference.MagnitudeSquared))
{
minkowskiDifference = new Vector <T>(
Compute.Divide(Constant <T> .One, Compute.Convert <int, T>(100000)),
Constant <T> .Zero,
Constant <T> .Zero);
}
Vector <T> a_xenoScan1 = Quaternion <T> .Rotate(a.Orientation, a.XenoScan(Quaternion <T> .Rotate(a.Orientation, minkowskiDifference)));
Vector <T> b_xenoScan1 = Quaternion <T> .Rotate(b.Orientation, b.XenoScan(Quaternion <T> .Rotate(b.Orientation, normal)));
Vector <T> xenoScans1_subtract = b_xenoScan1 - a_xenoScan1;
if (Compute.LessThanOrEqual(Vector <T> .DotProduct(xenoScans1_subtract, normal), Constant <T> .Zero))
{
return(false);
}
Vector <T> crossOfXenoAndMD = Vector <T> .CrossProduct(xenoScans1_subtract, minkowskiDifference);
if (NearlyZero(crossOfXenoAndMD.MagnitudeSquared))
{
crossOfXenoAndMD = (xenoScans1_subtract - minkowskiDifference).Normalize();
point = (a_xenoScan1 + b_xenoScan1) * Compute.Divide(Constant <T> .One, Constant <T> .Two);
penetration = Vector <T> .DotProduct(xenoScans1_subtract, crossOfXenoAndMD);
return(true);
}
Vector <T> a_xenoScan2 = Quaternion <T> .Rotate(a.Orientation, a.XenoScan(Quaternion <T> .Rotate(a.Orientation, -crossOfXenoAndMD)));
Vector <T> b_xenoScan2 = Quaternion <T> .Rotate(b.Orientation, b.XenoScan(Quaternion <T> .Rotate(b.Orientation, normal)));
Vector <T> xenoScans2_subtract = b_xenoScan2 - a_xenoScan2;
if (Compute.LessThanOrEqual(Vector <T> .DotProduct(xenoScans2_subtract, normal), Constant <T> .Zero))
{
return(false);
}
Vector <T> crossOfXenoScans = Vector <T> .CrossProduct(xenoScans1_subtract - minkowskiDifference, xenoScans2_subtract - minkowskiDifference);
T distance = Vector <T> .DotProduct(crossOfXenoAndMD, minkowskiDifference);
if (Compute.LessThan(distance, Constant <T> .Zero))
{
Vector <T> temp;
temp = xenoScans1_subtract;
xenoScans1_subtract = xenoScans2_subtract;
xenoScans2_subtract = temp;
temp = a_xenoScan1;
a_xenoScan1 = a_xenoScan2;
a_xenoScan2 = temp;
temp = b_xenoScan1;
b_xenoScan1 = b_xenoScan2;
b_xenoScan2 = temp;
normal = -normal;
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
while (true)
{
if (phase1 > maxIterations)
{
return(false);
}
phase1++;
Vector <T> neg_normal = -normal;
Vector <T> a_xenoScan3 = Quaternion <T> .Rotate(a.Orientation, a.XenoScan(Quaternion <T> .Rotate(a.Orientation, neg_normal)));
Vector <T> b_xenoScan3 = Quaternion <T> .Rotate(b.Orientation, b.XenoScan(Quaternion <T> .Rotate(b.Orientation, normal)));
Vector <T> xenoScans3_subtract = b_xenoScan3 - a_xenoScan3;
if (Compute.LessThan(Vector <T> .DotProduct(xenoScans3_subtract, normal), Constant <T> .Zero))
{
return(false);
}
if (Compute.LessThan(Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans1_subtract, xenoScans3_subtract), minkowskiDifference), Constant <T> .Zero))
{
xenoScans2_subtract = xenoScans3_subtract;
a_xenoScan2 = a_xenoScan3;
b_xenoScan2 = b_xenoScan3;
normal = Vector <T> .CrossProduct(xenoScans1_subtract - minkowskiDifference, xenoScans3_subtract - minkowskiDifference);
continue;
}
if (Compute.LessThan(Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans3_subtract, xenoScans2_subtract), minkowskiDifference), Constant <T> .Zero))
{
xenoScans1_subtract = xenoScans3_subtract;
a_xenoScan1 = a_xenoScan3;
b_xenoScan1 = b_xenoScan3;
normal = Vector <T> .CrossProduct(xenoScans3_subtract - minkowskiDifference, xenoScans2_subtract - minkowskiDifference);
continue;
}
while (true)
{
phase2++;
normal = Vector <T> .CrossProduct(xenoScans2_subtract - xenoScans1_subtract, xenoScans3_subtract - xenoScans1_subtract);
// Ommited because appears to be an error
//if (NearlyZero(normal.Magnitude()))
// return true;
normal = normal.Normalize();
if (!hit && Compute.GreaterThanOrEqual(Vector <T> .DotProduct(normal, xenoScans1_subtract), Constant <T> .Zero))
{
hit = true;
}
neg_normal = -normal;
Vector <T> a_xenoScan4 = Quaternion <T> .Rotate(a.Orientation, a.XenoScan(Quaternion <T> .Rotate(a.Orientation, neg_normal)));
Vector <T> b_xenoScan4 = Quaternion <T> .Rotate(b.Orientation, b.XenoScan(Quaternion <T> .Rotate(b.Orientation, normal)));
Vector <T> xenoScans4_subtract = b_xenoScan4 - a_xenoScan4;
T delta = Vector <T> .DotProduct(xenoScans4_subtract - xenoScans3_subtract, normal);
penetration = Vector <T> .DotProduct(xenoScans4_subtract, normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (Compute.LessThanOrEqual(delta, CollideEpsilon) || Compute.LessThanOrEqual(penetration, Constant <T> .Zero) || phase2 > maxIterations)
{
if (hit)
{
T b0 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans1_subtract, xenoScans2_subtract), xenoScans3_subtract);
T b1 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans3_subtract, xenoScans2_subtract), minkowskiDifference);
T b2 = Vector <T> .DotProduct(Vector <T> .CrossProduct(minkowskiDifference, xenoScans1_subtract), xenoScans3_subtract);
T b3 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans2_subtract, xenoScans1_subtract), minkowskiDifference);
T sum = Compute.Add(b0, b1, b2, b3);
if (Compute.LessThanOrEqual(sum, Constant <T> .Zero))
{
b0 = Constant <T> .Zero;
b1 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans2_subtract, xenoScans3_subtract), normal);
b2 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans3_subtract, xenoScans1_subtract), normal);
b3 = Vector <T> .DotProduct(Vector <T> .CrossProduct(xenoScans1_subtract, xenoScans2_subtract), normal);
sum = Compute.Add(b0, b1, b2, b3);
}
T inv = Compute.Divide(Constant <T> .One, sum);
point =
(a.Position * b0 +
a_xenoScan1 * b1 +
a_xenoScan2 * b2 +
a_xenoScan3 * b3 +
b.Position * b0 +
b_xenoScan1 * b1 +
b_xenoScan2 * b2 +
b_xenoScan3 * b3)
* inv;
}
return(hit);
}
Vector <T> xeno4CrossMD = Vector <T> .CrossProduct(xenoScans4_subtract, minkowskiDifference);
T dot = Vector <T> .DotProduct(xeno4CrossMD, xenoScans1_subtract);
if (Compute.GreaterThanOrEqual(dot, Constant <T> .Zero))
{
dot = Vector <T> .DotProduct(xeno4CrossMD, xenoScans2_subtract);
if (Compute.GreaterThanOrEqual(dot, Constant <T> .Zero))
{
xenoScans1_subtract = xenoScans4_subtract;
a_xenoScan1 = a_xenoScan4;
b_xenoScan1 = b_xenoScan4;
}
else
{
xenoScans3_subtract = xenoScans4_subtract;
a_xenoScan3 = a_xenoScan4;
b_xenoScan3 = b_xenoScan4;
}
}
else
{
dot = Vector <T> .DotProduct(xeno4CrossMD, xenoScans3_subtract);
if (Compute.GreaterThanOrEqual(dot, Constant <T> .Zero))
{
xenoScans2_subtract = xenoScans4_subtract;
a_xenoScan2 = a_xenoScan4;
b_xenoScan2 = b_xenoScan4;
}
else
{
xenoScans1_subtract = xenoScans4_subtract;
a_xenoScan1 = a_xenoScan4;
b_xenoScan1 = b_xenoScan4;
}
}
}
}
}
