public static XMVector RgbToHsv(XMVector rgb)
{
XMVector r = XMVector.SplatX(rgb);
XMVector g = XMVector.SplatY(rgb);
XMVector b = XMVector.SplatZ(rgb);
XMVector min = XMVector.Min(r, XMVector.Min(g, b));
XMVector v = XMVector.Max(r, XMVector.Max(g, b));
XMVector d = XMVector.Subtract(v, min);
XMVector s = XMVector3.NearEqual(v, XMGlobalConstants.Zero, XMGlobalConstants.Epsilon) ?
XMGlobalConstants.Zero :
XMVector.Divide(d, v);
if (XMVector3.Less(d, XMGlobalConstants.Epsilon))
{
// Achromatic, assume H of 0
XMVector hv = XMVector.Select(v, XMGlobalConstants.Zero, XMGlobalConstants.Select1000);
XMVector hva = XMVector.Select(rgb, hv, XMGlobalConstants.Select1110);
return(XMVector.Select(s, hva, XMGlobalConstants.Select1011));
}
else
{
XMVector h;
if (XMVector3.Equal(r, v))
{
// Red is max
h = XMVector.Divide(XMVector.Subtract(g, b), d);
if (XMVector3.Less(g, b))
{
h = XMVector.Add(h, XMGlobalConstants.Six);
}
}
else if (XMVector3.Equal(g, v))
{
// Green is max
h = XMVector.Divide(XMVector.Subtract(b, r), d);
h = XMVector.Add(h, XMGlobalConstants.Two);
}
else
{
// Blue is max
h = XMVector.Divide(XMVector.Subtract(r, g), d);
h = XMVector.Add(h, XMGlobalConstants.Four);
}
h = XMVector.Divide(h, XMGlobalConstants.Six);
XMVector hv = XMVector.Select(v, h, XMGlobalConstants.Select1000);
XMVector hva = XMVector.Select(rgb, hv, XMGlobalConstants.Select1110);
return(XMVector.Select(s, hva, XMGlobalConstants.Select1011));
}
}
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 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 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 XMVector Ln(XMVector q)
{
XMVector oneMinusEpsilon = XMVector.FromFloat(1.0f - 0.00001f, 1.0f - 0.00001f, 1.0f - 0.00001f, 1.0f - 0.00001f);
XMVector qw = XMVector.SplatW(q);
XMVector q0 = XMVector.Select(XMGlobalConstants.Select1110, q, XMGlobalConstants.Select1110);
XMVector controlW = qw.InBounds(oneMinusEpsilon);
XMVector theta = qw.ACos();
XMVector sinTheta = theta.Sin();
XMVector s = XMVector.Divide(theta, sinTheta);
XMVector result = XMVector.Multiply(q0, s);
result = XMVector.Select(q0, result, controlW);
return(result);
}
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 static XMVector Exp(XMVector q)
{
XMVector theta = XMVector3.Length(q);
XMVector sinTheta;
XMVector cosTheta;
theta.SinCos(out sinTheta, out cosTheta);
XMVector s = XMVector.Divide(sinTheta, theta);
XMVector result = XMVector.Multiply(q, s);
XMVector zero = XMVector.Zero;
XMVector control = XMVector.NearEqual(theta, zero, XMGlobalConstants.Epsilon);
result = XMVector.Select(result, q, control);
result = XMVector.Select(cosTheta, result, XMGlobalConstants.Select1110);
return(result);
}
public static XMVector SrgbToXyz(XMVector srgb)
{
XMVector scale0 = XMVector.FromFloat(0.4124f, 0.2126f, 0.0193f, 0.0f);
XMVector scale1 = XMVector.FromFloat(0.3576f, 0.7152f, 0.1192f, 0.0f);
XMVector scale2 = XMVector.FromFloat(0.1805f, 0.0722f, 0.9505f, 0.0f);
XMVector cutoff = XMVector.FromFloat(0.04045f, 0.04045f, 0.04045f, 0.0f);
XMVector exp = XMVector.FromFloat(2.4f, 2.4f, 2.4f, 1.0f);
XMVector sel = XMVector.Greater(srgb, cutoff);
// lclr = clr / 12.92
XMVector smallC = XMVector.Divide(srgb, XMGlobalConstants.MsrgbScale);
// lclr = pow( (clr + a) / (1+a), 2.4 )
XMVector largeC = XMVector.Pow(XMVector.Divide(XMVector.Add(srgb, XMGlobalConstants.MsrgbA), XMGlobalConstants.MsrgbA1), exp);
XMVector lclr = XMVector.Select(smallC, largeC, sel);
XMMatrix m = new XMMatrix(scale0, scale1, scale2, XMGlobalConstants.Zero);
XMVector clr = XMVector3.Transform(lclr, m);
return(XMVector.Select(srgb, clr, XMGlobalConstants.Select1110));
}
public static BoundingSphere CreateMerged(BoundingSphere s1, BoundingSphere s2)
{
XMVector center1 = s1.center;
float r1 = s1.radius;
XMVector center2 = s2.center;
float r2 = s2.radius;
XMVector v = XMVector.Subtract(center2, center1);
XMVector dist = XMVector3.Length(v);
float d = dist.X;
if (r1 + r2 >= d)
{
if (r1 - r2 >= d)
{
return(s1);
}
else if (r2 - r1 >= d)
{
return(s2);
}
}
XMVector n = XMVector.Divide(v, dist);
float t1 = Math.Min(-r1, d - r2);
float t2 = Math.Max(r1, d + r2);
float t_5 = (t2 - t1) * 0.5f;
XMVector n_center = XMVector.Add(center1, XMVector.Multiply(n, XMVector.Replicate(t_5 + t1)));
return(new BoundingSphere(n_center, t_5));
}
public static XMVector LinePointDistance(XMVector linePoint1, XMVector linePoint2, XMVector point)
{
//// Given a vector PointVector from LinePoint1 to Point and a vector
//// LineVector from LinePoint1 to LinePoint2, the scaled distance
//// PointProjectionScale from LinePoint1 to the perpendicular projection
//// of PointVector onto the line is defined as:
////
//// PointProjectionScale = dot(PointVector, LineVector) / LengthSq(LineVector)
XMVector pointVector = XMVector.Subtract(point, linePoint1);
XMVector lineVector = XMVector.Subtract(linePoint2, linePoint1);
XMVector lengthSq = XMVector3.LengthSquare(lineVector);
XMVector pointProjectionScale = XMVector3.Dot(pointVector, lineVector);
pointProjectionScale = XMVector.Divide(pointProjectionScale, lengthSq);
XMVector distanceVector = XMVector.Multiply(lineVector, pointProjectionScale);
distanceVector = XMVector.Subtract(pointVector, distanceVector);
return(XMVector3.Length(distanceVector));
}
public static XMVector RgbToHsl(XMVector rgb)
{
XMVector r = XMVector.SplatX(rgb);
XMVector g = XMVector.SplatY(rgb);
XMVector b = XMVector.SplatZ(rgb);
XMVector min = XMVector.Min(r, XMVector.Min(g, b));
XMVector max = XMVector.Max(r, XMVector.Max(g, b));
XMVector l = XMVector.Multiply(XMVector.Add(min, max), XMGlobalConstants.OneHalf);
XMVector d = XMVector.Subtract(max, min);
XMVector la = XMVector.Select(rgb, l, XMGlobalConstants.Select1110);
if (XMVector3.Less(d, XMGlobalConstants.Epsilon))
{
// Achromatic, assume H and S of 0
return(XMVector.Select(la, XMGlobalConstants.Zero, XMGlobalConstants.Select1100));
}
else
{
XMVector s;
XMVector h;
XMVector d2 = XMVector.Add(min, max);
if (XMVector3.Greater(l, XMGlobalConstants.OneHalf))
{
// d / (2-max-min)
s = XMVector.Divide(d, XMVector.Subtract(XMGlobalConstants.Two, d2));
}
else
{
// d / (max+min)
s = XMVector.Divide(d, d2);
}
if (XMVector3.Equal(r, max))
{
// Red is max
h = XMVector.Divide(XMVector.Subtract(g, b), d);
}
else if (XMVector3.Equal(g, max))
{
// Green is max
h = XMVector.Divide(XMVector.Subtract(b, r), d);
h = XMVector.Add(h, XMGlobalConstants.Two);
}
else
{
// Blue is max
h = XMVector.Divide(XMVector.Subtract(r, g), d);
h = XMVector.Add(h, XMGlobalConstants.Four);
}
h = XMVector.Divide(h, XMGlobalConstants.Six);
if (XMVector3.Less(h, XMGlobalConstants.Zero))
{
h = XMVector.Add(h, XMGlobalConstants.One);
}
XMVector lha = XMVector.Select(la, h, XMGlobalConstants.Select1100);
return(XMVector.Select(s, lha, XMGlobalConstants.Select1011));
}
}
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 BoundingSphere CreateFromPoints(XMFloat3[] points)
{
if (points == null)
{
throw new ArgumentNullException("points");
}
if (points.Length == 0)
{
throw new ArgumentOutOfRangeException("points");
}
// Find the points with minimum and maximum x, y, and z
XMVector minX, maxX, minY, maxY, minZ, maxZ;
minX = maxX = minY = maxY = minZ = maxZ = points[0];
for (int i = 1; i < points.Length; i++)
{
XMVector point = points[i];
float px = point.X;
float py = point.Y;
float pz = point.Z;
if (px < minX.X)
{
minX = point;
}
if (px > maxX.X)
{
maxX = point;
}
if (py < minY.Y)
{
minY = point;
}
if (py > maxY.Y)
{
maxY = point;
}
if (pz < minZ.Z)
{
minZ = point;
}
if (pz > maxZ.Z)
{
maxZ = point;
}
}
// Use the min/max pair that are farthest apart to form the initial sphere.
XMVector deltaX = maxX - minX;
XMVector distX = XMVector3.Length(deltaX);
XMVector deltaY = maxY - minY;
XMVector distY = XMVector3.Length(deltaY);
XMVector deltaZ = maxZ - minZ;
XMVector distZ = XMVector3.Length(deltaZ);
XMVector v_center;
XMVector v_radius;
if (XMVector3.Greater(distX, distY))
{
if (XMVector3.Greater(distX, distZ))
{
// Use min/max x.
v_center = XMVector.Lerp(maxX, minX, 0.5f);
v_radius = distX * 0.5f;
}
else
{
// Use min/max z.
v_center = XMVector.Lerp(maxZ, minZ, 0.5f);
v_radius = distZ * 0.5f;
}
}
else
{
//// Y >= X
if (XMVector3.Greater(distY, distZ))
{
// Use min/max y.
v_center = XMVector.Lerp(maxY, minY, 0.5f);
v_radius = distY * 0.5f;
}
else
{
// Use min/max z.
v_center = XMVector.Lerp(maxZ, minZ, 0.5f);
v_radius = distZ * 0.5f;
}
}
// Add any points not inside the sphere.
for (int i = 0; i < points.Length; i++)
{
XMVector point = points[i];
XMVector delta = point - v_center;
XMVector dist = XMVector3.Length(delta);
if (XMVector3.Greater(dist, v_radius))
{
// Adjust sphere to include the new point.
v_radius = (v_radius + dist) * 0.5f;
v_center += (XMVector.Replicate(1.0f) - XMVector.Divide(v_radius, dist)) * delta;
}
}
return(new BoundingSphere(v_center, v_radius.X));
}
