public ContainmentType Contains(BoundingOrientedBox box)
{
if (!box.Intersects(this))
{
return(ContainmentType.Disjoint);
}
XMVector boxCenter = this.center;
XMVector boxExtents = this.extents;
// Subtract off the AABB center to remove a subtract below
XMVector o_center = box.Center - boxCenter;
XMVector o_extents = box.Extents;
XMVector o_orientation = box.Orientation;
Debug.Assert(Internal.XMQuaternionIsUnit(o_orientation), "Reviewed");
XMVector inside = XMVector.TrueInt;
for (int i = 0; i < BoundingOrientedBox.CornerCount; i++)
{
XMVector c = XMVector3.Rotate(o_extents * CollisionGlobalConstants.BoxOffsets[i], o_orientation) + o_center;
XMVector d = c.Abs();
inside = XMVector.AndInt(inside, XMVector.LessOrEqual(d, boxExtents));
}
return(XMVector3.EqualInt(inside, XMVector.TrueInt) ? ContainmentType.Contains : ContainmentType.Intersects);
}
public ContainmentType Contains(BoundingOrientedBox box)
{
if (!box.Intersects(this))
{
return(ContainmentType.Disjoint);
}
XMVector v_center = this.center;
XMVector v_radius = XMVector.Replicate(this.radius);
XMVector radiusSq = v_radius * v_radius;
XMVector boxCenter = box.Center;
XMVector boxExtents = box.Extents;
XMVector boxOrientation = box.Orientation;
Debug.Assert(Internal.XMQuaternionIsUnit(boxOrientation), "Reviewed");
XMVector insideAll = XMVector.TrueInt;
for (int i = 0; i < BoundingOrientedBox.CornerCount; i++)
{
XMVector c = XMVector3.Rotate(boxExtents * CollisionGlobalConstants.BoxOffsets[i], boxOrientation) + boxCenter;
XMVector d = XMVector3.LengthSquare(XMVector.Subtract(v_center, c));
insideAll = XMVector.AndInt(insideAll, XMVector.LessOrEqual(d, radiusSq));
}
return(XMVector3.EqualInt(insideAll, XMVector.TrueInt) ? ContainmentType.Contains : ContainmentType.Intersects);
}
public ContainmentType Contains(BoundingBox box)
{
XMVector centerA = this.center;
XMVector extentsA = this.extents;
XMVector centerB = box.center;
XMVector extentsB = box.extents;
XMVector minA = centerA - extentsA;
XMVector maxA = centerA + extentsA;
XMVector minB = centerB - extentsB;
XMVector maxB = centerB + extentsB;
// for each i in (x, y, z) if a_min(i) > b_max(i) or b_min(i) > a_max(i) then return false
XMVector disjoint = XMVector.OrInt(XMVector.Greater(minA, maxB), XMVector.Greater(minB, maxA));
if (Internal.XMVector3AnyTrue(disjoint))
{
return(ContainmentType.Disjoint);
}
// for each i in (x, y, z) if a_min(i) <= b_min(i) and b_max(i) <= a_max(i) then A contains B
XMVector inside = XMVector.AndInt(XMVector.LessOrEqual(minA, minB), XMVector.LessOrEqual(maxB, maxA));
return(Internal.XMVector3AllTrue(inside) ? ContainmentType.Contains : ContainmentType.Intersects);
}
public ContainmentType Contains(BoundingBox box)
{
if (!box.Intersects(this))
{
return(ContainmentType.Disjoint);
}
XMVector v_center = this.center;
XMVector v_radius = XMVector.Replicate(this.radius);
XMVector radiusSq = v_radius * v_radius;
XMVector boxCenter = box.Center;
XMVector boxExtents = box.Extents;
XMVector insideAll = XMVector.TrueInt;
XMVector offset = boxCenter - v_center;
for (int i = 0; i < BoundingBox.CornerCount; i++)
{
XMVector c = XMVector.MultiplyAdd(boxExtents, CollisionGlobalConstants.BoxOffsets[i], offset);
XMVector d = XMVector3.LengthSquare(c);
insideAll = XMVector.AndInt(insideAll, XMVector.LessOrEqual(d, radiusSq));
}
return(XMVector3.EqualInt(insideAll, XMVector.TrueInt) ? ContainmentType.Contains : ContainmentType.Intersects);
}
public bool Intersects(XMVector origin, XMVector direction, out float distance)
{
Debug.Assert(Internal.XMVector3IsUnit(direction), "Reviewed");
// Load the box.
XMVector v_center = this.center;
XMVector v_extents = this.extents;
// Adjust ray origin to be relative to center of the box.
XMVector t_origin = v_center - origin;
// Compute the dot product againt each axis of the box.
// Since the axii are (1,0,0), (0,1,0), (0,0,1) no computation is necessary.
XMVector axisDotOrigin = t_origin;
XMVector axisDotDirection = direction;
// if (fabs(AxisDotDirection) <= Epsilon) the ray is nearly parallel to the slab.
XMVector isParallel = XMVector.LessOrEqual(axisDotDirection.Abs(), CollisionGlobalConstants.RayEpsilon);
// Test against all three axii simultaneously.
XMVector inverseAxisDotDirection = axisDotDirection.Reciprocal();
XMVector t1 = (axisDotOrigin - v_extents) * inverseAxisDotDirection;
XMVector t2 = (axisDotOrigin + v_extents) * inverseAxisDotDirection;
// Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't
// use the results from any directions parallel to the slab.
XMVector t_min = XMVector.Select(XMVector.Min(t1, t2), CollisionGlobalConstants.FltMin, isParallel);
XMVector t_max = XMVector.Select(XMVector.Max(t1, t2), CollisionGlobalConstants.FltMax, isParallel);
// t_min.x = maximum( t_min.x, t_min.y, t_min.z );
// t_max.x = minimum( t_max.x, t_max.y, t_max.z );
t_min = XMVector.Max(t_min, XMVector.SplatY(t_min)); // x = max(x,y)
t_min = XMVector.Max(t_min, XMVector.SplatZ(t_min)); // x = max(max(x,y),z)
t_max = XMVector.Min(t_max, XMVector.SplatY(t_max)); // x = min(x,y)
t_max = XMVector.Min(t_max, XMVector.SplatZ(t_max)); // x = min(min(x,y),z)
// if ( t_min > t_max ) return false;
XMVector noIntersection = XMVector.Greater(XMVector.SplatX(t_min), XMVector.SplatX(t_max));
// if ( t_max < 0.0f ) return false;
noIntersection = XMVector.OrInt(noIntersection, XMVector.Less(XMVector.SplatX(t_max), XMGlobalConstants.Zero));
// if (IsParallel && (-Extents > AxisDotOrigin || Extents < AxisDotOrigin)) return false;
XMVector parallelOverlap = axisDotOrigin.InBounds(v_extents);
noIntersection = XMVector.OrInt(noIntersection, XMVector.AndComplementInt(isParallel, parallelOverlap));
if (!Internal.XMVector3AnyTrue(noIntersection))
{
// Store the x-component to *pDist
t_min.StoreFloat(out distance);
return(true);
}
distance = 0.0f;
return(false);
}
public bool Intersects(XMVector v0, XMVector v1, XMVector v2)
{
// Load the sphere.
XMVector v_center = this.center;
XMVector v_radius = XMVector.Replicate(this.radius);
// Compute the plane of the triangle (has to be normalized).
XMVector n = XMVector3.Normalize(XMVector3.Cross(v1 - v0, v2 - v0));
// Assert that the triangle is not degenerate.
Debug.Assert(!XMVector3.Equal(n, XMGlobalConstants.Zero), "Reviewed");
// Find the nearest feature on the triangle to the sphere.
XMVector dist = XMVector3.Dot(v_center - v0, n);
// If the center of the sphere is farther from the plane of the triangle than
// the radius of the sphere, then there cannot be an intersection.
XMVector noIntersection = XMVector.Less(dist, -v_radius);
noIntersection = XMVector.OrInt(noIntersection, XMVector.Greater(dist, v_radius));
// Project the center of the sphere onto the plane of the triangle.
XMVector point = v_center - (n * dist);
// Is it inside all the edges? If so we intersect because the distance
// to the plane is less than the radius.
XMVector intersection = Internal.PointOnPlaneInsideTriangle(point, v0, v1, v2);
// Find the nearest point on each edge.
XMVector radiusSq = v_radius * v_radius;
// Edge 0,1
point = Internal.PointOnLineSegmentNearestPoint(v0, v1, v_center);
// If the distance to the center of the sphere to the point is less than
// the radius of the sphere then it must intersect.
intersection = XMVector.OrInt(intersection, XMVector.LessOrEqual(XMVector3.LengthSquare(v_center - point), radiusSq));
// Edge 1,2
point = Internal.PointOnLineSegmentNearestPoint(v1, v2, v_center);
// If the distance to the center of the sphere to the point is less than
// the radius of the sphere then it must intersect.
intersection = XMVector.OrInt(intersection, XMVector.LessOrEqual(XMVector3.LengthSquare(v_center - point), radiusSq));
// Edge 2,0
point = Internal.PointOnLineSegmentNearestPoint(v2, v0, v_center);
// If the distance to the center of the sphere to the point is less than
// the radius of the sphere then it must intersect.
intersection = XMVector.OrInt(intersection, XMVector.LessOrEqual(XMVector3.LengthSquare(v_center - point), radiusSq));
return(XMVector4.EqualInt(XMVector.AndComplementInt(intersection, noIntersection), XMVector.TrueInt));
}
public static XMVector Inverse(XMVector q)
{
XMVector zero = XMVector.Zero;
XMVector l = XMVector4.LengthSquare(q);
XMVector conjugate = XMQuaternion.Conjugate(q);
XMVector control = XMVector.LessOrEqual(l, XMGlobalConstants.Epsilon);
XMVector result = XMVector.Divide(conjugate, l);
result = XMVector.Select(result, zero, control);
return(result);
}
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 ContainmentType Contains(BoundingFrustum fr)
{
if (!fr.Intersects(this))
{
return(ContainmentType.Disjoint);
}
XMVector v_center = this.center;
XMVector v_radius = XMVector.Replicate(this.radius);
XMVector radiusSq = v_radius * v_radius;
XMVector v_origin = fr.Origin;
XMVector v_orientation = fr.Orientation;
Debug.Assert(Internal.XMQuaternionIsUnit(v_orientation), "Reviewed");
// Build the corners of the frustum.
XMVector v_rightTop = new XMVector(fr.RightSlope, fr.TopSlope, 1.0f, 0.0f);
XMVector v_rightBottom = new XMVector(fr.RightSlope, fr.BottomSlope, 1.0f, 0.0f);
XMVector v_leftTop = new XMVector(fr.LeftSlope, fr.TopSlope, 1.0f, 0.0f);
XMVector v_leftBottom = new XMVector(fr.LeftSlope, fr.BottomSlope, 1.0f, 0.0f);
XMVector v_near = XMVector.Replicate(fr.Near);
XMVector v_far = XMVector.Replicate(fr.Far);
XMVector[] corners = new XMVector[BoundingFrustum.CornerCount];
corners[0] = v_rightTop * v_near;
corners[1] = v_rightBottom * v_near;
corners[2] = v_leftTop * v_near;
corners[3] = v_leftBottom * v_near;
corners[4] = v_rightTop * v_far;
corners[5] = v_rightBottom * v_far;
corners[6] = v_leftTop * v_far;
corners[7] = v_leftBottom * v_far;
XMVector insideAll = XMVector.TrueInt;
for (int i = 0; i < BoundingFrustum.CornerCount; i++)
{
XMVector c = XMVector3.Rotate(corners[i], v_orientation) + v_origin;
XMVector d = XMVector3.LengthSquare(XMVector.Subtract(v_center, c));
insideAll = XMVector.AndInt(insideAll, XMVector.LessOrEqual(d, radiusSq));
}
return(XMVector3.EqualInt(insideAll, XMVector.TrueInt) ? ContainmentType.Contains : ContainmentType.Intersects);
}
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 void IntersectPlane(out XMVector linePoint1, out XMVector linePoint2, XMVector p1, XMVector p2)
{
XMVector v1 = XMVector3.Cross(p2, p1);
XMVector lengthSq = XMVector3.LengthSquare(v1);
XMVector v2 = XMVector3.Cross(p2, v1);
XMVector p1W = XMVector.SplatW(p1);
XMVector point = XMVector.Multiply(v2, p1W);
XMVector v3 = XMVector3.Cross(v1, p1);
XMVector p2W = XMVector.SplatW(p2);
point = XMVector.MultiplyAdd(v3, p2W, point);
XMVector lineP1 = XMVector.Divide(point, lengthSq);
XMVector lineP2 = XMVector.Add(lineP1, v1);
XMVector control = XMVector.LessOrEqual(lengthSq, XMGlobalConstants.Epsilon);
linePoint1 = XMVector.Select(lineP1, XMGlobalConstants.QNaN, control);
linePoint2 = XMVector.Select(lineP2, XMGlobalConstants.QNaN, control);
}
public ContainmentType Contains(XMVector v0, XMVector v1, XMVector v2)
{
if (!this.Intersects(v0, v1, v2))
{
return(ContainmentType.Disjoint);
}
XMVector boxCenter = this.center;
XMVector boxExtents = this.extents;
XMVector d = XMVector.Subtract(v0, boxCenter).Abs();
XMVector inside = XMVector.LessOrEqual(d, boxExtents);
d = XMVector.Subtract(v1, boxCenter).Abs();
inside = XMVector.AndInt(inside, XMVector.LessOrEqual(d, boxExtents));
d = XMVector.Subtract(v2, boxCenter).Abs();
inside = XMVector.AndInt(inside, XMVector.LessOrEqual(d, boxExtents));
return(XMVector3.EqualInt(inside, XMVector.TrueInt) ? ContainmentType.Contains : ContainmentType.Intersects);
}
public ContainmentType Contains(XMVector v0, XMVector v1, XMVector v2)
{
if (!this.Intersects(v0, v1, v2))
{
return(ContainmentType.Disjoint);
}
XMVector v_center = this.center;
XMVector v_radius = XMVector.Replicate(this.radius);
XMVector radiusSquared = XMVector.Multiply(v_radius, v_radius);
XMVector distanceSquared = XMVector3.LengthSquare(v0 - v_center);
XMVector inside = XMVector.LessOrEqual(distanceSquared, radiusSquared);
distanceSquared = XMVector3.LengthSquare(v1 - v_center);
inside = XMVector.AndInt(inside, XMVector.LessOrEqual(distanceSquared, radiusSquared));
distanceSquared = XMVector3.LengthSquare(v2 - v_center);
inside = XMVector.AndInt(inside, XMVector.LessOrEqual(distanceSquared, radiusSquared));
return(XMVector3.EqualInt(inside, XMVector.TrueInt) ? ContainmentType.Contains : ContainmentType.Intersects);
}
public ContainmentType Contains(BoundingFrustum fr)
{
if (!fr.Intersects(this))
{
return(ContainmentType.Disjoint);
}
XMFloat3[] corners = fr.GetCorners();
XMVector v_center = this.center;
XMVector v_extents = this.extents;
XMVector inside = XMVector.TrueInt;
for (int i = 0; i < BoundingFrustum.CornerCount; i++)
{
XMVector point = corners[i];
XMVector d = XMVector.Subtract(point, v_center).Abs();
inside = XMVector.AndInt(inside, XMVector.LessOrEqual(d, v_extents));
}
return(XMVector3.EqualInt(inside, XMVector.TrueInt) ? ContainmentType.Contains : ContainmentType.Intersects);
}
