public static bool Intersects(XMVector a0, XMVector a1, XMVector a2, XMVector b0, XMVector b1, XMVector b2)
{
XMVector selectY = XMVector.FromInt((uint)XMSelection.Select0, (uint)XMSelection.Select1, (uint)XMSelection.Select0, (uint)XMSelection.Select0);
XMVector selectZ = XMVector.FromInt((uint)XMSelection.Select0, (uint)XMSelection.Select0, (uint)XMSelection.Select1, (uint)XMSelection.Select0);
XMVector select0111 = XMVector.FromInt((uint)XMSelection.Select0, (uint)XMSelection.Select1, (uint)XMSelection.Select1, (uint)XMSelection.Select1);
XMVector select1011 = XMVector.FromInt((uint)XMSelection.Select1, (uint)XMSelection.Select0, (uint)XMSelection.Select1, (uint)XMSelection.Select1);
XMVector select1101 = XMVector.FromInt((uint)XMSelection.Select1, (uint)XMSelection.Select1, (uint)XMSelection.Select0, (uint)XMSelection.Select1);
XMVector zero = XMGlobalConstants.Zero;
// Compute the normal of triangle A.
XMVector n1 = XMVector3.Cross(a1 - a0, a2 - a0);
// Assert that the triangle is not degenerate.
Debug.Assert(!XMVector3.Equal(n1, zero), "Reviewed");
// Test points of B against the plane of A.
XMVector b_dist = XMVector3.Dot(n1, b0 - a0);
b_dist = XMVector.Select(b_dist, XMVector3.Dot(n1, b1 - a0), selectY);
b_dist = XMVector.Select(b_dist, XMVector3.Dot(n1, b2 - a0), selectZ);
// Ensure robustness with co-planar triangles by zeroing small distances.
XMComparisonRecord b_distIsZeroCR;
XMVector b_distIsZero = XMVector.GreaterR(out b_distIsZeroCR, CollisionGlobalConstants.RayEpsilon, b_dist.Abs());
b_dist = XMVector.Select(b_dist, zero, b_distIsZero);
XMComparisonRecord b_distIsLessCR;
XMVector b_distIsLess = XMVector.GreaterR(out b_distIsLessCR, zero, b_dist);
XMComparisonRecord b_distIsGreaterCR;
XMVector b_distIsGreater = XMVector.GreaterR(out b_distIsGreaterCR, b_dist, zero);
// If all the points are on the same side we don't intersect.
if (b_distIsLessCR.IsAllTrue || b_distIsGreaterCR.IsAllTrue)
{
return(false);
}
// Compute the normal of triangle B.
XMVector n2 = XMVector3.Cross(b1 - b0, b2 - b0);
// Assert that the triangle is not degenerate.
Debug.Assert(!XMVector3.Equal(n2, zero), "Reviewed");
// Test points of A against the plane of B.
XMVector a_dist = XMVector3.Dot(n2, a0 - b0);
a_dist = XMVector.Select(a_dist, XMVector3.Dot(n2, a1 - b0), selectY);
a_dist = XMVector.Select(a_dist, XMVector3.Dot(n2, a2 - b0), selectZ);
// Ensure robustness with co-planar triangles by zeroing small distances.
XMComparisonRecord a_distIsZeroCR;
XMVector a_distIsZero = XMVector.GreaterR(out a_distIsZeroCR, CollisionGlobalConstants.RayEpsilon, b_dist.Abs());
a_dist = XMVector.Select(a_dist, zero, a_distIsZero);
XMComparisonRecord a_distIsLessCR;
XMVector a_distIsLess = XMVector.GreaterR(out a_distIsLessCR, zero, a_dist);
XMComparisonRecord a_distIsGreaterCR;
XMVector a_distIsGreater = XMVector.GreaterR(out a_distIsGreaterCR, a_dist, zero);
// If all the points are on the same side we don't intersect.
if (a_distIsLessCR.IsAllTrue || a_distIsGreaterCR.IsAllTrue)
{
return(false);
}
// Special case for co-planar triangles.
if (a_distIsZeroCR.IsAllTrue || b_distIsZeroCR.IsAllTrue)
{
XMVector axis, dist, minDist;
// Compute an axis perpindicular to the edge (points out).
axis = XMVector3.Cross(n1, a1 - a0);
dist = XMVector3.Dot(axis, a0);
// Test points of B against the axis.
minDist = XMVector3.Dot(b0, axis);
minDist = XMVector.Min(minDist, XMVector3.Dot(b1, axis));
minDist = XMVector.Min(minDist, XMVector3.Dot(b2, axis));
if (XMVector4.GreaterOrEqual(minDist, dist))
{
return(false);
}
// Edge (A1, A2)
axis = XMVector3.Cross(n1, a2 - a1);
dist = XMVector3.Dot(axis, a1);
minDist = XMVector3.Dot(b0, axis);
minDist = XMVector.Min(minDist, XMVector3.Dot(b1, axis));
minDist = XMVector.Min(minDist, XMVector3.Dot(b2, axis));
if (XMVector4.GreaterOrEqual(minDist, dist))
{
return(false);
}
// Edge (A2, A0)
axis = XMVector3.Cross(n1, a0 - a2);
dist = XMVector3.Dot(axis, a2);
minDist = XMVector3.Dot(b0, axis);
minDist = XMVector.Min(minDist, XMVector3.Dot(b1, axis));
minDist = XMVector.Min(minDist, XMVector3.Dot(b2, axis));
if (XMVector4.GreaterOrEqual(minDist, dist))
{
return(false);
}
// Edge (B0, B1)
axis = XMVector3.Cross(n2, b1 - b0);
dist = XMVector3.Dot(axis, b0);
minDist = XMVector3.Dot(a0, axis);
minDist = XMVector.Min(minDist, XMVector3.Dot(a1, axis));
minDist = XMVector.Min(minDist, XMVector3.Dot(a2, axis));
if (XMVector4.GreaterOrEqual(minDist, dist))
{
return(false);
}
// Edge (B1, B2)
axis = XMVector3.Cross(n2, b2 - b1);
dist = XMVector3.Dot(axis, b1);
minDist = XMVector3.Dot(a0, axis);
minDist = XMVector.Min(minDist, XMVector3.Dot(a1, axis));
minDist = XMVector.Min(minDist, XMVector3.Dot(a2, axis));
if (XMVector4.GreaterOrEqual(minDist, dist))
{
return(false);
}
// Edge (B2,B0)
axis = XMVector3.Cross(n2, b0 - b2);
dist = XMVector3.Dot(axis, b2);
minDist = XMVector3.Dot(a0, axis);
minDist = XMVector.Min(minDist, XMVector3.Dot(a1, axis));
minDist = XMVector.Min(minDist, XMVector3.Dot(a2, axis));
if (XMVector4.GreaterOrEqual(minDist, dist))
{
return(false);
}
return(true);
}
//// Find the single vertex of A and B (ie the vertex on the opposite side
//// of the plane from the other two) and reorder the edges so we can compute
//// the signed edge/edge distances.
////
//// if ( (V0 >= 0 && V1 < 0 && V2 < 0) ||
//// (V0 > 0 && V1 <= 0 && V2 <= 0) ||
//// (V0 <= 0 && V1 > 0 && V2 > 0) ||
//// (V0 < 0 && V1 >= 0 && V2 >= 0) ) then V0 is singular;
////
//// If our singular vertex is not on the positive side of the plane we reverse
//// the triangle winding so that the overlap comparisons will compare the
//// correct edges with the correct signs.
XMVector a_distIsLessEqual = XMVector.OrInt(a_distIsLess, a_distIsZero);
XMVector a_distIsGreaterEqual = XMVector.OrInt(a_distIsGreater, a_distIsZero);
XMVector aa0, aa1, aa2;
bool b_positiveA;
if (Internal.XMVector3AllTrue(XMVector.Select(a_distIsGreaterEqual, a_distIsLess, select0111)) ||
Internal.XMVector3AllTrue(XMVector.Select(a_distIsGreater, a_distIsLessEqual, select0111)))
{
// A0 is singular, crossing from positive to negative.
aa0 = a0;
aa1 = a1;
aa2 = a2;
b_positiveA = true;
}
else if (Internal.XMVector3AllTrue(XMVector.Select(a_distIsLessEqual, a_distIsGreater, select0111)) ||
Internal.XMVector3AllTrue(XMVector.Select(a_distIsLess, a_distIsGreaterEqual, select0111)))
{
// A0 is singular, crossing from negative to positive.
aa0 = a0;
aa1 = a2;
aa2 = a1;
b_positiveA = false;
}
else if (Internal.XMVector3AllTrue(XMVector.Select(a_distIsGreaterEqual, a_distIsLess, select1011)) ||
Internal.XMVector3AllTrue(XMVector.Select(a_distIsGreater, a_distIsLessEqual, select1011)))
{
// A1 is singular, crossing from positive to negative.
aa0 = a1;
aa1 = a2;
aa2 = a0;
b_positiveA = true;
}
else if (Internal.XMVector3AllTrue(XMVector.Select(a_distIsLessEqual, a_distIsGreater, select1011)) ||
Internal.XMVector3AllTrue(XMVector.Select(a_distIsLess, a_distIsGreaterEqual, select1011)))
{
// A1 is singular, crossing from negative to positive.
aa0 = a1;
aa1 = a0;
aa2 = a2;
b_positiveA = false;
}
else if (Internal.XMVector3AllTrue(XMVector.Select(a_distIsGreaterEqual, a_distIsLess, select1101)) ||
Internal.XMVector3AllTrue(XMVector.Select(a_distIsGreater, a_distIsLessEqual, select1101)))
{
// A2 is singular, crossing from positive to negative.
aa0 = a2;
aa1 = a0;
aa2 = a1;
b_positiveA = true;
}
else if (Internal.XMVector3AllTrue(XMVector.Select(a_distIsLessEqual, a_distIsGreater, select1101)) ||
Internal.XMVector3AllTrue(XMVector.Select(a_distIsLess, a_distIsGreaterEqual, select1101)))
{
// A2 is singular, crossing from negative to positive.
aa0 = a2;
aa1 = a1;
aa2 = a0;
b_positiveA = false;
}
else
{
Debug.Assert(false, "Reviewed");
return(false);
}
XMVector b_distIsLessEqual = XMVector.OrInt(b_distIsLess, b_distIsZero);
XMVector b_distIsGreaterEqual = XMVector.OrInt(b_distIsGreater, b_distIsZero);
XMVector bb0, bb1, bb2;
bool b_positiveB;
if (Internal.XMVector3AllTrue(XMVector.Select(b_distIsGreaterEqual, b_distIsLess, select0111)) ||
Internal.XMVector3AllTrue(XMVector.Select(b_distIsGreater, b_distIsLessEqual, select0111)))
{
// B0 is singular, crossing from positive to negative.
bb0 = b0;
bb1 = b1;
bb2 = b2;
b_positiveB = true;
}
else if (Internal.XMVector3AllTrue(XMVector.Select(b_distIsLessEqual, b_distIsGreater, select0111)) ||
Internal.XMVector3AllTrue(XMVector.Select(b_distIsLess, b_distIsGreaterEqual, select0111)))
{
// B0 is singular, crossing from negative to positive.
bb0 = b0;
bb1 = b2;
bb2 = b1;
b_positiveB = false;
}
else if (Internal.XMVector3AllTrue(XMVector.Select(b_distIsGreaterEqual, b_distIsLess, select1011)) ||
Internal.XMVector3AllTrue(XMVector.Select(b_distIsGreater, b_distIsLessEqual, select1011)))
{
// B1 is singular, crossing from positive to negative.
bb0 = b1;
bb1 = b2;
bb2 = b0;
b_positiveB = true;
}
else if (Internal.XMVector3AllTrue(XMVector.Select(b_distIsLessEqual, b_distIsGreater, select1011)) ||
Internal.XMVector3AllTrue(XMVector.Select(b_distIsLess, b_distIsGreaterEqual, select1011)))
{
// B1 is singular, crossing from negative to positive.
bb0 = b1;
bb1 = b0;
bb2 = b2;
b_positiveB = false;
}
else if (Internal.XMVector3AllTrue(XMVector.Select(b_distIsGreaterEqual, b_distIsLess, select1101)) ||
Internal.XMVector3AllTrue(XMVector.Select(b_distIsGreater, b_distIsLessEqual, select1101)))
{
// B2 is singular, crossing from positive to negative.
bb0 = b2;
bb1 = b0;
bb2 = b1;
b_positiveB = true;
}
else if (Internal.XMVector3AllTrue(XMVector.Select(b_distIsLessEqual, b_distIsGreater, select1101)) ||
Internal.XMVector3AllTrue(XMVector.Select(b_distIsLess, b_distIsGreaterEqual, select1101)))
{
// B2 is singular, crossing from negative to positive.
bb0 = b2;
bb1 = b1;
bb2 = b0;
b_positiveB = false;
}
else
{
Debug.Assert(false, "Reviewed");
return(false);
}
XMVector delta0, delta1;
// Reverse the direction of the test depending on whether the singular vertices are
// the same sign or different signs.
if (b_positiveA ^ b_positiveB)
{
delta0 = bb0 - aa0;
delta1 = aa0 - bb0;
}
else
{
delta0 = aa0 - bb0;
delta1 = bb0 - aa0;
}
// Check if the triangles overlap on the line of intersection between the
// planes of the two triangles by finding the signed line distances.
XMVector dist0 = XMVector3.Dot(delta0, XMVector3.Cross(bb2 - bb0, aa2 - aa0));
if (XMVector4.Greater(dist0, zero))
{
return(false);
}
XMVector dist1 = XMVector3.Dot(delta1, XMVector3.Cross(bb1 - bb0, aa1 - aa0));
if (XMVector4.Greater(dist1, zero))
{
return(false);
}
return(true);
}
