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