public void InverseWithNearSingularMatrix() { Matrix33D m = new Matrix33D(0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001); Vector3D v = Vector3D.One; Vector3D w = m * v; Assert.IsTrue(Vector3D.AreNumericallyEqual(v, m.Inverse * w)); Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.Identity, m * m.Inverse)); }
public void HomogeneousDivide() { Vector4D v4 = new Vector4D(1.0, 2.0, 3.0, 1.0); Vector3D v3 = Vector4D.HomogeneousDivide(v4); Assert.AreEqual(new Vector3D(1.0, 2.0, 3.0), v3); v4 = new Vector4D(1.0, 2.0, 3.0, 10.0); v3 = Vector4D.HomogeneousDivide(v4); Assert.IsTrue(Vector3D.AreNumericallyEqual(new Vector3D(1.0 / 10.0, 2.0 / 10.0, 3.0 / 10.0), v3)); }
public void CreateRotationZ() { double angle = (double)MathHelper.ToRadians(30); Matrix33D m = Matrix33D.CreateRotationZ(angle); Assert.IsTrue(Vector3D.AreNumericallyEqual(new Vector3D((double)Math.Cos(angle), (double)Math.Sin(angle), 0), m * Vector3D.UnitX)); QuaternionD q = QuaternionD.CreateRotation(Vector3D.UnitZ, angle); Assert.IsTrue(Vector3D.AreNumericallyEqual(q.Rotate(Vector3D.One), m * Vector3D.One)); Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.CreateRotation(Vector3D.UnitZ, angle), m)); }
public void AreEqualWithEpsilon() { double epsilon = 0.001; Vector3D u = new Vector3D(1.0, 2.0, 3.0); Vector3D v = new Vector3D(1.002, 2.002, 3.002); Vector3D w = new Vector3D(1.0001, 2.0001, 3.0001); Assert.IsTrue(Vector3D.AreNumericallyEqual(u, u, epsilon)); Assert.IsFalse(Vector3D.AreNumericallyEqual(u, v, epsilon)); Assert.IsTrue(Vector3D.AreNumericallyEqual(u, w, epsilon)); }
public void Inverse() { Assert.AreEqual(Matrix33D.Identity, Matrix33D.Identity.Inverse); Matrix33D m = new Matrix33D(1, 2, 3, 2, 5, 8, 7, 6, -1); Vector3D v = Vector3D.One; Vector3D w = m * v; Assert.IsTrue(Vector3D.AreNumericallyEqual(v, m.Inverse * w)); Assert.IsTrue(Matrix33D.AreNumericallyEqual(Matrix33D.Identity, m * m.Inverse)); }
public void Ln4() { double θ = 0.0; Vector3D v = new Vector3D(1.0, 2.0, 3.0); v.Normalize(); QuaternionD q = new QuaternionD((double)Math.Cos(θ), (double)Math.Sin(θ) * v); q.Ln(); Assert.IsTrue(Numeric.AreEqual(0.0, q.W)); Assert.IsTrue(Vector3D.AreNumericallyEqual(θ * v, q.V)); }
public void SlerpNegatedDoublePrecision() { QuaternionD q1 = new QuaternionD(-1.0, 0.0, 0.0, 0.0); QuaternionD q2 = QuaternionD.CreateRotation(-Vector3.UnitZ, -Math.PI / 2); QuaternionD slerp = InterpolationHelper.Slerp(q1, q2, 0.5); Assert.IsTrue(slerp.IsNumericallyNormalized); Vector3D v = slerp.Rotate(Vector3D.UnitX); Vector3D result = new Vector3D(1.0, 1.0, 0.0).Normalized; Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v)); }
public void SlerpXDoublePrecision() { QuaternionD q1 = QuaternionD.Identity; QuaternionD q2 = QuaternionD.CreateRotation(Vector3.UnitX, Math.PI / 2); QuaternionD slerp = InterpolationHelper.Slerp(q1, q2, 0.5); Assert.IsTrue(slerp.IsNumericallyNormalized); Vector3D v = slerp.Rotate(Vector3.UnitY); Vector3D result = new Vector3D(0.0, 1.0, 1.0).Normalized; Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v)); }
public void CosineInterpolationVector3D() { Vector3D v = new Vector3D(1.0, 10.0, 100.0); Vector3D w = new Vector3D(2.0, 20.0, 200.0); Vector3D lerp0 = InterpolationHelper.CosineInterpolation(v, w, 0.0); Vector3D lerp1 = InterpolationHelper.CosineInterpolation(v, w, 1.0); Vector3D lerp05 = InterpolationHelper.CosineInterpolation(v, w, 0.5); Assert.AreEqual(v, lerp0); Assert.AreEqual(w, lerp1); Assert.IsTrue(Vector3D.AreNumericallyEqual(new Vector3D(1.5, 15.0, 150.0), lerp05)); }
public void RotationMatrix33() { double angle = -1.6; Vector3D axis = new Vector3D(1.0, 2.0, -3.0); QuaternionD q = QuaternionD.CreateRotation(axis, angle); Matrix33D m33 = Matrix33D.CreateRotation(axis, angle); Vector3D v = new Vector3D(0.3, -2.4, 5.6); Vector3D result1 = q.ToRotationMatrix33() * v; Vector3D result2 = m33 * v; Assert.IsTrue(Vector3D.AreNumericallyEqual(result1, result2)); }
public void Exp4() { double θ = 0.0; Vector3D v = new Vector3D(1.0, 2.0, 3.0); v.Normalize(); QuaternionD q = new QuaternionD(0.0, θ * v); q.Exp(); Assert.IsTrue(Numeric.AreEqual(1, q.W)); Assert.IsTrue(Vector3D.AreNumericallyEqual(Vector3D.Zero, q.V)); }
public void Exp2() { double θ = -0.3; Vector3D v = new Vector3D(1.0, 2.0, 3.0); v.Normalize(); QuaternionD q = new QuaternionD(0.0, θ * v); q.Exp(); Assert.IsTrue(Numeric.AreEqual((double)Math.Cos(θ), q.W)); Assert.IsTrue(Vector3D.AreNumericallyEqual((double)Math.Sin(θ) * v, q.V)); }
public void CreateRotation() { Matrix33D m = Matrix33D.CreateRotation(Vector3D.UnitX, 0.0); Assert.AreEqual(Matrix33D.Identity, m); m = Matrix33D.CreateRotation(Vector3D.UnitX, (double)Math.PI / 2); Assert.IsTrue(Vector3D.AreNumericallyEqual(Vector3D.UnitZ, m * Vector3D.UnitY)); m = Matrix33D.CreateRotation(Vector3D.UnitY, (double)Math.PI / 2); Assert.IsTrue(Vector3D.AreNumericallyEqual(Vector3D.UnitX, m * Vector3D.UnitZ)); m = Matrix33D.CreateRotation(Vector3D.UnitZ, (double)Math.PI / 2); Assert.IsTrue(Vector3D.AreNumericallyEqual(Vector3D.UnitY, m * Vector3D.UnitX)); }
public void MultiplyVector() { Vector3D v = new Vector3D(2.34, 3.45, 4.56); Assert.AreEqual(v, Matrix33D.Multiply(Matrix33D.Identity, v)); Assert.AreEqual(Vector3D.Zero, Matrix33D.Multiply(Matrix33D.Zero, v)); Matrix33D m = new Matrix33D(12, 23, 45, 67, 89, 90, 43, 65, 87); Assert.IsTrue(Vector3D.AreNumericallyEqual(v, Matrix33D.Multiply(m * m.Inverse, v))); for (int i = 0; i < 3; i++) Assert.AreEqual(Vector3D.Dot(m.GetRow(i), v), Matrix33D.Multiply(m, v)[i]); }
public void QuaternionDFromMatrix33() { Vector3D v = Vector3D.One; Matrix33D m = Matrix33D.Identity; QuaternionD q = QuaternionD.CreateRotation(m); Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v))); m = Matrix33D.CreateRotation(Vector3D.UnitX, 0.3); q = QuaternionD.CreateRotation(m); Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v))); m = Matrix33D.CreateRotation(Vector3D.UnitY, 1.0); q = QuaternionD.CreateRotation(m); Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v))); m = Matrix33D.CreateRotation(Vector3D.UnitZ, 4.0); q = QuaternionD.CreateRotation(m); Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v))); m = Matrix33D.Identity; q = QuaternionD.CreateRotation(m); Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v))); m = Matrix33D.CreateRotation(-Vector3D.UnitX, 1.3); q = QuaternionD.CreateRotation(m); Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v))); m = Matrix33D.CreateRotation(-Vector3D.UnitY, -1.4); q = QuaternionD.CreateRotation(m); Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v))); m = Matrix33D.CreateRotation(-Vector3D.UnitZ, -0.1); q = QuaternionD.CreateRotation(m); Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v))); m = new Matrix33D(0, 0, 1, 0, -1, 0, 1, 0, 0); q = QuaternionD.CreateRotation(m); Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v))); m = new Matrix33D(-1, 0, 0, 0, 1, 0, 0, 0, -1); q = QuaternionD.CreateRotation(m); Assert.IsTrue(Vector3D.AreNumericallyEqual(m * v, q.Rotate(v))); }
public void AreEqual() { double originalEpsilon = Numeric.EpsilonD; Numeric.EpsilonD = 1e-8; Vector3D u = new Vector3D(1.0, 2.0, 3.0); Vector3D v = new Vector3D(1.000001, 2.000001, 3.000001); Vector3D w = new Vector3D(1.00000001, 2.00000001, 3.00000001); Assert.IsTrue(Vector3D.AreNumericallyEqual(u, u)); Assert.IsFalse(Vector3D.AreNumericallyEqual(u, v)); Assert.IsTrue(Vector3D.AreNumericallyEqual(u, w)); Numeric.EpsilonD = originalEpsilon; }
public void Interpolate() { PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3)); PoseD p2 = new PoseD(new Vector3D(-4, 5, -6), QuaternionD.CreateRotationZ(-0.1)); Assert.IsTrue(Vector3D.AreNumericallyEqual(p1.Position, PoseD.Interpolate(p1, p2, 0).Position)); Assert.IsTrue(Matrix33D.AreNumericallyEqual(p1.Orientation, PoseD.Interpolate(p1, p2, 0).Orientation)); Assert.IsTrue(Vector3D.AreNumericallyEqual(p2.Position, PoseD.Interpolate(p1, p2, 1).Position)); Assert.IsTrue(Matrix33D.AreNumericallyEqual(p2.Orientation, PoseD.Interpolate(p1, p2, 1).Orientation)); Assert.IsTrue(Vector3D.AreNumericallyEqual(InterpolationHelper.Lerp(p1.Position, p2.Position, 0.3), PoseD.Interpolate(p1, p2, 0.3).Position)); Assert.IsTrue( QuaternionD.AreNumericallyEqual( InterpolationHelper.Lerp(QuaternionD.CreateRotation(p1.Orientation), QuaternionD.CreateRotation(p2.Orientation), 0.3), QuaternionD.CreateRotation(PoseD.Interpolate(p1, p2, 0.3).Orientation))); }
public void Division() { double angle1 = 0.4; Vector3D axis1 = new Vector3D(1.0, 2.0, 3.0); QuaternionD q1 = QuaternionD.CreateRotation(axis1, angle1); Matrix33D m1 = Matrix33D.CreateRotation(axis1, angle1); double angle2 = -1.6; Vector3D axis2 = new Vector3D(1.0, -2.0, -3.5); QuaternionD q2 = QuaternionD.CreateRotation(axis2, angle2); Matrix33D m2 = Matrix33D.CreateRotation(axis2, angle2); Vector3D v = new Vector3D(0.3, -2.4, 5.6); Vector3D result1 = QuaternionD.Divide(q2, q1).Rotate(v); Vector3D result2 = m2 * m1.Inverse * v; Assert.IsTrue(Vector3D.AreNumericallyEqual(result1, result2)); }
public void LerpQuaternionD() { // Warning: The not all results are not verified QuaternionD q1 = new QuaternionD(1.0, 2.0, 3.0, 4.0).Normalized; QuaternionD q2 = new QuaternionD(2.0, 4.0, 6.0, 8.0).Normalized; QuaternionD lerp = InterpolationHelper.Lerp(q1, q2, 0.75); Assert.IsTrue(lerp.IsNumericallyNormalized); lerp = InterpolationHelper.Lerp(q1, q2, 0); Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, lerp)); lerp = InterpolationHelper.Lerp(q1, q2, 1); Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, lerp)); q1 = QuaternionD.Identity; q2 = QuaternionD.CreateRotation(Vector3D.UnitZ, Math.PI / 2); lerp = InterpolationHelper.Lerp(q1, q2, 0.5); Vector3D v = lerp.Rotate(Vector3D.UnitX); Vector3D result = new Vector3D(1.0, 1.0, 0.0).Normalized; Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v)); q1 = QuaternionD.Identity; q2 = QuaternionD.CreateRotation(Vector3D.UnitY, Math.PI / 2); lerp = InterpolationHelper.Lerp(q1, q2, 0.5); v = lerp.Rotate(Vector3D.UnitZ); result = new Vector3D(1.0, 0.0, 1.0).Normalized; Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v)); q1 = QuaternionD.Identity; q2 = QuaternionD.CreateRotation(Vector3D.UnitX, Math.PI / 2); lerp = InterpolationHelper.Lerp(q1, q2, 0.5); v = lerp.Rotate(Vector3D.UnitY); result = new Vector3D(0.0, 1.0, 1.0).Normalized; Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v)); q1 = new QuaternionD(-1.0, 0.0, 0.0, 0.0); q2 = QuaternionD.CreateRotation(-Vector3D.UnitZ, -Math.PI / 2); lerp = InterpolationHelper.Lerp(q1, q2, 0.5); v = lerp.Rotate(Vector3D.UnitX); result = new Vector3D(1.0, 1.0, 0.0).Normalized; Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v)); }
public void Test1() { PoseD p = PoseD.Identity; Assert.AreEqual(Matrix44D.Identity, p.ToMatrix44D()); Assert.AreEqual(Matrix33D.Identity, p.Orientation); Assert.AreEqual(Vector3D.Zero, p.Position); p.Position = new Vector3D(1, 2, 3); p.Orientation = Matrix33D.CreateRotation(new Vector3D(3, -4, 9), 0.49); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitX), 0), p * new Vector4D(1, 0, 0, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitY), 0), p * new Vector4D(0, 1, 0, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldDirection(Vector3D.UnitZ), 0), p * new Vector4D(0, 0, 1, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitX), 1), p * new Vector4D(1, 0, 0, 1))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitY), 1), p * new Vector4D(0, 1, 0, 1))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToWorldPosition(Vector3D.UnitZ), 1), p * new Vector4D(0, 0, 1, 1))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitX), 0), p.Inverse * new Vector4D(1, 0, 0, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitY), 0), p.Inverse * new Vector4D(0, 1, 0, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalDirection(Vector3D.UnitZ), 0), p.Inverse * new Vector4D(0, 0, 1, 0))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitX), 1), p.Inverse * new Vector4D(1, 0, 0, 1))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitY), 1), p.Inverse * new Vector4D(0, 1, 0, 1))); Assert.IsTrue(Vector4D.AreNumericallyEqual(new Vector4D(p.ToLocalPosition(Vector3D.UnitZ), 1), p.Inverse * new Vector4D(0, 0, 1, 1))); PoseD p2 = PoseD.FromMatrix(new Matrix44D(p.Orientation, Vector3D.Zero)); Assert.IsTrue(Matrix33D.AreNumericallyEqual(p.Orientation, p2.Orientation)); Assert.IsTrue(Vector3D.AreNumericallyEqual(p2.Position, Vector3D.Zero)); Matrix44D m = p2; m.SetColumn(3, new Vector4D(p.Position, 1)); p2 = PoseD.FromMatrix(m); Assert.IsTrue(Matrix33D.AreNumericallyEqual(p.Orientation, p2.Orientation)); Assert.AreEqual(p.Position, p2.Position); //Assert.IsTrue(Vector3D.AreNumericallyEqual(p.Position, p2.Position)); // Test other constructors. Assert.AreEqual(Vector3D.Zero, new PoseD(QuaternionD.CreateRotationX(0.3)).Position); Assert.AreEqual(Matrix33D.CreateRotationX(0.3), new PoseD(Matrix33D.CreateRotationX(0.3)).Orientation); Assert.AreEqual(new Vector3D(1, 2, 3), new PoseD(new Vector3D(1, 2, 3)).Position); Assert.AreEqual(Matrix33D.Identity, new PoseD(new Vector3D(1, 2, 3)).Orientation); }
public void Axis() { Vector3D axis = new Vector3D(1.0, 2.0, 3.0); double angle = 0.2; QuaternionD q = QuaternionD.CreateRotation(axis, angle); Assert.IsTrue(Numeric.AreEqual(angle, q.Angle)); Assert.IsTrue(Vector3D.AreNumericallyEqual(axis.Normalized, q.Axis)); axis = new Vector3D(1.0, 1.0, 1.0); q.Axis = axis; Assert.IsTrue(Numeric.AreEqual(angle, q.Angle)); Assert.IsTrue(Vector3D.AreNumericallyEqual(axis.Normalized, q.Axis)); Assert.IsTrue(Vector3D.AreNumericallyEqual(Matrix33D.CreateRotation(axis, angle) * Vector3D.One, q.Rotate(Vector3D.One))); Assert.AreEqual(Vector3D.Zero, QuaternionD.Identity.Axis); q.Axis = Vector3D.Zero; Assert.AreEqual(QuaternionD.Identity, q); }
public void Inverse() { QuaternionD identity = QuaternionD.Identity; QuaternionD inverseIdentity = identity.Inverse; Assert.AreEqual(inverseIdentity, identity); double angle = 0.4; Vector3D axis = new Vector3D(1.0, 1.0, 1.0); axis.Normalize(); QuaternionD q = QuaternionD.CreateRotation(axis, angle); QuaternionD inverse = q.Inverse; Assert.IsTrue(Vector3D.AreNumericallyEqual(-axis, inverse.Axis)); q = new QuaternionD(1, 2, 3, 4); inverse = q.Inverse; Assert.IsTrue(QuaternionD.AreNumericallyEqual(QuaternionD.Identity, inverse * q)); }
public void SlerpGeneralDoublePrecision() { QuaternionD q1 = QuaternionD.CreateRotation(-Vector3D.UnitY, Math.PI / 2); QuaternionD q2 = QuaternionD.CreateRotation(Vector3D.UnitZ, Math.PI / 2); QuaternionD slerp = InterpolationHelper.Slerp(q1, q2, 0.5); Assert.IsTrue(slerp.IsNumericallyNormalized); Vector3D v = slerp.Rotate(Vector3D.UnitX); Vector3D result = new Vector3D(1.0 / 3.0, 2.0 / 3.0, 2.0 / 3.0); // I hope this is correct. Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v)); q1 = QuaternionD.CreateRotation(-Vector3D.UnitY, Math.PI / 2); q2 = QuaternionD.CreateRotation(-Vector3D.UnitZ, -Math.PI / 2); slerp = InterpolationHelper.Slerp(q1, q2, 0.5); Assert.IsTrue(slerp.IsNumericallyNormalized); v = slerp.Rotate(Vector3D.UnitX); result = new Vector3D(1.0 / 3.0, 2.0 / 3.0, 2.0 / 3.0); // I hope this is correct. Assert.IsTrue(Vector3D.AreNumericallyEqual(result, v)); }
public void CreateRotation() { QuaternionD q; // From matrix vs. from angle/axis Matrix33D m = Matrix33D.CreateRotation(Vector3D.UnitX, (double)Math.PI / 4); q = QuaternionD.CreateRotation(m); QuaternionD q2 = QuaternionD.CreateRotation(Vector3D.UnitX, (double)Math.PI / 4); Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, q)); m = Matrix33D.CreateRotation(Vector3D.UnitY, (double)Math.PI / 4); q = QuaternionD.CreateRotation(m); q2 = QuaternionD.CreateRotation(Vector3D.UnitY, (double)Math.PI / 4); Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, q)); m = Matrix33D.CreateRotation(Vector3D.UnitZ, (double)Math.PI / 4); q = QuaternionD.CreateRotation(m); q2 = QuaternionD.CreateRotation(Vector3D.UnitZ, (double)Math.PI / 4); Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, q)); // From vector-vector Vector3D start, end; start = Vector3D.UnitX; end = Vector3D.UnitY; q = QuaternionD.CreateRotation(start, end); Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start)); start = Vector3D.UnitY; end = Vector3D.UnitZ; q = QuaternionD.CreateRotation(start, end); Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start)); start = Vector3D.UnitZ; end = Vector3D.UnitX; q = QuaternionD.CreateRotation(start, end); Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start)); start = new Vector3D(1, 1, 1); end = new Vector3D(1, 1, 1); q = QuaternionD.CreateRotation(start, end); Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start)); start = new Vector3D(1.0, 1.0, 1.0); end = new Vector3D(-1.0, -1.0, -1.0); q = QuaternionD.CreateRotation(start, end); Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start)); start = new Vector3D(-1.0, 2.0, 1.0); end = new Vector3D(-2.0, -1.0, -1.0); q = QuaternionD.CreateRotation(start, end); Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start)); double degree45 = MathHelper.ToRadians(45); q = QuaternionD.CreateRotation(degree45, Vector3D.UnitZ, degree45, Vector3D.UnitY, degree45, Vector3D.UnitX, false); QuaternionD expected = QuaternionD.CreateRotation(Vector3D.UnitZ, degree45) * QuaternionD.CreateRotation(Vector3D.UnitY, degree45) * QuaternionD.CreateRotation(Vector3D.UnitX, degree45); Assert.IsTrue(QuaternionD.AreNumericallyEqual(expected, q)); q = QuaternionD.CreateRotation(degree45, Vector3D.UnitZ, degree45, Vector3D.UnitY, degree45, Vector3D.UnitX, true); expected = QuaternionD.CreateRotation(Vector3D.UnitX, degree45) * QuaternionD.CreateRotation(Vector3D.UnitY, degree45) * QuaternionD.CreateRotation(Vector3D.UnitZ, degree45); Assert.IsTrue(QuaternionD.AreNumericallyEqual(expected, q)); }