private void TestClampRotation(Vector3 input, Vector3 expected)
{
var inputTS = new TwistSwing(Twist.MakeFromAngle(input.X), Swing.MakeFromAxisAngleProduct(input.Y, input.Z));
var clampedTS = constraint.Clamp(inputTS);
var expectedTS = new TwistSwing(Twist.MakeFromAngle(expected.X), Swing.MakeFromAxisAngleProduct(expected.Y, expected.Z));
MathAssert.AreEqual(expectedTS, clampedTS, 1e-4f);
}
public void TestCalculateDelta()
{
var initialTwist = Twist.MakeFromAngle(0.2f);
var finalTwist = Twist.MakeFromAngle(0.8f);
var deltaTwist = Twist.CalculateDelta(initialTwist, finalTwist);
MathAssert.AreEqual(Twist.MakeFromAngle(0.6f), deltaTwist, Acc);
}
public void TestApplyDelta()
{
var initialTwist = Twist.MakeFromAngle(0.5f);
var deltaTwist = Twist.MakeFromAngle(0.3f);
var finalTwist = Twist.ApplyDelta(initialTwist, deltaTwist);
MathAssert.AreEqual(Twist.MakeFromAngle(0.8f), finalTwist, Acc);
}
public TwistSwing FromTwistSwingAngles(Vector3 angles /* in radians */)
{
Twist twist = Twist.MakeFromAngle(angles[primaryAxis]);
Swing swing = Swing.MakeFromAxisAngleProduct(
angles[(primaryAxis + 1) % 3],
angles[(primaryAxis + 2) % 3]);
return(new TwistSwing(twist, swing));
}
public void TestMakeFromAngle()
{
float angle = 0.8f;
var twist = Twist.MakeFromAngle(angle);
Assert.AreEqual(angle, twist.Angle, Acc);
var expectedQ = Quaternion.RotationAxis(Vector3.UnitZ, angle);
var q = twist.AsQuaternion(CartesianAxis.Z);
MathAssert.AreEqual(expectedQ, q, Acc);
}