public static XMVector Normalize(XMVector v)
{
XMVector result = XMVector3.Length(v);
float length = result.X;
// Prevent divide by zero
if (length > 0)
{
length = 1.0f / length;
}
return(new XMVector(
v.X * length,
v.Y * length,
v.Z * length,
v.W * length));
}
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 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));
}
