@Test public void testDifference() { Random random = new Random(65445L); T diff = createEmptyTuple(); T expected = createEmptyTuple(); for (int i = 0; i < ITERATIONS; i++) { T q1 = createRandomTuple(random); T q2 = createRandomTuple(random); diff.difference(q1, q2); QuaternionTools.multiplyConjugateLeft(q1, q2, expected); EuclidCoreTestTools.assertQuaternionEquals(diff, expected, getEpsilon()); } }
@Test public void testSetToZero() { Random random = new Random(621541L); T quaternion = createRandomTuple(random); quaternion.setToZero(); T zeroQ = createTuple(0.0, 0.0, 0.0, 1.0); EuclidCoreTestTools.assertQuaternionEquals(quaternion, zeroQ, getEpsilon()); }
@Test public void testSetAxisAngle() { Random random = new Random(574631L); T actualQuaternion = createEmptyTuple(); T expectedQuaternion = createEmptyTuple(); AxisAngle axisAngle; for (int i = 0; i < ITERATIONS; i++) { // Test set(AxisAngleReadOnly axisAngle) axisAngle = EuclidCoreRandomTools.nextAxisAngle(random); QuaternionConversion.convertAxisAngleToQuaternion(axisAngle, expectedQuaternion); actualQuaternion.set(axisAngle); EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, actualQuaternion, getEpsilon()); } }
T tuple1 = createEmptyTuple(); T tuple2 = createEmptyTuple(); T original = createRandomTuple(random); double xOriginal = signX * original.getX(); double yOriginal = signY * original.getY(); double zOriginal = signZ * original.getZ(); double sOriginal = signS * original.getS(); tuple1 = createTuple(xOriginal, yOriginal, zOriginal, sOriginal); assertEquals(tuple2.getX(), -xOriginal, getEpsilon()); assertEquals(tuple2.getY(), -yOriginal, getEpsilon()); assertEquals(tuple2.getZ(), -zOriginal, getEpsilon()); assertEquals(tuple2.getS(), -sOriginal, getEpsilon()); assertEquals(tuple1.getX(), xOriginal, getEpsilon()); assertEquals(tuple1.getY(), yOriginal, getEpsilon()); assertEquals(tuple1.getZ(), zOriginal, getEpsilon()); assertEquals(tuple1.getS(), sOriginal, getEpsilon()); assertEquals(tuple1.getX(), -xOriginal, getEpsilon()); assertEquals(tuple1.getY(), -yOriginal, getEpsilon()); assertEquals(tuple1.getZ(), -zOriginal, getEpsilon()); assertEquals(tuple1.getS(), -sOriginal, getEpsilon());
@Test public void testGetRotationVector() throws Exception { Random random = new Random(23423L); for (int i = 0; i < ITERATIONS; i++) { T quaternion = createRandomTuple(random); Vector3D expectedRotationVector = new Vector3D(); Vector3D actualRotationVector = new Vector3D(); RotationVectorConversion.convertQuaternionToRotationVector(quaternion, expectedRotationVector); quaternion.getRotationVector(actualRotationVector); EuclidCoreTestTools.assertTuple3DEquals(expectedRotationVector, actualRotationVector, getEpsilon()); } }
@Override public double getEpsilon() { return QuaternionBasicsTest.this.getEpsilon(); } };
@Override public Orientation3DBasics createEmptyOrientation3DBasics() { return createEmptyTuple(); }
T quaternion = createEmptyTuple(); Tuple3DBasics actualTuple = new Vector3D(tuple); Tuple3DBasics expectedTuple = EuclidCoreRandomTools.nextVector3D(random); quaternion = createRandomTuple(random); EuclidCoreTestTools.assertTuple3DEquals(expectedTuple, actualTuple, getEpsilon()); Tuple3DBasics actualTuple = new Vector3D(tuple); Tuple3DBasics expectedTuple = EuclidCoreRandomTools.nextVector3D(random); quaternion = createRandomTuple(random); EuclidCoreTestTools.assertTuple3DEquals(expectedTuple, actualTuple, getEpsilon()); float qz = (float) Math.sin(0.5 * theta); float qs = (float) Math.cos(0.5 * theta); quaternion = createTuple(0.0f, 0.0f, qz, qs); EuclidCoreTestTools.assertTuple2DEquals(expectedTuple, actualTuple, getEpsilon()); actualTuple.set(tuple); quaternion.inverseTransform(actualTuple, true); EuclidCoreTestTools.assertTuple2DEquals(expectedTuple, actualTuple, getEpsilon()); actualTuple.set(tuple); quaternion.inverseTransform(actualTuple, false); EuclidCoreTestTools.assertTuple2DEquals(expectedTuple, actualTuple, getEpsilon()); float qz = (float) Math.sin(0.5 * theta); float qs = (float) Math.cos(0.5 * theta); quaternion = createTuple(0.0f, 0.0f, qz, qs);
@Test public void testSetRollQuaternion() { Random random = new Random(574631L); T actualQuaternion = createEmptyTuple(); T expectedQuaternion = createEmptyTuple(); for (int i = 0; i < ITERATIONS; i++) { // Test setToRollQuaternion(double roll) double roll = EuclidCoreRandomTools.nextDouble(random, 2.0 * Math.PI); actualQuaternion.setToRollQuaternion(roll); expectedQuaternion.set(new AxisAngle(1.0, 0.0, 0.0, roll)); EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, actualQuaternion, getEpsilon()); } }
for (int i = 0; i < ITERATIONS; i++) quaternion = createRandomTuple(random); Assert.assertEquals(yawPitchRoll[j], expectedYawPitchRoll[j], getEpsilon()); Vector3DBasics expectedEulerAngles = new Vector3D(); YawPitchRollConversion.convertQuaternionToYawPitchRoll(quaternion, expectedEulerAngles); EuclidCoreTestTools.assertTuple3DEquals(expectedEulerAngles, eulerAngles, getEpsilon()); Assert.assertEquals(yaw, expectedYaw, getEpsilon()); Assert.assertEquals(pitch, expectedPitch, getEpsilon()); Assert.assertEquals(roll, expectedRoll, getEpsilon());
@Test public void testGetAngle() { Random random = new Random(65445L); double expectedAngle = 2.0 * Math.PI * random.nextDouble(); // Sign issue when theta < 0.0 double c = Math.cos(expectedAngle / 2.0); double s = Math.sin(expectedAngle / 2.0); Vector3D axis = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); Quaternion q = new Quaternion(); double qx = s * axis.getX(); double qy = s * axis.getY(); double qz = s * axis.getZ(); double qs = c; q.setUnsafe(qx, qy, qz, qs); assertEquals(expectedAngle, q.getAngle(), getEpsilon()); }
@Test public void testPreMultiply() { Random random = new Random(65445L); for (int i = 0; i < ITERATIONS; i++) { T qOther1 = createRandomTuple(random); T qOther2 = createRandomTuple(random); T qActual = createRandomTuple(random); T qExpected = createEmptyTuple(); { // Test preMultiply(QuaternionBasics other) qActual.set(qOther1); qExpected.set(qOther1); qActual.preMultiply(qOther2); QuaternionTools.multiply(qOther2, qExpected, qExpected); EuclidCoreTestTools.assertQuaternionEquals(qActual, qExpected, getEpsilon()); } } }
T quaternion = createEmptyTuple(); Tuple3DBasics actualTuple = new Vector3D(tuple); Tuple3DBasics expectedTuple = EuclidCoreRandomTools.nextVector3D(random); quaternion = createRandomTuple(random); EuclidCoreTestTools.assertTuple3DEquals(expectedTuple, actualTuple, getEpsilon()); Tuple3DBasics actualTuple = new Vector3D(tuple); Tuple3DBasics expectedTuple = EuclidCoreRandomTools.nextVector3D(random); quaternion = createRandomTuple(random); EuclidCoreTestTools.assertTuple3DEquals(expectedTuple, actualTuple, getEpsilon()); Tuple3DBasics actualTuple = new Vector3D(tuple); Tuple3DBasics expectedTuple = new Vector3D(tuple); quaternion = createRandomTuple(random); EuclidCoreTestTools.assertTuple3DEquals(expectedTuple, actualTuple, getEpsilon()); Tuple3DBasics actualTuple = EuclidCoreRandomTools.nextVector3D(random); Tuple3DBasics expectedTuple = new Vector3D(actualTuple); quaternion = createRandomTuple(random); EuclidCoreTestTools.assertTuple3DEquals(expectedTuple, actualTuple, getEpsilon()); float qz = (float) Math.sin(0.5 * theta); float qs = (float) Math.cos(0.5 * theta); quaternion = createTuple(0.0f, 0.0f, qz, qs);
@Test public void testSetRotationMatrix() { Random random = new Random(574631L); T actualQuaternion = createEmptyTuple(); T expectedQuaternion = createEmptyTuple(); RotationMatrix rotationMatrix; for (int i = 0; i < ITERATIONS; i++) { // Test set(RotationMatrixReadOnly rotationMatrix) rotationMatrix = EuclidCoreRandomTools.nextRotationMatrix(random); QuaternionConversion.convertMatrixToQuaternion(rotationMatrix, expectedQuaternion); actualQuaternion.set(rotationMatrix); EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, actualQuaternion, getEpsilon()); } }
@Test public void testIsUnitary() { Random random = new Random(65445L); for (int i = 0; i < ITERATIONS; i++) { T q1 = createRandomTuple(random); assertTrue(q1.isUnitary(getEpsilon())); // Quaternion should have norm = 1 T q2 = createRandomTuple(random); q1 = createTuple(q2.getX(), q2.getY(), q2.getZ(), q2.getS()); assertTrue(q1.isUnitary(getEpsilon())); double delta = 6.0 * Math.sqrt(getEpsilon()); q1 = createTuple(delta + q2.getX(), q2.getY(), q2.getZ(), q2.getS()); assertFalse(q1.isUnitary(getEpsilon())); q1 = createTuple(q2.getX(), delta + q2.getY(), q2.getZ(), q2.getS()); assertFalse(q1.isUnitary(getEpsilon())); q1 = createTuple(q2.getX(), q2.getY(), delta + q2.getZ(), q2.getS()); assertFalse(q1.isUnitary(getEpsilon())); q1 = createTuple(q2.getX(), q2.getY(), q2.getZ(), delta + q2.getS()); assertFalse(q1.isUnitary(getEpsilon())); } }
@Override @Test public void testNormalize() { super.testNormalize(); Quaternion expected = new Quaternion(); Quaternion actual = new Quaternion(); actual.setUnsafe(0.0, 0.0, 0.0, 0.0); actual.normalize(); EuclidCoreTestTools.assertTuple4DEquals(expected, actual, getEpsilon()); Quaternion q = new Quaternion(); q.setUnsafe(1.0, 1.0, 1.0, 1.0); actual.setAndNormalize(q); assertTrue(actual.isUnitary(getEpsilon())); q.setUnsafe(1.0, 1.0, 1.0, 1.0); actual.setAndNormalize((Orientation3DReadOnly) q); assertTrue(actual.isUnitary(getEpsilon())); }
@Test public void testMultiply() { Random random = new Random(65445L); for (int i = 0; i < ITERATIONS; i++) { T qOther1 = createRandomTuple(random); T qOther2 = createRandomTuple(random); T qActual = createRandomTuple(random); T qExpected = createEmptyTuple(); { // Test multiply(QuaternionBasics other) qActual.set(qOther1); qExpected.set(qOther1); qActual.multiply(qOther2); QuaternionTools.multiply(qExpected, qOther2, qExpected); EuclidCoreTestTools.assertQuaternionEquals(qActual, qExpected, getEpsilon()); } { // Test multiply(QuaternionBasics q1, QuaternionBasics q2) qActual.multiply(qOther1, qOther2); QuaternionTools.multiply(qOther1, qOther2, qExpected); EuclidCoreTestTools.assertQuaternionEquals(qActual, qExpected, getEpsilon()); } } }
@Test public void testSetRotationVector() { Random random = new Random(574631L); T actualQuaternion = createEmptyTuple(); T expectedQuaternion = createEmptyTuple(); Vector3D rotationVector; for (int i = 0; i < ITERATIONS; i++) { // Test set(Vector3DReadOnly rotationVector) rotationVector = EuclidCoreRandomTools.nextRotationVector(random); QuaternionConversion.convertRotationVectorToQuaternion(rotationVector, expectedQuaternion); actualQuaternion.setRotationVector(rotationVector); EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, actualQuaternion, getEpsilon()); } }
quaternionA = createRandomTuple(random); quaternionB = createRandomTuple(random); quaternionC.difference(quaternionA, quaternionB); if (Math.abs(angle) <= getEpsilon()) assertTrue(quaternionA.geometricallyEquals(quaternionB, getEpsilon())); assertFalse(quaternionA.geometricallyEquals(quaternionB, getEpsilon())); double epsilon = EuclidCoreRandomTools.nextDouble(random, 100.0, 1000.0) * getEpsilon(); quaternionA = createRandomTuple(random); double angleDiff = 0.99 * epsilon; AxisAngle aa = new AxisAngle(EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0), angleDiff); quaternionB = createTuple(quaternionT.getX(), quaternionT.getY(), quaternionT.getZ(), quaternionT.getS()); double epsilon = EuclidCoreRandomTools.nextDouble(random, 100.0, 1000.0) * getEpsilon(); quaternionA = createRandomTuple(random); double angleDiff = 1.01 * epsilon; AxisAngle aa = new AxisAngle(EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0), angleDiff); quaternionB = createTuple(quaternionT.getX(), quaternionT.getY(), quaternionT.getZ(), quaternionT.getS()); quaternionA = createRandomTuple(random); double angleDiff = 0.99 * epsilon; AxisAngle aa = new AxisAngle(EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0), angleDiff); quaternionB = createTuple(quaternionT.getX(), quaternionT.getY(), quaternionT.getZ(), quaternionT.getS());
@Test public void testPow() throws Exception { Random random = new Random(541651L); for (int i = 0; i < ITERATIONS; i++) { QuaternionBasics original = createRandomTuple(random); double alpha = EuclidCoreRandomTools.nextDouble(random, 2.0); AxisAngle axisAngle = new AxisAngle(original); axisAngle.setAngle(alpha * axisAngle.getAngle()); QuaternionBasics expected = createEmptyTuple(); expected.set(axisAngle); QuaternionBasics actual = createEmptyTuple(); actual.set(original); actual.pow(alpha); EuclidCoreTestTools.assertQuaternionEquals(expected, actual, getEpsilon()); } }