public static Swing FromTo(CartesianAxis twistAxis, Vector3 from, Vector3 to) { /* * This function is not optimized. * I should write an optimized version if I need to use FromTo in production. */ DebugUtilities.AssertIsUnit(from); DebugUtilities.AssertIsUnit(to); float fromX = from[((int)twistAxis + 0) % 3]; float fromY = from[((int)twistAxis + 1) % 3]; float fromZ = from[((int)twistAxis + 2) % 3]; float toX = to[((int)twistAxis + 0) % 3]; float toY = to[((int)twistAxis + 1) % 3]; float toZ = to[((int)twistAxis + 2) % 3]; Vector2 axis = Vector2.Normalize(new Vector2(fromZ - toZ, toY - fromY)); float projectionLength = axis.X * fromY + axis.Y * fromZ; Vector2 projection = axis * projectionLength; //by construction, projection onto axis is same for from and to Vector3 fromRejection = new Vector3(fromY - projection.X, fromZ - projection.Y, fromX); Vector3 toRejection = new Vector3(toY - projection.X, toZ - projection.Y, toX); Vector3 rejectionCross = Vector3.Cross(fromRejection, toRejection); float rejectionDot = Vector3.Dot(fromRejection, toRejection); float angle = (float)Atan2(axis.X * rejectionCross.X + axis.Y * rejectionCross.Y, rejectionDot); return(Swing.AxisAngle(axis.X, axis.Y, angle)); }
public static TwistSwing Decompose(CartesianAxis twistAxis, Quaternion q) { DebugUtilities.AssertIsUnit(q); float w = q.W; float x = q[(int)twistAxis]; float y = q[((int)twistAxis + 1) % 3]; float z = q[((int)twistAxis + 2) % 3]; float swingW = (float)Sqrt(Sqr(w) + Sqr(x)); float twistW, twistZ; if (swingW != 0) { twistW = w / swingW; twistZ = x / swingW; } else { //if swingW is 0, then there isn't a unique decomposition, so I'll arbitrarily assume no twist twistW = 1; twistZ = 0; } float swingY = twistW * y - twistZ * z; float swingZ = twistW * z + twistZ * y; var twist = new Twist(Sign(twistW) * twistZ); var swing = new Swing(swingY, swingZ); return(new TwistSwing(twist, swing)); }
public static void DecomposeIntoTwistThenSwing(this Quaternion q, Vector3 axis, out Quaternion twist, out Quaternion swing) { DebugUtilities.AssertIsUnit(axis); float dot = axis.X * q.X + axis.Y * q.Y + axis.Z * q.Z; twist = new Quaternion(dot * axis, q.W); twist.Normalize(); swing = q * Quaternion.Invert(twist); }
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)); }
public static Swing AxisAngle(float axisY, float axisZ, float angle) { angle = (float)IEEERemainder(angle, 2 * PI); if (angle == 0) { return(new Swing(0, 0)); } DebugUtilities.AssertIsUnit(axisY, axisZ); float halfSin = (float)Sin(angle / 2); return(new Swing(halfSin * axisY, halfSin * axisZ)); }