public void SerializationBinary()
{
CurveKey2F curveKey1 = new CurveKey2F
{
Interpolation = SplineInterpolation.Bezier,
Point = new Vector2F(1.2f, 3.4f),
TangentIn = new Vector2F(0.7f, 2.6f),
TangentOut = new Vector2F(1.9f, 3.3f)
};
CurveKey2F curveKey2;
const string fileName = "SerializationCurve2FKey.bin";
if (File.Exists(fileName))
{
File.Delete(fileName);
}
FileStream fs = new FileStream(fileName, FileMode.Create);
BinaryFormatter formatter = new BinaryFormatter();
formatter.Serialize(fs, curveKey1);
fs.Close();
fs = new FileStream(fileName, FileMode.Open);
formatter = new BinaryFormatter();
curveKey2 = (CurveKey2F)formatter.Deserialize(fs);
fs.Close();
MathAssert.AreEqual(curveKey1, curveKey2);
}
public void SerializationXml()
{
PathKey3F pathKey1 = new PathKey3F
{
Interpolation = SplineInterpolation.Bezier,
Parameter = 56.7f,
Point = new Vector3(1.2f, 3.4f, 5.6f),
TangentIn = new Vector3(0.7f, 2.6f, 5.1f),
TangentOut = new Vector3(1.9f, 3.3f, 5.9f)
};
PathKey3F pathKey2;
const string fileName = "SerializationPath3FKey.xml";
if (File.Exists(fileName))
{
File.Delete(fileName);
}
XmlSerializer serializer = new XmlSerializer(typeof(PathKey3F));
StreamWriter writer = new StreamWriter(fileName);
serializer.Serialize(writer, pathKey1);
writer.Close();
serializer = new XmlSerializer(typeof(PathKey3F));
FileStream fileStream = new FileStream(fileName, FileMode.Open);
pathKey2 = (PathKey3F)serializer.Deserialize(fileStream);
MathAssert.AreEqual(pathKey1, pathKey2);
}
public void SerializationBinary()
{
PathKey3F pathKey1 = new PathKey3F
{
Interpolation = SplineInterpolation.Bezier,
Parameter = 56.7f,
Point = new Vector3(1.2f, 3.4f, 5.6f),
TangentIn = new Vector3(0.7f, 2.6f, 5.1f),
TangentOut = new Vector3(1.9f, 3.3f, 5.9f)
};
PathKey3F pathKey2;
const string fileName = "SerializationPath3FKey.bin";
if (File.Exists(fileName))
{
File.Delete(fileName);
}
FileStream fs = new FileStream(fileName, FileMode.Create);
BinaryFormatter formatter = new BinaryFormatter();
formatter.Serialize(fs, pathKey1);
fs.Close();
fs = new FileStream(fileName, FileMode.Open);
formatter = new BinaryFormatter();
pathKey2 = (PathKey3F)formatter.Deserialize(fs);
fs.Close();
MathAssert.AreEqual(pathKey1, pathKey2);
}
public void TestAsQuaternion()
{
var twist = new Twist(0.4f);
var swing = new Swing(0.2f, 0.3f);
var twistSwing = new TwistSwing(twist, swing);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
MathAssert.AreEqual(
twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)),
twistSwing.AsQuaternion(CartesianAxis.X),
Acc);
}
public void SerializationXml()
{
CurveKey2F curveKey1 = new CurveKey2F
{
Interpolation = SplineInterpolation.Bezier,
Point = new Vector2F(1.2f, 3.4f),
TangentIn = new Vector2F(0.7f, 2.6f),
TangentOut = new Vector2F(1.9f, 3.3f)
};
CurveKey2F curveKey2;
const string fileName = "SerializationCurve2FKey.xml";
if (File.Exists(fileName))
{
File.Delete(fileName);
}
XmlSerializer serializer = new XmlSerializer(typeof(CurveKey2F));
StreamWriter writer = new StreamWriter(fileName);
serializer.Serialize(writer, curveKey1);
writer.Close();
serializer = new XmlSerializer(typeof(CurveKey2F));
FileStream fileStream = new FileStream(fileName, FileMode.Open);
curveKey2 = (CurveKey2F)serializer.Deserialize(fileStream);
MathAssert.AreEqual(curveKey1, curveKey2);
}
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 TestToRotationVector()
{
Vector3 v = new Vector3(0.1f, 0.2f, 0.3f);
var q = Quaternion.RotationAxis(v, v.Length());
MathAssert.AreEqual(v, q.ToRotationVector(), 1e-4f);
MathAssert.AreEqual(v, (-q).ToRotationVector(), 1e-4f);
}
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 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 void TestCrossProductMatrix()
{
Vector3 v = new Vector3(2, 3, 4);
Vector3 u = new Vector3(9, 7, 5);
Matrix3x3 m = MathExtensions.CrossProductMatrix(v);
MathAssert.AreEqual(Vector3.Cross(v, u), MathExtensions.Mul(m, u), 1e-5f);
}
public void TestFromRotationVector()
{
Vector3 v = new Vector3(0.1f, 0.2f, 0.3f);
var expected = Quaternion.RotationAxis(v, v.Length());
MathAssert.AreEqual(expected, QuaternionExtensions.FromRotationVector(v), 1e-4f);
MathAssert.AreEqual(Quaternion.Invert(expected), QuaternionExtensions.FromRotationVector(-v), 1e-4f);
}
public void TestToEdgeCase()
{
Vector3 to = -Vector3.UnitZ;
var swing = Swing.To(CartesianAxis.Z, to);
var swungTwistAxisV = swing.TransformTwistAxis(CartesianAxis.Z);
MathAssert.AreEqual(to, swungTwistAxisV, Acc);
}
public void TestCalculateApplyDeltaRoundtrip()
{
var initial = new Swing(0.2f, 0.1f);
var final = new Swing(-0.3f, 0.4f);
var delta = Swing.CalculateDelta(initial, final);
var roundtripFinal = Swing.ApplyDelta(initial, delta);
MathAssert.AreEqual(final, roundtripFinal, Acc);
}
private void TestToTwistSwingAngles(RotationOrder order)
{
MathAssert.AreEqual(new Vector3(0, 0, 0), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.Identity)), Acc);
MathAssert.AreEqual(new Vector3(+1, 0, 0), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitX, +1))), Acc);
MathAssert.AreEqual(new Vector3(-1, 0, 0), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitX, -1))), Acc);
MathAssert.AreEqual(new Vector3(0, +1, 0), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitY, +1))), Acc);
MathAssert.AreEqual(new Vector3(0, -1, 0), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitY, -1))), Acc);
MathAssert.AreEqual(new Vector3(0, 0, +1), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitZ, +1))), Acc);
MathAssert.AreEqual(new Vector3(0, 0, -1), order.ToTwistSwingAngles(TwistSwing.Decompose(order.TwistAxis, Quaternion.RotationAxis(Vector3.UnitZ, -1))), Acc);
}
public void TestClamp()
{
var constraint = new TwistConstraint(-0.1f, +0.2f);
MathAssert.AreEqual(new Twist(-0.09f), constraint.Clamp(new Twist(-0.09f)), Acc);
MathAssert.AreEqual(new Twist(-0.10f), constraint.Clamp(new Twist(-0.11f)), Acc);
MathAssert.AreEqual(new Twist(+0.19f), constraint.Clamp(new Twist(+0.19f)), Acc);
MathAssert.AreEqual(new Twist(+0.20f), constraint.Clamp(new Twist(+0.20f)), Acc);
}
public void TestCenter()
{
var constraint = new TwistSwingConstraint(
new TwistConstraint(-0.1f, 0.3f),
new SwingConstraint(
-0.2f, 0.6f,
-0.4f, 1.0f));
MathAssert.AreEqual(TwistSwing.MakeFromCoordinates(0.1f, 0.2f, 0.3f), constraint.Center, 1e-4f);
}
private void TestFromTwistSwingAngles(RotationOrder order)
{
MathAssert.AreEqual(order.FromTwistSwingAngles(new Vector3(0, 0, 0)).AsQuaternion(order.TwistAxis), Quaternion.Identity, Acc);
MathAssert.AreEqual(order.FromTwistSwingAngles(new Vector3(+1, 0, 0)).AsQuaternion(order.TwistAxis), Quaternion.RotationAxis(Vector3.UnitX, +1), Acc);
MathAssert.AreEqual(order.FromTwistSwingAngles(new Vector3(-1, 0, 0)).AsQuaternion(order.TwistAxis), Quaternion.RotationAxis(Vector3.UnitX, -1), Acc);
MathAssert.AreEqual(order.FromTwistSwingAngles(new Vector3(0, +1, 0)).AsQuaternion(order.TwistAxis), Quaternion.RotationAxis(Vector3.UnitY, +1), Acc);
MathAssert.AreEqual(order.FromTwistSwingAngles(new Vector3(0, -1, 0)).AsQuaternion(order.TwistAxis), Quaternion.RotationAxis(Vector3.UnitY, -1), Acc);
MathAssert.AreEqual(order.FromTwistSwingAngles(new Vector3(0, 0, +1)).AsQuaternion(order.TwistAxis), Quaternion.RotationAxis(Vector3.UnitZ, +1), Acc);
MathAssert.AreEqual(order.FromTwistSwingAngles(new Vector3(0, 0, -1)).AsQuaternion(order.TwistAxis), Quaternion.RotationAxis(Vector3.UnitZ, -1), Acc);
}
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 static void AreEqual(MassMoment expected, MassMoment actual, float delta)
{
Assert.AreEqual(expected.Mass, actual.Mass, delta, "Mass");
MathAssert.AreEqual(expected.MassPosition, actual.MassPosition, delta);
Assert.AreEqual(expected.InertiaXX, actual.InertiaXX, delta, "InertiaXX");
Assert.AreEqual(expected.InertiaYY, actual.InertiaYY, delta, "InertiaYY");
Assert.AreEqual(expected.InertiaZZ, actual.InertiaZZ, delta, "InertiaZZ");
Assert.AreEqual(expected.InertiaXY, actual.InertiaXY, delta, "InertiaXY");
Assert.AreEqual(expected.InertiaXZ, actual.InertiaXZ, delta, "InertiaXZ");
Assert.AreEqual(expected.InertiaYZ, actual.InertiaYZ, delta, "InertiaYZ");
}
public void TestChain()
{
var rnd = new Random(0);
RigidTransform transform1 = RandomUtil.RigidTransform(rnd);
RigidTransform transform2 = RandomUtil.RigidTransform(rnd);
var testPoint = RandomUtil.Vector3(rnd);
MathAssert.AreEqual(
transform2.Transform(transform1.Transform(testPoint)),
transform1.Chain(transform2).Transform(testPoint),
Acc);
}
public void TestTransform()
{
var rnd = new Random(0);
var swing = RandomUtil.Swing(rnd);
var point = RandomUtil.Vector3(rnd);
var twistAxis = CartesianAxis.Y;
var expected = Vector3.Transform(point, swing.AsQuaternion(twistAxis));
var actual = swing.Transform(twistAxis, point);
MathAssert.AreEqual(expected, actual, Acc);
}
public void TestDecompose()
{
var q = Quaternion.Normalize(new Quaternion(0.1f, 0.2f, 0.3f, 0.4f));
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.X, q).AsQuaternion(CartesianAxis.X), Acc);
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.Y, q).AsQuaternion(CartesianAxis.Y), Acc);
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.Z, q).AsQuaternion(CartesianAxis.Z), Acc);
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.X, -q).AsQuaternion(CartesianAxis.X), Acc);
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.Y, -q).AsQuaternion(CartesianAxis.Y), Acc);
MathAssert.AreEqual(q, TwistSwing.Decompose(CartesianAxis.Z, -q).AsQuaternion(CartesianAxis.Z), Acc);
}
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);
}
public void TestFromTo()
{
var rnd = new Random(0);
Vector3 a = MakeRandomUnitVector(rnd);
Vector3 b = MakeRandomUnitVector(rnd);
foreach (CartesianAxis twistAxis in CartesianAxes.Values)
{
Swing fromAToB = Swing.FromTo(twistAxis, a, b);
MathAssert.AreEqual(b, Vector3.Transform(a, fromAToB.AsQuaternion(twistAxis)), Acc);
Swing fromBToA = Swing.FromTo(twistAxis, b, a);
MathAssert.AreEqual(a, Vector3.Transform(b, fromBToA.AsQuaternion(twistAxis)), Acc);
}
}
public void TestFromRotationAboutCenter()
{
var rnd = new Random(0);
var rotation = RandomUtil.UnitQuaternion(rnd);
var center = RandomUtil.Vector3(rnd);
RigidTransform transform = RigidTransform.FromRotation(rotation, center);
var testPoint = RandomUtil.Vector3(rnd);
MathAssert.AreEqual(
Vector3.Transform(testPoint - center, rotation) + center,
transform.Transform(testPoint),
Acc);
}
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);
}
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 TestTo()
{
Vector3 to = Vector3.Normalize(new Vector3(1, -2, 3));
var swing = Swing.To(CartesianAxis.Z, to);
var swungTwistAxisV = swing.TransformTwistAxis(CartesianAxis.Z);
MathAssert.AreEqual(to, swungTwistAxisV, Acc);
var q = swing.AsQuaternion(CartesianAxis.Z);
var twistAxisV = new Vector3(0, 0, 1);
var swungTwistAxisUsingQV = Vector3.Transform(twistAxisV, q);
MathAssert.AreEqual(to, swungTwistAxisUsingQV, Acc);
}
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 TestAsQuaternion()
{
MathAssert.AreEqual(Quaternion.Identity, new Twist(0).AsQuaternion(CartesianAxis.X), Acc);
MathAssert.AreEqual(Quaternion.Identity, new Twist(0).AsQuaternion(CartesianAxis.Y), Acc);
MathAssert.AreEqual(Quaternion.Identity, new Twist(0).AsQuaternion(CartesianAxis.Z), Acc);
float sinHalfOne = (float)Math.Sin(0.5);
MathAssert.AreEqual(Quaternion.RotationAxis(Vector3.UnitX, 1), new Twist(sinHalfOne).AsQuaternion(CartesianAxis.X), Acc);
MathAssert.AreEqual(Quaternion.RotationAxis(Vector3.UnitY, 1), new Twist(sinHalfOne).AsQuaternion(CartesianAxis.Y), Acc);
MathAssert.AreEqual(Quaternion.RotationAxis(Vector3.UnitZ, 1), new Twist(sinHalfOne).AsQuaternion(CartesianAxis.Z), Acc);
MathAssert.AreEqual(Quaternion.RotationAxis(Vector3.UnitX, -1), new Twist(-sinHalfOne).AsQuaternion(CartesianAxis.X), Acc);
MathAssert.AreEqual(Quaternion.RotationAxis(Vector3.UnitY, -1), new Twist(-sinHalfOne).AsQuaternion(CartesianAxis.Y), Acc);
MathAssert.AreEqual(Quaternion.RotationAxis(Vector3.UnitZ, -1), new Twist(-sinHalfOne).AsQuaternion(CartesianAxis.Z), Acc);
}
