public void CreateRotationY() { double angle = 0.3; QuaternionD q = QuaternionD.CreateRotation(Vector3D.UnitY, angle); QuaternionD qy = QuaternionD.CreateRotationY(angle); Assert.AreEqual(q, qy); }
public void MultiplyOperator() { 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(Vector4D.AreNumericallyEqual( p1.ToMatrix44D() * p2.ToMatrix44D() * new Vector4D(1, 2, 3, 1), p1 * p2 * new Vector4D(1, 2, 3, 1))); }
public void Equals() { PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3)); PoseD p2 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3)); Assert.AreEqual(p1, p2); Assert.IsTrue(p1.Equals((object)p2)); Assert.IsTrue(p1.Equals(p2)); Assert.IsFalse(p1.Equals(p2.ToMatrix44D())); }
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 GetHashCodeTest() { PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3)); PoseD p2 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3)); Assert.AreEqual(p1.GetHashCode(), p2.GetHashCode()); p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3)); p2 = new PoseD(new Vector3D(2, 1, 3), QuaternionD.CreateRotationY(0.3)); Assert.AreNotEqual(p1.GetHashCode(), p2.GetHashCode()); // Too bad two rotation matrices that differ only by the sign of the angle // (+/- angle with same axis) have the same hashcodes. See KB -> .NET --> GetHashCode //p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3)); //p2 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(-0.3)); //Assert.AreNotEqual(p1.GetHashCode(), p2.GetHashCode()); }