@Override protected Quaternion getRotation(QuaternionBasedTransform data) { return (Quaternion) data.getQuaternion(); }
/** * 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()); }
@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 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()); }
/** * 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 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 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()); }
for (int row = 0; row < 3; row++) EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); double z = translation.getZ(); actualTransform.setTranslation(x, y, z); EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); EuclidCoreTestTools.assertQuaternionEquals(expectedQuaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS);
EuclidCoreTestTools.assertQuaternionIsSetToZero(transform.getQuaternion()); EuclidCoreTestTools.assertTuple3DIsSetToZero(transform.getTranslationVector()); translation.get(4, denseMatrix); QuaternionBasedTransform transform = new QuaternionBasedTransform(denseMatrix); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); translation.get(4, array); QuaternionBasedTransform transform = new QuaternionBasedTransform(array); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); Vector3D translation = EuclidCoreRandomTools.nextVector3D(random); QuaternionBasedTransform transform = new QuaternionBasedTransform(new RotationMatrix(quaternion), translation); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); Vector3D translation = EuclidCoreRandomTools.nextVector3D(random); QuaternionBasedTransform transform = new QuaternionBasedTransform(quaternion, translation); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); Vector3D translation = EuclidCoreRandomTools.nextVector3D(random); QuaternionBasedTransform transform = new QuaternionBasedTransform(new AxisAngle(quaternion), translation); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS);
RotationMatrix expectedRotation = new RotationMatrix(original.getQuaternion()); double yaw = EuclidCoreRandomTools.nextDouble(random, Math.PI); expectedRotation.appendYawRotation(yaw); RotationMatrix expectedRotation = new RotationMatrix(original.getQuaternion()); double pitch = EuclidCoreRandomTools.nextDouble(random, Math.PI); expectedRotation.appendPitchRotation(pitch); RotationMatrix expectedRotation = new RotationMatrix(original.getQuaternion()); double roll = EuclidCoreRandomTools.nextDouble(random, Math.PI); expectedRotation.appendRollRotation(roll);
interpolatedRotation.interpolate(q0.getQuaternion(), qf.getQuaternion(), alpha);
quaternion.preMultiply(qbtA.getQuaternion()); quaternion.preMultiply(qbtA.getQuaternion()); qbtB = new QuaternionBasedTransform(new Quaternion(qbtA.getQuaternion()), perturb); qbtB = new QuaternionBasedTransform(new Quaternion(qbtA.getQuaternion()), perturb); qbtB = new QuaternionBasedTransform(new Quaternion(qbtA.getQuaternion()), perturb); qbtB = new QuaternionBasedTransform(new Quaternion(qbtA.getQuaternion()), perturb); qbtB = new QuaternionBasedTransform(new Quaternion(qbtA.getQuaternion()), perturb); qbtB = new QuaternionBasedTransform(new Quaternion(qbtA.getQuaternion()), perturb); quaternion.preMultiply(qbtA.getQuaternion());
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 z = translation.getZ(); actualTransform.setUnsafe(qx, qy, qz, qs, x, y, z); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); Vector3D translation = EuclidCoreRandomTools.nextVector3D(random); actualTransform.set(new RotationMatrix(quaternion), translation); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); Vector3D translation = EuclidCoreRandomTools.nextVector3D(random); actualTransform.set(new AxisAngle(quaternion), translation); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(translation, actualTransform.getTranslationVector(), EPS); Vector3D translation = EuclidCoreRandomTools.nextVector3D(random); actualTransform.set(quaternion, translation); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS);
EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, actualTransform.getTranslationVector(), EPS); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, actualTransform.getTranslationVector(), EPS); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, actualTransform.getTranslationVector(), EPS); quaternion.getRotationVector(rotationVector); actualTransform.setRotation(rotationVector); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, actualTransform.getTranslationVector(), EPS);
QuaternionReadOnly expectedQuaternion = transform.getQuaternion(); Quaternion actualQuaternion = new Quaternion();
QuaternionReadOnly expectedQuaternion = transform.getQuaternion(); Vector3DReadOnly expectedTranslation = transform.getTranslationVector();
expected.preMultiply(transform.getQuaternion()); actual.set(original); actual.applyTransform(transform);
expected.prepend(transform.getQuaternion()); actual.set(original); actual.applyTransform(transform);