public bool Intersects(XMVector origin, XMVector direction, out float distance)
{
Debug.Assert(Internal.XMVector3IsUnit(direction), "Reviewed");
XMVector v_center = this.center;
XMVector v_radius = XMVector.Replicate(this.radius);
// l is the vector from the ray origin to the center of the sphere.
XMVector l = v_center - origin;
// s is the projection of the l onto the ray direction.
XMVector s = XMVector3.Dot(l, direction);
XMVector l2 = XMVector3.Dot(l, l);
XMVector r2 = v_radius * v_radius;
// m2 is squared distance from the center of the sphere to the projection.
XMVector m2 = l2 - (s * s);
XMVector noIntersection;
// If the ray origin is outside the sphere and the center of the sphere is
// behind the ray origin there is no intersection.
noIntersection = XMVector.AndInt(XMVector.Less(s, XMGlobalConstants.Zero), XMVector.Greater(l2, r2));
// If the squared distance from the center of the sphere to the projection
// is greater than the radius squared the ray will miss the sphere.
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(m2, r2));
// The ray hits the sphere, compute the nearest intersection point.
XMVector q = (r2 - m2).Sqrt();
XMVector t1 = s - q;
XMVector t2 = s + q;
XMVector originInside = XMVector.LessOrEqual(l2, r2);
XMVector t = XMVector.Select(t1, t2, originInside);
if (XMVector4.NotEqualInt(noIntersection, XMVector.TrueInt))
{
// Store the x-component to *pDist.
t.StoreFloat(out distance);
return(true);
}
distance = 0.0f;
return(false);
}
public static void FastIntersectAxisAlignedBoxPlane(XMVector center, XMVector extents, XMVector plane, out XMVector outside, out XMVector inside)
{
// Compute the distance to the center of the box.
XMVector dist = XMVector4.Dot(center, plane);
// Project the axes of the box onto the normal of the plane. Half the
// length of the projection (sometime called the "radius") is equal to
// h(u) * abs(n dot b(u))) + h(v) * abs(n dot b(v)) + h(w) * abs(n dot b(w))
// where h(i) are extents of the box, n is the plane normal, and b(i) are the
// axes of the box. In this case b(i) = [(1,0,0), (0,1,0), (0,0,1)].
XMVector radius = XMVector3.Dot(extents, plane.Abs());
// Outside the plane?
outside = XMVector.Greater(dist, radius);
// Fully inside the plane?
inside = XMVector.Less(dist, -radius);
}
public ContainmentType Contains(BoundingSphere sh)
{
XMVector sphereCenter = sh.Center;
XMVector sphereRadius = XMVector.Replicate(sh.Radius);
XMVector boxCenter = this.center;
XMVector boxExtents = this.extents;
XMVector boxMin = boxCenter - boxExtents;
XMVector boxMax = boxCenter + boxExtents;
//// Find the distance to the nearest point on the box.
//// for each i in (x, y, z)
//// if (SphereCenter(i) < BoxMin(i)) d2 += (SphereCenter(i) - BoxMin(i)) ^ 2
//// else if (SphereCenter(i) > BoxMax(i)) d2 += (SphereCenter(i) - BoxMax(i)) ^ 2
XMVector d = XMGlobalConstants.Zero;
// Compute d for each dimension.
XMVector lessThanMin = XMVector.Less(sphereCenter, boxMin);
XMVector greaterThanMax = XMVector.Greater(sphereCenter, boxMax);
XMVector minDelta = sphereCenter - boxMin;
XMVector maxDelta = sphereCenter - boxMax;
// Choose value for each dimension based on the comparison.
d = XMVector.Select(d, minDelta, lessThanMin);
d = XMVector.Select(d, maxDelta, greaterThanMax);
// Use a dot-product to square them and sum them together.
XMVector d2 = XMVector3.Dot(d, d);
if (XMVector3.Greater(d2, XMVector.Multiply(sphereRadius, sphereRadius)))
{
return(ContainmentType.Disjoint);
}
XMVector insideAll = XMVector.LessOrEqual(boxMin + sphereRadius, sphereCenter);
insideAll = XMVector.AndInt(insideAll, XMVector.LessOrEqual(sphereCenter, boxMax - sphereRadius));
insideAll = XMVector.AndInt(insideAll, XMVector.Greater(boxMax - boxMin, sphereRadius));
return(XMVector3.EqualInt(insideAll, XMVector.TrueInt) ? ContainmentType.Contains : ContainmentType.Intersects);
}
public static XMVector PointOnPlaneInsideTriangle(XMVector p, XMVector v0, XMVector v1, XMVector v2)
{
// Compute the triangle normal.
XMVector n = XMVector3.Cross(v2 - v0, v1 - v0);
// Compute the cross products of the vector from the base of each edge to
// the point with each edge vector.
XMVector c0 = XMVector3.Cross(p - v0, v1 - v0);
XMVector c1 = XMVector3.Cross(p - v1, v2 - v1);
XMVector c2 = XMVector3.Cross(p - v2, v0 - v2);
// If the cross product points in the same direction as the normal the the
// point is inside the edge (it is zero if is on the edge).
XMVector zero = XMGlobalConstants.Zero;
XMVector inside0 = XMVector.GreaterOrEqual(XMVector3.Dot(c0, n), zero);
XMVector inside1 = XMVector.GreaterOrEqual(XMVector3.Dot(c1, n), zero);
XMVector inside2 = XMVector.GreaterOrEqual(XMVector3.Dot(c2, n), zero);
// If the point inside all of the edges it is inside.
return(XMVector.AndInt(XMVector.AndInt(inside0, inside1), inside2));
}
public static XMVector PointOnLineSegmentNearestPoint(XMVector s1, XMVector s2, XMVector p)
{
XMVector dir = s1 - s1;
XMVector projection = XMVector3.Dot(p, dir) - XMVector3.Dot(s1, dir);
XMVector lengthSq = XMVector3.Dot(dir, dir);
XMVector t = projection * lengthSq.Reciprocal();
XMVector point = s1 + (t * dir);
// t < 0
XMVector selectS1 = XMVector.Less(projection, XMGlobalConstants.Zero);
point = XMVector.Select(point, s1, selectS1);
// t > 1
XMVector selectS2 = XMVector.Greater(projection, lengthSq);
point = XMVector.Select(point, s2, selectS2);
return(point);
}
public static bool CalculateEigenVectors(
float m11,
float m12,
float m13,
float m22,
float m23,
float m33,
float e1,
float e2,
float e3,
out XMVector pV1,
out XMVector pV2,
out XMVector pV3)
{
pV1 = Internal.CalculateEigenVector(m11, m12, m13, m22, m23, m33, e1);
pV2 = Internal.CalculateEigenVector(m11, m12, m13, m22, m23, m33, e2);
pV3 = Internal.CalculateEigenVector(m11, m12, m13, m22, m23, m33, e3);
bool v1z = false;
bool v2z = false;
bool v3z = false;
XMVector zero = XMGlobalConstants.Zero;
if (XMVector3.Equal(pV1, zero))
{
v1z = true;
}
if (XMVector3.Equal(pV2, zero))
{
v2z = true;
}
if (XMVector3.Equal(pV3, zero))
{
v3z = true;
}
// check for non-orthogonal vectors
bool e12 = Math.Abs(XMVector3.Dot(pV1, pV2).X) > 0.1f;
bool e13 = Math.Abs(XMVector3.Dot(pV1, pV3).X) > 0.1f;
bool e23 = Math.Abs(XMVector3.Dot(pV2, pV3).X) > 0.1f;
// all eigenvectors are 0- any basis set
if ((v1z && v2z && v3z) || (e12 && e13 && e23) ||
(e12 && v3z) || (e13 && v2z) || (e23 && v1z))
{
pV1 = XMGlobalConstants.IdentityR0;
pV2 = XMGlobalConstants.IdentityR1;
pV3 = XMGlobalConstants.IdentityR2;
return(true);
}
if (v1z && v2z)
{
XMVector v_tmp = XMVector3.Cross(XMGlobalConstants.IdentityR1, pV3);
if (XMVector3.LengthSquare(v_tmp).X < 1e-5f)
{
v_tmp = XMVector3.Cross(XMGlobalConstants.IdentityR0, pV3);
}
pV1 = XMVector3.Normalize(v_tmp);
pV2 = XMVector3.Cross(pV3, pV1);
return(true);
}
if (v3z && v1z)
{
XMVector v_tmp = XMVector3.Cross(XMGlobalConstants.IdentityR1, pV2);
if (XMVector3.LengthSquare(v_tmp).X < 1e-5f)
{
v_tmp = XMVector3.Cross(XMGlobalConstants.IdentityR0, pV2);
}
pV3 = XMVector3.Normalize(v_tmp);
pV1 = XMVector3.Cross(pV2, pV3);
return(true);
}
if (v2z && v3z)
{
XMVector v_tmp = XMVector3.Cross(XMGlobalConstants.IdentityR1, pV1);
if (XMVector3.LengthSquare(v_tmp).X < 1e-5f)
{
v_tmp = XMVector3.Cross(XMGlobalConstants.IdentityR0, pV1);
}
pV2 = XMVector3.Normalize(v_tmp);
pV3 = XMVector3.Cross(pV1, pV2);
return(true);
}
if (v1z || e12)
{
pV1 = XMVector3.Cross(pV2, pV3);
return(true);
}
if (v2z || e23)
{
pV2 = XMVector3.Cross(pV3, pV1);
return(true);
}
if (v3z || e13)
{
pV3 = XMVector3.Cross(pV1, pV2);
return(true);
}
return(true);
}
public static bool Intersects(XMVector origin, XMVector direction, XMVector v0, XMVector v1, XMVector v2, out float uCoordinate, out float vCoordinate, out float distance)
{
Debug.Assert(Internal.XMVector3IsUnit(direction), "Reviewed");
XMVector zero = XMGlobalConstants.Zero;
XMVector e1 = v1 - v0;
XMVector e2 = v2 - v0;
// p = Direction ^ e2;
XMVector p = XMVector3.Cross(direction, e2);
// det = e1 * p;
XMVector det = XMVector3.Dot(e1, p);
XMVector u, v, t;
if (XMVector3.GreaterOrEqual(det, CollisionGlobalConstants.RayEpsilon))
{
// Determinate is positive (front side of the triangle).
XMVector s = origin - v0;
// u = s * p;
u = XMVector3.Dot(s, p);
XMVector noIntersection = XMVector.Less(u, zero);
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(u, det));
// q = s ^ e1;
XMVector q = XMVector3.Cross(s, e1);
// v = Direction * q;
v = XMVector3.Dot(direction, q);
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(v, zero));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(u + v, det));
// t = e2 * q;
t = XMVector3.Dot(e2, q);
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(t, zero));
if (XMVector4.EqualInt(noIntersection, XMVector.TrueInt))
{
uCoordinate = 0.0f;
vCoordinate = 0.0f;
distance = 0.0f;
return(false);
}
}
else if (XMVector3.LessOrEqual(det, CollisionGlobalConstants.RayNegEpsilon))
{
// Determinate is negative (back side of the triangle).
XMVector s = origin - v0;
// u = s * p;
u = XMVector3.Dot(s, p);
XMVector noIntersection = XMVector.Greater(u, zero);
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(u, det));
// q = s ^ e1;
XMVector q = XMVector3.Cross(s, e1);
// v = Direction * q;
v = XMVector3.Dot(direction, q);
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(v, zero));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(u + v, det));
// t = e2 * q;
t = XMVector3.Dot(e2, q);
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(t, zero));
if (XMVector4.EqualInt(noIntersection, XMVector.TrueInt))
{
uCoordinate = 0.0f;
vCoordinate = 0.0f;
distance = 0.0f;
return(false);
}
}
else
{
// Parallel ray.
uCoordinate = 0.0f;
vCoordinate = 0.0f;
distance = 0.0f;
return(false);
}
// (u / det) and (v / dev) are the barycentric coordinates of the intersection.
u = XMVector.Divide(u, det);
v = XMVector.Divide(v, det);
t = XMVector.Divide(t, det);
// Store the x-component to *pDist
u.StoreFloat(out uCoordinate);
v.StoreFloat(out vCoordinate);
t.StoreFloat(out distance);
return(true);
}
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);
}
public bool Intersects(XMVector v0, XMVector v1, XMVector v2)
{
XMVector zero = XMGlobalConstants.Zero;
// Load the box.
XMVector v_center = this.center;
XMVector v_extents = this.extents;
XMVector boxMin = v_center - v_extents;
XMVector boxMax = v_center + v_extents;
// Test the axes of the box (in effect test the AAB against the minimal AAB
// around the triangle).
XMVector triMin = XMVector.Min(XMVector.Min(v0, v1), v2);
XMVector triMax = XMVector.Max(XMVector.Max(v0, v1), v2);
// for each i in (x, y, z) if a_min(i) > b_max(i) or b_min(i) > a_max(i) then disjoint
XMVector disjoint = XMVector.OrInt(XMVector.Greater(triMin, boxMax), XMVector.Greater(boxMin, triMax));
if (Internal.XMVector3AnyTrue(disjoint))
{
return(false);
}
// Test the plane of the triangle.
XMVector normal = XMVector3.Cross(v1 - v0, v2 - v0);
XMVector dist = XMVector3.Dot(normal, v0);
// Assert that the triangle is not degenerate.
Debug.Assert(!XMVector3.Equal(normal, zero), "Reviewed");
// for each i in (x, y, z) if n(i) >= 0 then v_min(i)=b_min(i), v_max(i)=b_max(i)
// else v_min(i)=b_max(i), v_max(i)=b_min(i)
XMVector normalSelect = XMVector.Greater(normal, zero);
XMVector v_min = XMVector.Select(boxMax, boxMin, normalSelect);
XMVector v_max = XMVector.Select(boxMin, boxMax, normalSelect);
// if n dot v_min + d > 0 || n dot v_max + d < 0 then disjoint
XMVector minDist = XMVector3.Dot(v_min, normal);
XMVector maxDist = XMVector3.Dot(v_max, normal);
XMVector noIntersection = XMVector.Greater(minDist, dist);
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(maxDist, dist));
// Move the box center to zero to simplify the following tests.
XMVector tV0 = v0 - v_center;
XMVector tV1 = v1 - v_center;
XMVector tV2 = v2 - v_center;
// Test the edge/edge axes (3*3).
XMVector e0 = tV1 - tV0;
XMVector e1 = tV2 - tV1;
XMVector e2 = tV0 - tV2;
// Make w zero.
e0.W = zero.W;
e1.W = zero.W;
e2.W = zero.W;
XMVector axis;
XMVector p0, p1, p2;
XMVector min, max;
XMVector radius;
//// Axis == (1,0,0) x e0 = (0, -e0.z, e0.y)
axis = new XMVector(e1.W, -e0.Z, e1.Y, e1.X);
p0 = XMVector3.Dot(tV0, axis);
//// p1 = XMVector3Dot( V1, Axis ); // p1 = p0;
p2 = XMVector3.Dot(tV2, axis);
min = XMVector.Min(p0, p2);
max = XMVector.Max(p0, p2);
radius = XMVector3.Dot(v_extents, axis.Abs());
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(min, radius));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(max, -radius));
//// Axis == (1,0,0) x e1 = (0, -e1.z, e1.y)
axis = new XMVector(e1.W, -e1.Z, e1.Y, e1.X);
p0 = XMVector3.Dot(tV0, axis);
p1 = XMVector3.Dot(tV1, axis);
//// p2 = XMVector3Dot( V2, Axis ); // p2 = p1;
min = XMVector.Min(p0, p1);
max = XMVector.Max(p0, p1);
radius = XMVector3.Dot(v_extents, axis.Abs());
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(min, radius));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(max, -radius));
//// Axis == (1,0,0) x e2 = (0, -e2.z, e2.y)
axis = new XMVector(e2.W, -e2.Z, e2.Y, e2.X);
p0 = XMVector3.Dot(tV0, axis);
p1 = XMVector3.Dot(tV1, axis);
//// p2 = XMVector3Dot( V2, Axis ); // p2 = p0;
min = XMVector.Min(p0, p1);
max = XMVector.Max(p0, p1);
radius = XMVector3.Dot(v_extents, axis.Abs());
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(min, radius));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(max, -radius));
//// Axis == (0,1,0) x e0 = (e0.z, 0, -e0.x)
axis = new XMVector(e0.Z, e0.W, -e0.X, e0.Y);
p0 = XMVector3.Dot(tV0, axis);
//// p1 = XMVector3Dot( V1, Axis ); // p1 = p0;
p2 = XMVector3.Dot(tV2, axis);
min = XMVector.Min(p0, p2);
max = XMVector.Max(p0, p2);
radius = XMVector3.Dot(v_extents, axis.Abs());
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(min, radius));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(max, -radius));
//// Axis == (0,1,0) x e1 = (e1.z, 0, -e1.x)
axis = new XMVector(e1.Z, e1.W, -e1.X, e1.Y);
p0 = XMVector3.Dot(tV0, axis);
p1 = XMVector3.Dot(tV1, axis);
//// p2 = XMVector3Dot( V2, Axis ); // p2 = p1;
min = XMVector.Min(p0, p1);
max = XMVector.Max(p0, p1);
radius = XMVector3.Dot(v_extents, axis.Abs());
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(min, radius));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(max, -radius));
//// Axis == (0,0,1) x e2 = (e2.z, 0, -e2.x)
axis = new XMVector(e2.Z, e2.W, -e2.X, e2.Y);
p0 = XMVector3.Dot(tV0, axis);
p1 = XMVector3.Dot(tV1, axis);
//// p2 = XMVector3Dot( V2, Axis ); // p2 = p0;
min = XMVector.Min(p0, p1);
max = XMVector.Max(p0, p1);
radius = XMVector3.Dot(v_extents, axis.Abs());
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(min, radius));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(max, -radius));
//// Axis == (0,0,1) x e0 = (-e0.y, e0.x, 0)
axis = new XMVector(-e0.Y, e0.X, e0.W, e0.Z);
p0 = XMVector3.Dot(tV0, axis);
//// p1 = XMVector3Dot( V1, Axis ); // p1 = p0;
p2 = XMVector3.Dot(tV2, axis);
min = XMVector.Min(p0, p2);
max = XMVector.Max(p0, p2);
radius = XMVector3.Dot(v_extents, axis.Abs());
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(min, radius));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(max, -radius));
//// Axis == (0,0,1) x e1 = (-e1.y, e1.x, 0)
axis = new XMVector(-e1.Y, e1.X, e1.W, e1.Z);
p0 = XMVector3.Dot(tV0, axis);
p1 = XMVector3.Dot(tV1, axis);
//// p2 = XMVector3Dot( V2, Axis ); // p2 = p1;
min = XMVector.Min(p0, p1);
max = XMVector.Max(p0, p1);
radius = XMVector3.Dot(v_extents, axis.Abs());
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(min, radius));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(max, -radius));
//// Axis == (0,0,1) x e2 = (-e2.y, e2.x, 0)
axis = new XMVector(-e2.Y, e2.X, e2.W, e2.Z);
p0 = XMVector3.Dot(tV0, axis);
p1 = XMVector3.Dot(tV1, axis);
//// p2 = XMVector3Dot( V2, Axis ); // p2 = p0;
min = XMVector.Min(p0, p1);
max = XMVector.Max(p0, p1);
radius = XMVector3.Dot(v_extents, axis.Abs());
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(min, radius));
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(max, -radius));
return(XMVector4.NotEqualInt(noIntersection, XMVector.TrueInt));
}
