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 void TestAxisAngle()
{
Vector2 axis = Vector2.Normalize(new Vector2(-1, 3));
float angle = 0.8f;
var swing = Swing.AxisAngle(axis.X, axis.Y, angle);
MathAssert.AreEqual(axis, swing.Axis, Acc);
Assert.AreEqual(angle, swing.Angle, Acc);
var expectedQ = Quaternion.RotationAxis(new Vector3(axis.X, axis.Y, 0), angle);
var q = swing.AsQuaternion(CartesianAxis.Z);
MathAssert.AreEqual(expectedQ, q, Acc);
}
public void TestApplyDeltaEdgeCases()
{
var rnd = new Random(0);
var swing = RandomUtil.Swing(rnd);
var axis = swing.Axis;
var angle = swing.Angle;
var zero = Swing.Zero;
var full = new Swing(axis.X, axis.Y);
var complement = Swing.AxisAngle(axis.X, axis.Y, MathUtil.Pi - angle);
MathAssert.AreEqual(full, Swing.ApplyDelta(zero, full), Acc);
MathAssert.AreEqual(full, Swing.ApplyDelta(full, zero), Acc);
MathAssert.AreEqual(full, Swing.ApplyDelta(swing, complement), Acc);
MathAssert.AreEqual(full, Swing.ApplyDelta(complement, swing), Acc);
}
public void TestCalculateDeltaEdgeCases()
{
var rnd = new Random(1);
var swing = RandomUtil.Swing(rnd);
var axis = swing.Axis;
var angle = swing.Angle;
var zero = Swing.Zero;
var full = new Swing(axis.X, axis.Y);
var complement = Swing.AxisAngle(axis.X, axis.Y, MathUtil.Pi - angle);
MathAssert.AreEqual(zero, Swing.CalculateDelta(zero, zero), 1e-3f);
MathAssert.AreEqual(full, Swing.CalculateDelta(zero, full), 1e-3f);
MathAssert.AreEqual(zero, Swing.CalculateDelta(full, full), 1e-3f);
MathAssert.AreEqual(complement, Swing.CalculateDelta(swing, full), 1e-3f);
MathAssert.AreEqual(swing, Swing.CalculateDelta(complement, full), 1e-3f);
}
