public static XMVector BaryCentricV(XMVector q0, XMVector q1, XMVector q2, XMVector f, XMVector g)
{
Debug.Assert(f.Y == f.X && f.Z == f.X && f.W == f.X, "Reviewed");
Debug.Assert(g.Y == g.X && g.Z == g.X && g.W == g.X, "Reviewed");
XMVector epsilon = XMVector.FromSplatConstant(1, 16);
XMVector s = XMVector.Add(f, g);
XMVector result;
if (XMVector4.InBounds(s, epsilon))
{
result = q0;
}
else
{
XMVector q01 = XMQuaternion.SlerpV(q0, q1, s);
XMVector q02 = XMQuaternion.SlerpV(q0, q2, s);
XMVector gs = s.Reciprocal();
gs = XMVector.Multiply(g, gs);
result = XMQuaternion.SlerpV(q01, q02, gs);
}
return(result);
}
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 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 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 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 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 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);
}
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);
}