public static Swing To(CartesianAxis twistAxis, Vector3 to)
{
DebugUtilities.AssertIsUnit(to);
float toX = to[((int)twistAxis + 0) % 3];
float toY = to[((int)twistAxis + 1) % 3];
float toZ = to[((int)twistAxis + 2) % 3];
/*
* To reconstruct the swing quaternion, we need to calculate:
* <y, z> = Sin[angle / 2] * rotation-axis
* w = Cos[angle / 2]
*
* We know:
* Cos[angle]
* = Dot[twist-axis, to]
* = toX
*
* rotation-axis
* = Normalize[Cross[twist-axis, to]]
* = Normalize[<-toZ, toX>]
* = <-toZ, toX> / Sqrt[toX^2 + toZ^2]
* = <-toZ, toX> / Sqrt[1 - toX^2]
*
* So:
* w = Cos[angle / 2]
* = Sqrt[(1 + Cos[angle]) / 2] (half-angle trig identity)
* = Sqrt[(1 + toX) / 2]
*
* <y,z>
* = Sin[angle / 2] * rotation-axis
* = Sqrt[(1 - Cos[angle]) / 2] * rotation-axis (half-angle trig identity)
* = Sqrt[(1 - toX) / 2] * rotation-axis
* = Sqrt[(1 - toX) / 2] / Sqrt[1 - toX^2] * <-toZ, toY>
* = Sqrt[(1 - toX) / (2 * (1 - toX^2))] * <-toZ, toY>
* = Sqrt[(1 - toX) / (2 * (1 - toX) * (1 + toX))] * <-toZ, toY>
* = Sqrt[1 / (2 * (1 + toX))] * <-toZ, toY>
* = 1 / (2 * w) * <-toZ, toY>
*/
float ww = (1 + toX);
float wy = -toZ;
float wz = +toY;
if (ww < MathUtil.ZeroTolerance)
{
// This is a 180 degree swing (W = 0) so X and Y don't have a unique value
// I'll arbitrarily use:
return(new Swing(1, 0));
}
return(Swing.MakeUnitized(ww, wy, wz));
}
