QuaternionBasedTransform expected = new QuaternionBasedTransform(); QuaternionBasedTransform actual = new QuaternionBasedTransform(); RotationMatrix expectedRotation = new RotationMatrix(original.getQuaternion()); double yaw = EuclidCoreRandomTools.nextDouble(random, Math.PI); expectedRotation.appendYawRotation(yaw); expected.set(expectedRotation, original.getTranslationVector()); actual.set(original); actual.appendYawRotation(yaw); RotationMatrix expectedRotation = new RotationMatrix(original.getQuaternion()); double pitch = EuclidCoreRandomTools.nextDouble(random, Math.PI); expectedRotation.appendPitchRotation(pitch); expected.set(expectedRotation, original.getTranslationVector()); actual.set(original); actual.appendPitchRotation(pitch); RotationMatrix expectedRotation = new RotationMatrix(original.getQuaternion()); double roll = EuclidCoreRandomTools.nextDouble(random, Math.PI); expectedRotation.appendRollRotation(roll); expected.set(expectedRotation, original.getTranslationVector()); actual.set(original); actual.appendRollRotation(roll);
QuaternionBasedTransform m2 = new QuaternionBasedTransform(); double epsilon = 1.0e-3; double[] coeffs = new double[9]; assertFalse(m1.epsilonEquals(m2, epsilon)); m2.set(m1); assertTrue(m1.epsilonEquals(m2, epsilon)); m2.set(m1); assertTrue(m1.epsilonEquals(m2, epsilon)); m1.get(coeffs); coeffs[index] += 0.999 * epsilon; m2.setUnsafe(coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6]); assertTrue(m1.epsilonEquals(m2, epsilon)); m2.set(m1); assertTrue(m1.epsilonEquals(m2, epsilon)); m1.get(coeffs); coeffs[index] += 1.001 * epsilon; m2.setUnsafe(coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6]); assertFalse(m1.epsilonEquals(m2, epsilon)); m2.set(m1); assertTrue(m1.epsilonEquals(m2, epsilon)); m1.get(coeffs); coeffs[index] -= 0.999 * epsilon; m2.setUnsafe(coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5], coeffs[6]); assertTrue(m1.epsilonEquals(m2, epsilon));
QuaternionBasedTransform t2 = new QuaternionBasedTransform(); assertFalse(t1.equals(t2)); assertFalse(t1.equals(null)); assertFalse(t1.equals(new double[4])); t2.set(t1); assertTrue(t1.equals(t2)); assertTrue(t1.equals((Object) t2)); t2.set(t1); assertTrue(t1.equals(t2)); t1.get(coeffs); coeffs[index] += smallestEpsilon; t2.set(coeffs); assertFalse(t1.equals(t2)); t2.set(t1); assertTrue(t1.equals(t2)); t1.get(coeffs); coeffs[index] -= smallestEpsilon; t2.set(coeffs); assertFalse(t1.equals(t2));
@Test public void testContainsNaN() throws Exception { QuaternionBasedTransform transform = new QuaternionBasedTransform(); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); assertFalse(transform.containsNaN()); transform.setUnsafe(Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN, 0.0); assertTrue(transform.containsNaN()); transform.setUnsafe(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Double.NaN); assertTrue(transform.containsNaN()); }
/** * Sets this pose 3D to match the given quaternion-based transform. * * @param quaternionBasedTransform the transform use to set this pose 3D. Not modified. */ default void set(QuaternionBasedTransform quaternionBasedTransform) { setPosition(quaternionBasedTransform.getTranslationVector()); setOrientation(quaternionBasedTransform.getQuaternion()); }
QuaternionBasedTransform expected = new QuaternionBasedTransform(); QuaternionBasedTransform actual = new QuaternionBasedTransform(); QuaternionBasedTransform translationTransform = new QuaternionBasedTransform(); double x = EuclidCoreRandomTools.nextDouble(random, -10.0, 10.0); double z = EuclidCoreRandomTools.nextDouble(random, -10.0, 10.0); double y = EuclidCoreRandomTools.nextDouble(random, -10.0, 10.0); translationTransform.setTranslation(x, y, z); expected.set(original); expected.multiply(translationTransform); actual.set(original); actual.appendTranslation(x, y, z); QuaternionBasedTransform translationTransform = new QuaternionBasedTransform(); Tuple3DReadOnly translation = EuclidCoreRandomTools.nextPoint3D(random, 10.0, 10.0, 10.0); translationTransform.setTranslation(translation); expected.set(original); expected.multiply(translationTransform); actual.set(original); actual.appendTranslation(translation);
QuaternionBasedTransform actualTransform = new QuaternionBasedTransform(); Vector3D expectedTranslation = new Vector3D(); actualTransform.getTranslation(expectedTranslation); actualTransform.setRotation(new AxisAngle(quaternion)); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, actualTransform.getTranslationVector(), EPS); actualTransform.setRotation(quaternion); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, actualTransform.getTranslationVector(), EPS); actualTransform.setRotation(new RotationMatrix(quaternion)); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, actualTransform.getTranslationVector(), EPS); Vector3D rotationVector = new Vector3D(); quaternion.getRotationVector(rotationVector); actualTransform.setRotation(rotationVector); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, actualTransform.getTranslationVector(), EPS); double yaw = EuclidCoreRandomTools.nextDouble(random, Math.PI); rigidBodyTransform.setRotationYaw(yaw); QuaternionBasedTransform expectedTransform = new QuaternionBasedTransform(rigidBodyTransform); actualTransform.setRotationYaw(yaw); EuclidCoreTestTools.assertQuaternionBasedTransformGeometricallyEquals(expectedTransform, actualTransform, EPS); double pitch = EuclidCoreRandomTools.nextDouble(random, Math.PI);
QuaternionBasedTransform actualTransform = new QuaternionBasedTransform(); Quaternion expectedQuaternion = new Quaternion(); actualTransform.getRotation(expectedQuaternion); double y = translation.getY(); double z = translation.getZ(); actualTransform.setTranslationX(x); actualTransform.setTranslationY(y); actualTransform.setTranslationZ(z); for (int row = 0; row < 3; row++) EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); double y = translation.getY(); double z = translation.getZ(); actualTransform.setTranslation(x, y, z); EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); actualTransform.setTranslation(translation); EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS);
QuaternionBasedTransform expected = new QuaternionBasedTransform(); QuaternionBasedTransform actual = new QuaternionBasedTransform(); QuaternionBasedTransform yawTransform = new QuaternionBasedTransform(); yawTransform.setRotationYaw(yaw); expected.set(original); expected.preMultiply(yawTransform); actual.set(original); actual.prependYawRotation(yaw); QuaternionBasedTransform pitchTransform = new QuaternionBasedTransform(); pitchTransform.setRotationPitch(pitch); expected.set(original); expected.preMultiply(pitchTransform); actual.set(original); actual.prependPitchRotation(pitch); QuaternionBasedTransform rollTransform = new QuaternionBasedTransform(); rollTransform.setRotationRoll(roll); expected.set(original); expected.preMultiply(rollTransform); actual.set(original); actual.prependRollRotation(roll);
QuaternionBasedTransform actualTransform = new QuaternionBasedTransform(); double y = translation.getY(); double z = translation.getZ(); actualTransform.set(qx, qy, qz, qs, x, y, z); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); double y = translation.getY(); double z = translation.getZ(); actualTransform.setUnsafe(qx, qy, qz, qs, x, y, z); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); actualTransform.set(expectedTransform); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expectedTransform, actualTransform, EPS); actualTransform.set(new RigidBodyTransform(expectedTransform)); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expectedTransform, actualTransform, EPS); quaternion.get(denseMatrix); translation.get(4, denseMatrix); actualTransform.set(denseMatrix); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); quaternion.get(array); translation.get(4, array);
@Test public void testSetToNaN() throws Exception { Random random = new Random(45L); QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); assertFalse(Double.isNaN(transform.getQuaternion().getX())); assertFalse(Double.isNaN(transform.getQuaternion().getY())); assertFalse(Double.isNaN(transform.getQuaternion().getZ())); assertFalse(Double.isNaN(transform.getQuaternion().getS())); assertFalse(Double.isNaN(transform.getTranslationVector().getX())); assertFalse(Double.isNaN(transform.getTranslationVector().getY())); assertFalse(Double.isNaN(transform.getTranslationVector().getZ())); transform.setToNaN(); EuclidCoreTestTools.assertTuple4DContainsOnlyNaN(transform.getQuaternion()); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(transform.getTranslationVector()); transform.setToZero(); EuclidCoreTestTools.assertTuple4DEquals(new Quaternion(), transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(), transform.getTranslationVector(), EPS); transform.setRotationToNaN(); EuclidCoreTestTools.assertTuple4DContainsOnlyNaN(transform.getQuaternion()); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(), transform.getTranslationVector(), EPS); transform.setToZero(); transform.setTranslationToNaN(); EuclidCoreTestTools.assertTuple4DEquals(new Quaternion(), transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(transform.getTranslationVector()); }
QuaternionBasedTransform qf = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); actual.interpolate(q0, qf, 0.0); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(q0, actual, EPS); actual.interpolate(qf, 0.0); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(q0, actual, EPS); actual.interpolate(qf, 1.0); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(qf, actual, EPS); actual.interpolate(q0, qf, 1.0); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(qf, actual, EPS); Quaternion interpolatedRotation = new Quaternion(); interpolatedVector.interpolate(q0.getTranslationVector(), qf.getTranslationVector(), alpha); interpolatedRotation.interpolate(q0.getQuaternion(), qf.getQuaternion(), alpha); expected.set(interpolatedRotation, interpolatedVector); actual.interpolate(q0, qf, alpha); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); actual.set(q0); actual.interpolate(qf, alpha); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS);
quaternion.preMultiply(qbtA.getQuaternion()); qbtB = new QuaternionBasedTransform(quaternion, qbtA.getTranslationVector()); assertTrue(qbtA.geometricallyEquals(qbtB, epsilon)); assertTrue(qbtB.geometricallyEquals(qbtA, epsilon)); quaternion.preMultiply(qbtA.getQuaternion()); qbtB = new QuaternionBasedTransform(quaternion, qbtA.getTranslationVector()); assertFalse(qbtA.geometricallyEquals(qbtB, epsilon)); assertFalse(qbtB.geometricallyEquals(qbtA, epsilon)); Vector3D translation = new Vector3D(qbtA.getTranslationVector()); Vector3D perturb = new Vector3D(translation); qbtB = new QuaternionBasedTransform(new Quaternion(qbtA.getQuaternion()), perturb); assertTrue(qbtA.geometricallyEquals(qbtB, epsilon)); assertTrue(qbtB.geometricallyEquals(qbtA, epsilon)); qbtB = new QuaternionBasedTransform(new Quaternion(qbtA.getQuaternion()), perturb); assertFalse(qbtA.geometricallyEquals(qbtB, epsilon)); assertFalse(qbtB.geometricallyEquals(qbtA, epsilon)); qbtB = new QuaternionBasedTransform(new Quaternion(qbtA.getQuaternion()), perturb);
@Test public void testGetTranslation() throws Exception { Random random = new Random(34L); QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); Vector3D expected = EuclidCoreRandomTools.nextVector3D(random); Vector3D actual = new Vector3D(); transform.setTranslation(expected); transform.getTranslation(actual); EuclidCoreTestTools.assertTuple3DEquals(expected, actual, EPS); EuclidCoreTestTools.assertTuple3DEquals(transform.getTranslationVector(), actual, EPS); Vector3D translation = new Vector3D(); translation.set(transform.getTranslationX(), transform.getTranslationY(), transform.getTranslationZ()); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); }
QuaternionBasedTransform expected = new QuaternionBasedTransform(); QuaternionBasedTransform actual = new QuaternionBasedTransform(); QuaternionBasedTransform translationTransform = new QuaternionBasedTransform(); double x = EuclidCoreRandomTools.nextDouble(random, -10.0, 10.0); double z = EuclidCoreRandomTools.nextDouble(random, -10.0, 10.0); double y = EuclidCoreRandomTools.nextDouble(random, -10.0, 10.0); translationTransform.setTranslation(x, y, z); expected.set(original); expected.preMultiply(translationTransform); actual.set(original); actual.prependTranslation(x, y, z); QuaternionBasedTransform translationTransform = new QuaternionBasedTransform(); Tuple3DReadOnly translation = EuclidCoreRandomTools.nextPoint3D(random, 10.0, 10.0, 10.0); translationTransform.setTranslation(translation); expected.set(original); expected.preMultiply(translationTransform); actual.set(original); actual.prependTranslation(translation);
@Test public void testResetRotation() throws Exception { Random random = new Random(3453L); QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); Vector3D expectedTranslation = new Vector3D(); transform.getTranslation(expectedTranslation); assertNotEquals(transform.getQuaternion().getX(), 0.0, EPS); assertNotEquals(transform.getQuaternion().getY(), 0.0, EPS); assertNotEquals(transform.getQuaternion().getZ(), 0.0, EPS); assertNotEquals(transform.getQuaternion().getS(), 1.0, EPS); assertNotEquals(transform.getTranslationVector().getX(), 0.0, EPS); assertNotEquals(transform.getTranslationVector().getY(), 0.0, EPS); assertNotEquals(transform.getTranslationVector().getZ(), 0.0, EPS); transform.setRotationToZero(); EuclidCoreTestTools.assertQuaternionIsSetToZero(transform.getQuaternion()); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, transform.getTranslationVector(), EPS); }
@Test public void testResetTranslation() throws Exception { Random random = new Random(45L); QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); Quaternion expectedQuaternion = new Quaternion(); transform.getRotation(expectedQuaternion); assertNotEquals(transform.getQuaternion().getX(), 0.0, EPS); assertNotEquals(transform.getQuaternion().getY(), 0.0, EPS); assertNotEquals(transform.getQuaternion().getZ(), 0.0, EPS); assertNotEquals(transform.getQuaternion().getS(), 1.0, EPS); assertNotEquals(transform.getTranslationVector().getX(), 0.0, EPS); assertNotEquals(transform.getTranslationVector().getY(), 0.0, EPS); assertNotEquals(transform.getTranslationVector().getZ(), 0.0, EPS); transform.setTranslationToZero(); EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DIsSetToZero(transform.getTranslationVector()); }
QuaternionBasedTransform transform = new QuaternionBasedTransform(); EuclidCoreTestTools.assertQuaternionIsSetToZero(transform.getQuaternion()); EuclidCoreTestTools.assertTuple3DIsSetToZero(transform.getTranslationVector()); QuaternionBasedTransform actual = new QuaternionBasedTransform(expected); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); QuaternionBasedTransform actual = new QuaternionBasedTransform(new RigidBodyTransform(expected)); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); quaternion.get(denseMatrix); translation.get(4, denseMatrix); QuaternionBasedTransform transform = new QuaternionBasedTransform(denseMatrix); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); quaternion.get(array); translation.get(4, array); QuaternionBasedTransform transform = new QuaternionBasedTransform(array); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); QuaternionBasedTransform transform = new QuaternionBasedTransform(new RotationMatrix(quaternion), translation); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); QuaternionBasedTransform transform = new QuaternionBasedTransform(quaternion, translation); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS);
@Test public void testPreMultiplyWithQuaternionBasedTransform() throws Exception { Random random = new Random(465416L); // Test against multiply(RigidBodyTransform) for (int i = 0; i < ITERATIONS; i++) { RigidBodyTransform original = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform expected = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform actual = EuclidCoreRandomTools.nextRigidBodyTransform(random); RigidBodyTransform multipliedWithRigidBody = EuclidCoreRandomTools.nextRigidBodyTransform(random); QuaternionBasedTransform multipliedWith = new QuaternionBasedTransform(multipliedWithRigidBody); expected.set(original); expected.preMultiply(multipliedWithRigidBody); actual.set(original); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); actual.preMultiply(multipliedWith); assertTrue(actual.hasRotation()); assertTrue(actual.hasTranslation()); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); multipliedWith.set(actual); multipliedWith.invert(); actual.preMultiply(multipliedWith); assertFalse(actual.hasRotation()); assertFalse(actual.hasTranslation()); expected.setToZero(); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); } }
@Test public void testInvert() throws Exception { Random random = new Random(345L); QuaternionBasedTransform original = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); RigidBodyTransform rigidBodyTransform = new RigidBodyTransform(); QuaternionBasedTransform expected = new QuaternionBasedTransform(); QuaternionBasedTransform actual = new QuaternionBasedTransform(); rigidBodyTransform.set(original); rigidBodyTransform.invert(); expected.set(rigidBodyTransform); actual.set(original); actual.invert(); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); rigidBodyTransform.set(original); rigidBodyTransform.invertRotation(); expected.set(rigidBodyTransform); actual.set(original); actual.invertRotation(); EuclidCoreTestTools.assertQuaternionBasedTransformEquals(expected, actual, EPS); }