@Override protected Vector3D getTranslation(QuaternionBasedTransform data) { return (Vector3D) data.getTranslationVector(); }
/** * 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()); }
/** * Appends the given {@code transform} to this pose 3D. * * @param transform the quaternion-based transform to append to this pose 3D. Not modified. */ default void appendTransform(QuaternionBasedTransform transform) { QuaternionTools.addTransform(getOrientation(), transform.getTranslationVector(), getPosition()); getOrientation().multiply(transform.getQuaternion()); }
@Test public void testSetToZero() throws Exception { Random random = new Random(45L); QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); 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.setToZero(); EuclidCoreTestTools.assertQuaternionIsSetToZero(transform.getQuaternion()); EuclidCoreTestTools.assertTuple3DIsSetToZero(transform.getTranslationVector()); }
@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()); }
@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); }
@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 testSetIdentity() throws Exception { Random random = new Random(45L); QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); 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.setIdentity(); EuclidCoreTestTools.assertQuaternionIsSetToZero(transform.getQuaternion()); EuclidCoreTestTools.assertTuple3DIsSetToZero(transform.getTranslationVector()); }
@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()); }
EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); 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);
EuclidCoreTestTools.assertTuple3DIsSetToZero(transform.getTranslationVector()); QuaternionBasedTransform transform = new QuaternionBasedTransform(denseMatrix); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); 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); QuaternionBasedTransform transform = new QuaternionBasedTransform(new AxisAngle(quaternion), translation); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS);
double yaw = EuclidCoreRandomTools.nextDouble(random, Math.PI); expectedRotation.appendYawRotation(yaw); expected.set(expectedRotation, original.getTranslationVector()); double pitch = EuclidCoreRandomTools.nextDouble(random, Math.PI); expectedRotation.appendPitchRotation(pitch); expected.set(expectedRotation, original.getTranslationVector()); double roll = EuclidCoreRandomTools.nextDouble(random, Math.PI); expectedRotation.appendRollRotation(roll); expected.set(expectedRotation, original.getTranslationVector());
Quaternion interpolatedRotation = new Quaternion(); interpolatedVector.interpolate(q0.getTranslationVector(), qf.getTranslationVector(), alpha); interpolatedRotation.interpolate(q0.getQuaternion(), qf.getQuaternion(), alpha);
actualTransform.set(qx, qy, qz, qs, x, y, z); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); actualTransform.setUnsafe(qx, qy, qz, qs, x, y, z); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); actualTransform.set(new RotationMatrix(quaternion), translation); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); actualTransform.set(new AxisAngle(quaternion), translation); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); actualTransform.set(quaternion, translation); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS);
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); actualTransform.setRotation(rotationVector); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, actualTransform.getTranslationVector(), EPS);
quaternion.preMultiply(qbtA.getQuaternion()); qbtB = new QuaternionBasedTransform(quaternion, qbtA.getTranslationVector()); quaternion.preMultiply(qbtA.getQuaternion()); qbtB = new QuaternionBasedTransform(quaternion, qbtA.getTranslationVector()); qbtA = createRandomTransform(random); Vector3D translation = new Vector3D(qbtA.getTranslationVector()); Vector3D perturb = new Vector3D(translation); quaternion.preMultiply(qbtA.getQuaternion()); qbtB = new QuaternionBasedTransform(quaternion, qbtA.getTranslationVector());
QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random); QuaternionReadOnly expectedQuaternion = transform.getQuaternion(); Vector3DReadOnly expectedTranslation = transform.getTranslationVector();