public BoundingBox Transform(XMMatrix m)
{
// Load center and extents.
XMVector boxCenter = this.center;
XMVector boxExtents = this.extents;
// Compute and transform the corners and find new min/max bounds.
XMVector corner = XMVector.MultiplyAdd(boxExtents, CollisionGlobalConstants.BoxOffsets[0], boxCenter);
corner = XMVector3.Transform(corner, m);
XMVector min, max;
min = max = corner;
for (int i = 1; i < BoundingBox.CornerCount; i++)
{
corner = XMVector.MultiplyAdd(boxExtents, CollisionGlobalConstants.BoxOffsets[i], boxCenter);
corner = XMVector3.Transform(corner, m);
min = XMVector.Min(min, corner);
max = XMVector.Max(max, corner);
}
// Store center and extents.
return(new BoundingBox((min + max) * 0.5f, (max - min) * 0.5f));
}
public static XMVector FresnelTerm(XMVector cosIncidentAngle, XMVector refractionIndex)
{
Debug.Assert(!XMVector4.IsInfinite(cosIncidentAngle), "Reviewed");
//// Result = 0.5f * (g - c)^2 / (g + c)^2 * ((c * (g + c) - 1)^2 / (c * (g - c) + 1)^2 + 1) where
//// c = CosIncidentAngle
//// g = sqrt(c^2 + RefractionIndex^2 - 1)
XMVector g = XMVector.MultiplyAdd(refractionIndex, refractionIndex, XMGlobalConstants.NegativeOne);
g = XMVector.MultiplyAdd(cosIncidentAngle, cosIncidentAngle, g);
g = g.Abs().Sqrt();
XMVector s = XMVector.Add(g, cosIncidentAngle);
XMVector d = XMVector.Subtract(g, cosIncidentAngle);
XMVector v0 = XMVector.Multiply(d, d);
XMVector v1 = XMVector.Multiply(s, s).Reciprocal();
v0 = XMVector.Multiply(XMGlobalConstants.OneHalf, v0);
v0 = XMVector.Multiply(v0, v1);
XMVector v2 = XMVector.MultiplyAdd(cosIncidentAngle, s, XMGlobalConstants.NegativeOne);
XMVector v3 = XMVector.MultiplyAdd(cosIncidentAngle, d, XMGlobalConstants.One);
v2 = XMVector.Multiply(v2, v2);
v3 = XMVector.Multiply(v3, v3);
v3 = v3.Reciprocal();
v2 = XMVector.MultiplyAdd(v2, v3, XMGlobalConstants.One);
return(XMVector.Multiply(v0, v2).Saturate());
}
public BoundingBox Transform(float scale, XMVector rotation, XMVector translation)
{
Debug.Assert(Internal.XMQuaternionIsUnit(rotation), "Reviewed");
// Load center and extents.
XMVector boxCenter = this.center;
XMVector boxExtents = this.extents;
XMVector vectorScale = XMVector.Replicate(scale);
// Compute and transform the corners and find new min/max bounds.
XMVector corner = XMVector.MultiplyAdd(boxExtents, CollisionGlobalConstants.BoxOffsets[0], boxCenter);
corner = XMVector3.Rotate(corner * vectorScale, rotation) + translation;
XMVector min, max;
min = max = corner;
for (int i = 1; i < BoundingBox.CornerCount; i++)
{
corner = XMVector.MultiplyAdd(boxExtents, CollisionGlobalConstants.BoxOffsets[i], boxCenter);
corner = XMVector3.Rotate(corner * vectorScale, rotation) + translation;
min = XMVector.Min(min, corner);
max = XMVector.Max(max, corner);
}
// Store center and extents.
return(new BoundingBox((min + max) * 0.5f, (max - min) * 0.5f));
}
public static XMVector Unproject(
XMVector v,
float viewportX,
float viewportY,
float viewportWidth,
float viewportHeight,
float viewportMinZ,
float viewportMaxZ,
XMMatrix projection,
XMMatrix view,
XMMatrix world)
{
XMVector d = XMVector.FromFloat(-1.0f, 1.0f, 0.0f, 0.0f);
XMVector scale = new XMVector(viewportWidth * 0.5f, -viewportHeight * 0.5f, viewportMaxZ - viewportMinZ, 1.0f);
scale = scale.Reciprocal();
XMVector offset = new XMVector(-viewportX, -viewportY, -viewportMinZ, 0.0f);
offset = XMVector.MultiplyAdd(scale, offset, d);
XMMatrix transform = XMMatrix.Multiply(world, view);
transform = XMMatrix.Multiply(transform, projection);
transform = transform.Inverse();
XMVector result = XMVector.MultiplyAdd(v, scale, offset);
return(XMVector3.TransformCoord(result, transform));
}
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 static XMVector Project(
XMVector v,
float viewportX,
float viewportY,
float viewportWidth,
float viewportHeight,
float viewportMinZ,
float viewportMaxZ,
XMMatrix projection,
XMMatrix view,
XMMatrix world)
{
float halfViewportWidth = viewportWidth * 0.5f;
float halfViewportHeight = viewportHeight * 0.5f;
XMVector scale = new XMVector(halfViewportWidth, -halfViewportHeight, viewportMaxZ - viewportMinZ, 0.0f);
XMVector offset = new XMVector(viewportX + halfViewportWidth, viewportY + halfViewportHeight, viewportMinZ, 0.0f);
XMMatrix transform = XMMatrix.Multiply(world, view);
transform = XMMatrix.Multiply(transform, projection);
XMVector result = XMVector3.TransformCoord(v, transform);
result = XMVector.MultiplyAdd(result, scale, offset);
return(result);
}
public static XMVector RotationRollPitchYawFromVector(XMVector angles)
{
XMVector sign = XMVector.FromFloat(1.0f, -1.0f, -1.0f, 1.0f);
XMVector halfAngles = XMVector.Multiply(angles, XMGlobalConstants.OneHalf);
XMVector sinAngles;
XMVector cosAngles;
halfAngles.SinCos(out sinAngles, out cosAngles);
XMVector p0 = new XMVector(sinAngles.X, cosAngles.X, cosAngles.X, cosAngles.X);
XMVector y0 = new XMVector(cosAngles.Y, sinAngles.Y, cosAngles.Y, cosAngles.Y);
XMVector r0 = new XMVector(cosAngles.Z, cosAngles.Z, sinAngles.Z, cosAngles.Z);
XMVector p1 = new XMVector(cosAngles.X, sinAngles.X, sinAngles.X, sinAngles.X);
XMVector y1 = new XMVector(sinAngles.Y, cosAngles.Y, sinAngles.Y, sinAngles.Y);
XMVector r1 = new XMVector(sinAngles.Z, sinAngles.Z, cosAngles.Z, sinAngles.Z);
XMVector q1 = XMVector.Multiply(p1, sign);
XMVector q0 = XMVector.Multiply(p0, y0);
q1 = XMVector.Multiply(q1, y1);
q0 = XMVector.Multiply(q0, r0);
return(XMVector.MultiplyAdd(q1, r1, q0));
}
public static XMVector RefractV(XMVector incident, XMVector normal, XMVector refractionIndex)
{
//// Result = RefractionIndex * Incident - Normal * (RefractionIndex * dot(Incident, Normal) +
//// sqrt(1 - RefractionIndex * RefractionIndex * (1 - dot(Incident, Normal) * dot(Incident, Normal))))
XMVector zero = XMVector.Zero;
XMVector incidentDotNormal = XMVector4.Dot(incident, normal);
// R = 1.0f - RefractionIndex * RefractionIndex * (1.0f - IDotN * IDotN)
XMVector r = XMVector.NegativeMultiplySubtract(incidentDotNormal, incidentDotNormal, XMGlobalConstants.One);
r = XMVector.Multiply(r, refractionIndex);
r = XMVector.NegativeMultiplySubtract(r, refractionIndex, XMGlobalConstants.One);
if (XMVector4.LessOrEqual(r, zero))
{
// Total internal reflection
return(zero);
}
else
{
// R = RefractionIndex * IDotN + sqrt(R)
r = r.Sqrt();
r = XMVector.MultiplyAdd(refractionIndex, incidentDotNormal, r);
// Result = RefractionIndex * Incident - Normal * R
XMVector result = XMVector.Multiply(refractionIndex, incident);
result = XMVector.NegativeMultiplySubtract(normal, r, result);
return(result);
}
}
public static XMVector TransformNormal(XMVector v, XMMatrix m)
{
XMVector y = XMVector.SplatY(v);
XMVector x = XMVector.SplatX(v);
XMVector result = XMVector.Multiply(y, ((XMVector *)&m)[1]);
result = XMVector.MultiplyAdd(x, ((XMVector *)&m)[0], result);
return(result);
}
public static XMVector TransformCoord(XMVector v, XMMatrix m)
{
XMVector y = XMVector.SplatY(v);
XMVector x = XMVector.SplatX(v);
XMVector result = XMVector.MultiplyAdd(y, ((XMVector *)&m)[1], ((XMVector *)&m)[3]);
result = XMVector.MultiplyAdd(x, ((XMVector *)&m)[0], result);
XMVector w = XMVector.SplatW(result);
return(XMVector.Divide(result, w));
}
public static XMVector Transform(XMVector v, XMMatrix m)
{
XMVector z = XMVector.SplatZ(v);
XMVector y = XMVector.SplatY(v);
XMVector x = XMVector.SplatX(v);
XMVector result = XMVector.MultiplyAdd(z, ((XMVector *)&m)[2], ((XMVector *)&m)[3]);
result = XMVector.MultiplyAdd(y, ((XMVector *)&m)[1], result);
result = XMVector.MultiplyAdd(x, ((XMVector *)&m)[0], result);
return(result);
}
public static XMVector SlerpV(XMVector q0, XMVector q1, XMVector t)
{
Debug.Assert(t.Y == t.X && t.Z == t.X && t.W == t.X, "Reviewed");
//// Result = Q0 * sin((1.0 - t) * Omega) / sin(Omega) + Q1 * sin(t * Omega) / sin(Omega)
XMVector oneMinusEpsilon = XMVector.FromFloat(1.0f - 0.00001f, 1.0f - 0.00001f, 1.0f - 0.00001f, 1.0f - 0.00001f);
XMVector cosOmega = XMQuaternion.Dot(q0, q1);
XMVector zero = XMVector.Zero;
XMVector control = XMVector.Less(cosOmega, zero);
XMVector sign = XMVector.Select(XMGlobalConstants.One, XMGlobalConstants.NegativeOne, control);
cosOmega = XMVector.Multiply(cosOmega, sign);
control = XMVector.Less(cosOmega, oneMinusEpsilon);
XMVector sinOmega = XMVector
.NegativeMultiplySubtract(cosOmega, cosOmega, XMGlobalConstants.One)
.Sqrt();
XMVector omega = XMVector.ATan2(sinOmega, cosOmega);
XMVector signMask = XMVector.SignMask;
XMVector v01 = XMVector.ShiftLeft(t, zero, 2);
signMask = XMVector.ShiftLeft(signMask, zero, 3);
v01 = XMVector.XorInt(v01, signMask);
v01 = XMVector.Add(XMGlobalConstants.IdentityR0, v01);
XMVector invSinOmega = sinOmega.Reciprocal();
XMVector s0 = XMVector
.Multiply(v01, omega)
.Sin();
s0 = XMVector.Multiply(s0, invSinOmega);
s0 = XMVector.Select(v01, s0, control);
XMVector s1 = XMVector.SplatY(s0);
s0 = XMVector.SplatX(s0);
s1 = XMVector.Multiply(s1, sign);
XMVector result = XMVector.Multiply(q0, s0);
result = XMVector.MultiplyAdd(q1, s1, result);
return(result);
}
public XMFloat3[] GetCorners()
{
XMFloat3[] corners = new XMFloat3[BoundingBox.CornerCount];
// Load the box
XMVector boxCenter = this.center;
XMVector boxExtents = this.extents;
for (int i = 0; i < BoundingBox.CornerCount; i++)
{
corners[i] = XMVector.MultiplyAdd(boxExtents, CollisionGlobalConstants.BoxOffsets[i], boxCenter);
}
return(corners);
}
public static XMVector IntersectLine(XMVector p, XMVector linePoint1, XMVector linePoint2)
{
XMVector v1 = XMVector3.Dot(p, linePoint1);
XMVector v2 = XMVector3.Dot(p, linePoint2);
XMVector d = XMVector.Subtract(v1, v2);
XMVector vt = XMPlane.DotCoord(p, linePoint1);
vt = XMVector.Divide(vt, d);
XMVector point = XMVector.Subtract(linePoint2, linePoint1);
point = XMVector.MultiplyAdd(point, vt, linePoint1);
XMVector control = XMVector.NearEqual(d, XMGlobalConstants.Zero, XMGlobalConstants.Epsilon);
return(XMVector.Select(point, XMGlobalConstants.QNaN, control));
}
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);
}
private static XMVector HueToClr(XMVector p, XMVector q, XMVector h)
{
XMVector oneSixth = XMVector.FromFloat(1.0f / 6.0f, 1.0f / 6.0f, 1.0f / 6.0f, 1.0f / 6.0f);
XMVector twoThirds = XMVector.FromFloat(2.0f / 3.0f, 2.0f / 3.0f, 2.0f / 3.0f, 2.0f / 3.0f);
XMVector t = h;
if (XMVector3.Less(t, XMGlobalConstants.Zero))
{
t = XMVector.Add(t, XMGlobalConstants.One);
}
if (XMVector3.Greater(t, XMGlobalConstants.One))
{
t = XMVector.Subtract(t, XMGlobalConstants.One);
}
if (XMVector3.Less(t, oneSixth))
{
// p + (q - p) * 6 * t
XMVector t1 = XMVector.Subtract(q, p);
XMVector t2 = XMVector.Multiply(XMGlobalConstants.Six, t);
return(XMVector.MultiplyAdd(t1, t2, p));
}
if (XMVector3.Less(t, XMGlobalConstants.OneHalf))
{
return(q);
}
if (XMVector3.Less(t, twoThirds))
{
// p + (q - p) * 6 * (2/3 - t)
XMVector t1 = XMVector.Subtract(q, p);
XMVector t2 = XMVector.Multiply(XMGlobalConstants.Six, XMVector.Subtract(twoThirds, t));
return(XMVector.MultiplyAdd(t1, t2, p));
}
return(p);
}
public static XMVector IntersectLine(XMVector line1Point1, XMVector line1Point2, XMVector line2Point1, XMVector line2Point2)
{
XMVector v1 = XMVector.Subtract(line1Point2, line1Point1);
XMVector v2 = XMVector.Subtract(line2Point2, line2Point1);
XMVector v3 = XMVector.Subtract(line1Point1, line2Point1);
XMVector c1 = XMVector2.Cross(v1, v2);
XMVector c2 = XMVector2.Cross(v2, v3);
XMVector result;
XMVector zero = XMVector.Zero;
if (XMVector2.NearEqual(c1, zero, XMGlobalConstants.Epsilon))
{
if (XMVector2.NearEqual(c2, zero, XMGlobalConstants.Epsilon))
{
// Coincident
result = XMGlobalConstants.Infinity;
}
else
{
// Parallel
result = XMGlobalConstants.QNaN;
}
}
else
{
//// Intersection point = Line1Point1 + V1 * (C2 / C1)
XMVector scale = c1.Reciprocal();
scale = XMVector.Multiply(c2, scale);
result = XMVector.MultiplyAdd(v1, scale, line1Point1);
}
return(result);
}
private void Collide()
{
// test collisions between objects and frustum
this.secondarySpheres[0].Collision = this.primaryFrustum.Contains(this.secondarySpheres[0].Sphere);
this.secondaryOrientedBoxes[0].Collision = this.primaryFrustum.Contains(this.secondaryOrientedBoxes[0].Box);
this.secondaryAABoxes[0].Collision = this.primaryFrustum.Contains(this.secondaryAABoxes[0].Box);
this.secondaryTriangles[0].Collision = this.primaryFrustum.Contains(this.secondaryTriangles[0].PointA, this.secondaryTriangles[0].PointB, this.secondaryTriangles[0].PointC);
// test collisions between objects and aligned box
this.secondarySpheres[1].Collision = this.primaryAABox.Contains(this.secondarySpheres[1].Sphere);
this.secondaryOrientedBoxes[1].Collision = this.primaryAABox.Contains(this.secondaryOrientedBoxes[1].Box);
this.secondaryAABoxes[1].Collision = this.primaryAABox.Contains(this.secondaryAABoxes[1].Box);
this.secondaryTriangles[1].Collision = this.primaryAABox.Contains(this.secondaryTriangles[1].PointA, this.secondaryTriangles[1].PointB, this.secondaryTriangles[1].PointC);
// test collisions between objects and oriented box
this.secondarySpheres[2].Collision = this.primaryOrientedBox.Contains(this.secondarySpheres[2].Sphere);
this.secondaryOrientedBoxes[2].Collision = this.primaryOrientedBox.Contains(this.secondaryOrientedBoxes[2].Box);
this.secondaryAABoxes[2].Collision = this.primaryOrientedBox.Contains(this.secondaryAABoxes[2].Box);
this.secondaryTriangles[2].Collision = this.primaryOrientedBox.Contains(this.secondaryTriangles[2].PointA, this.secondaryTriangles[2].PointB, this.secondaryTriangles[2].PointC);
// test collisions between objects and ray
float fDistance = -1.0f;
float d;
if (this.secondarySpheres[3].Sphere.Intersects(
this.primaryRay.Origin,
this.primaryRay.Direction,
out d))
{
this.secondarySpheres[3].Collision = ContainmentType.Intersects;
fDistance = d;
}
else
{
this.secondarySpheres[3].Collision = ContainmentType.Disjoint;
}
if (this.secondaryOrientedBoxes[3].Box.Intersects(
this.primaryRay.Origin,
this.primaryRay.Direction,
out d))
{
this.secondaryOrientedBoxes[3].Collision = ContainmentType.Intersects;
fDistance = d;
}
else
{
this.secondaryOrientedBoxes[3].Collision = ContainmentType.Disjoint;
}
if (this.secondaryAABoxes[3].Box.Intersects(
this.primaryRay.Origin,
this.primaryRay.Direction,
out d))
{
this.secondaryAABoxes[3].Collision = ContainmentType.Intersects;
fDistance = d;
}
else
{
this.secondaryAABoxes[3].Collision = ContainmentType.Disjoint;
}
if (TriangleTest.Intersects(
this.primaryRay.Origin,
this.primaryRay.Direction,
this.secondaryTriangles[3].PointA,
this.secondaryTriangles[3].PointB,
this.secondaryTriangles[3].PointC,
out d))
{
this.secondaryTriangles[3].Collision = ContainmentType.Intersects;
fDistance = d;
}
else
{
this.secondaryTriangles[3].Collision = ContainmentType.Disjoint;
}
// If one of the ray intersection tests was successful, fDistance will be positive.
// If so, compute the intersection location and store it in g_RayHitResultBox.
if (fDistance > 0)
{
// The primary ray's direction is assumed to be normalized.
XMVector hitLocation = XMVector.MultiplyAdd(
this.primaryRay.Direction,
XMVector.Replicate(fDistance),
this.primaryRay.Origin);
BoundingBox box = this.rayHitResultBox.Box;
box.Center = hitLocation;
this.rayHitResultBox.Box = box;
this.rayHitResultBox.Collision = ContainmentType.Intersects;
}
else
{
this.rayHitResultBox.Collision = ContainmentType.Disjoint;
}
}
