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 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);
}