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