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 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 TestAxisAngleProduct()
{
Vector2 axis = Vector2.Normalize(new Vector2(-1, 3));
float angle = 0.8f;
Vector2 axisAngleProduct = axis * angle;
var swing = Swing.MakeFromAxisAngleProduct(axisAngleProduct.X, axisAngleProduct.Y);
MathAssert.AreEqual(axisAngleProduct, swing.AxisAngleProduct, Acc);
var expectedQ = Quaternion.RotationAxis(new Vector3(axis.X, axis.Y, 0), angle);
var q = swing.AsQuaternion(CartesianAxis.Z);
MathAssert.AreEqual(expectedQ, q, Acc);
}
