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