public static TwistSwingConstraint MakeFromRadians(CartesianAxis twistAxis, Vector3 min, Vector3 max) { var twistConstraint = TwistConstraint.MakeFromRadians(twistAxis, min, max); var swingConstraint = SwingConstraint.MakeFromRadians(twistAxis, min, max); return(new TwistSwingConstraint(twistConstraint, swingConstraint)); }
public void TestOffClamp_OffAxis() { var constraint = new SwingConstraint(-0.1f, +0.2f, -0.3f, +0.4f); var unclampedSwing = new Swing(0.3f, 0.5f); var clampedSwing = constraint.Clamp(unclampedSwing); // NArgMin[{EuclideanDistance[{0.3, 0.5}, {x, y}], (x/0.2)^2 + (y/0.4)^2 <= 1}, {x, y}] var expectedClampedSwing = new Swing(0.10463055104437786f, 0.34089557289708267f); MathAssert.AreEqual(expectedClampedSwing, clampedSwing, 1e-4f); }
public void TestClamp_OnAxis() { var constraint = new SwingConstraint(-0.1f, +0.2f, -0.3f, +0.4f); MathAssert.AreEqual(new Swing(-0.09f, 0), constraint.Clamp(new Swing(-0.09f, 0)), Acc); MathAssert.AreEqual(new Swing(-0.10f, 0), constraint.Clamp(new Swing(-0.11f, 0)), Acc); MathAssert.AreEqual(new Swing(+0.19f, 0), constraint.Clamp(new Swing(+0.19f, 0)), Acc); MathAssert.AreEqual(new Swing(+0.20f, 0), constraint.Clamp(new Swing(+0.21f, 0)), Acc); MathAssert.AreEqual(new Swing(0, -0.29f), constraint.Clamp(new Swing(0, -0.29f)), Acc); MathAssert.AreEqual(new Swing(0, -0.30f), constraint.Clamp(new Swing(0, -0.31f)), Acc); MathAssert.AreEqual(new Swing(0, +0.39f), constraint.Clamp(new Swing(0, +0.39f)), Acc); MathAssert.AreEqual(new Swing(0, +0.40f), constraint.Clamp(new Swing(0, +0.41f)), Acc); }
public void TestTest() { var constraint = new SwingConstraint(-0.1f, +0.2f, -0.3f, +0.4f); Assert.IsTrue(constraint.Test(new Swing(-0.09f, 0))); Assert.IsFalse(constraint.Test(new Swing(-0.11f, 0))); Assert.IsTrue(constraint.Test(new Swing(+0.19f, 0))); Assert.IsFalse(constraint.Test(new Swing(+0.21f, 0))); Assert.IsTrue(constraint.Test(new Swing(0, -0.29f))); Assert.IsFalse(constraint.Test(new Swing(0, -0.31f))); Assert.IsTrue(constraint.Test(new Swing(0, +0.39f))); Assert.IsFalse(constraint.Test(new Swing(0, +0.41f))); }
public TwistSwingConstraint(TwistConstraint twistConstraint, SwingConstraint swingConstraint) { this.twistConstraint = twistConstraint; this.swingConstraint = swingConstraint; }