/** {@inheritDoc} */ @Override public final void setUnsafe(double qx, double qy, double qz, double qs) { quaternion.setUnsafe(qx, qy, qz, qs); }
@Override public void setUnsafe(double qx, double qy, double qz, double qs) { pose.getOrientation().setUnsafe(qx, qy, qz, qs); }
@Override public Quaternion createTuple(double x, double y, double z, double s) { Quaternion quaternion = new Quaternion(); quaternion.setUnsafe(x, y, z, s); return quaternion; }
@Override public FrameQuaternion createFrameTuple(ReferenceFrame referenceFrame, double x, double y, double z, double s) { Quaternion quaternion = new Quaternion(); quaternion.setUnsafe(x, y, z, s); return new FrameQuaternion(referenceFrame, quaternion); }
@Override protected void setZ(Quaternion data, double z) { data.setUnsafe(data.getX(), data.getY(), z, data.getS()); }
@Override protected void setX(Quaternion data, double x) { data.setUnsafe(x, data.getY(), data.getZ(), data.getS()); }
@Override protected void setW(Quaternion data, double w) { data.setUnsafe(data.getX(), data.getY(), data.getZ(), w); }
@Override protected void setY(Quaternion data, double y) { data.setUnsafe(data.getX(), y, data.getZ(), data.getS()); }
@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()); }
expectedQuaternion.setUnsafe(0.0, 0.0, 0.0, 0.0); RotationMatrixConversion.convertQuaternionToMatrix(expectedQuaternion, actualMatrix); EuclidCoreTestTools.assertIdentity(actualMatrix, EPSILON); expectedQuaternion.setUnsafe(Double.NaN, 0.0, 0.0, 0.0); RotationMatrixConversion.convertQuaternionToMatrix(expectedQuaternion, actualMatrix); EuclidCoreTestTools.assertMatrix3DContainsOnlyNaN(actualMatrix); expectedQuaternion.setUnsafe(0.0, Double.NaN, 0.0, 0.0); RotationMatrixConversion.convertQuaternionToMatrix(expectedQuaternion, actualMatrix); EuclidCoreTestTools.assertMatrix3DContainsOnlyNaN(actualMatrix); expectedQuaternion.setUnsafe(0.0, 0.0, Double.NaN, 0.0); RotationMatrixConversion.convertQuaternionToMatrix(expectedQuaternion, actualMatrix); EuclidCoreTestTools.assertMatrix3DContainsOnlyNaN(actualMatrix); expectedQuaternion.setUnsafe(0.0, 0.0, 0.0, Double.NaN); RotationMatrixConversion.convertQuaternionToMatrix(expectedQuaternion, actualMatrix); EuclidCoreTestTools.assertMatrix3DContainsOnlyNaN(actualMatrix); expectedQuaternion.setUnsafe(scale * sin(angle / 2.0), 0.0, 0.0, scale * cos(angle / 2.0)); RotationMatrixConversion.convertQuaternionToMatrix(expectedQuaternion, actualMatrix); EuclidCoreTestTools.assertMatrix3DEquals(expectedMatrix, actualMatrix, EPSILON); expectedQuaternion.setUnsafe(0.0, scale * sin(angle / 2.0), 0.0, scale * cos(angle / 2.0)); RotationMatrixConversion.convertQuaternionToMatrix(expectedQuaternion, actualMatrix); EuclidCoreTestTools.assertMatrix3DEquals(expectedMatrix, actualMatrix, EPSILON); expectedQuaternion.setUnsafe(0.0, 0.0, scale * sin(angle / 2.0), scale * cos(angle / 2.0));
break; q.setUnsafe(qx, qy, qz, qs); newHashCode = q.hashCode(); assertNotEquals(newHashCode, previousHashCode);
@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())); }
quaternion.setUnsafe(qx, qy, qz, qs); RotationVectorConversion.convertQuaternionToRotationVector(quaternion, actualRotationVector); EuclidCoreTestTools.assertTuple3DEquals(expectedRotationVector, actualRotationVector, EPSILON); quaternion.setUnsafe(qx, qy, qz, qs); RotationVectorConversion.convertQuaternionToRotationVector(quaternion, actualRotationVector); EuclidCoreTestTools.assertTuple3DEquals(expectedRotationVector, actualRotationVector, EPSILON); quaternion.setUnsafe(0.0, 0.0, 0.0, 0.0); RotationVectorConversion.convertQuaternionToRotationVector(quaternion, actualRotationVector); EuclidCoreTestTools.assertTuple3DIsSetToZero(actualRotationVector); quaternion.setUnsafe(Double.NaN, 0.0, 0.0, 0.0); RotationVectorConversion.convertQuaternionToRotationVector(quaternion, actualRotationVector); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(actualRotationVector); quaternion.setUnsafe(0.0, Double.NaN, 0.0, 0.0); RotationVectorConversion.convertQuaternionToRotationVector(quaternion, actualRotationVector); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(actualRotationVector); quaternion.setUnsafe(0.0, 0.0, Double.NaN, 0.0); RotationVectorConversion.convertQuaternionToRotationVector(quaternion, actualRotationVector); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(actualRotationVector); quaternion.setUnsafe(0.0, 0.0, 0.0, Double.NaN); RotationVectorConversion.convertQuaternionToRotationVector(quaternion, actualRotationVector); EuclidCoreTestTools.assertTuple3DContainsOnlyNaN(actualRotationVector);
double qz = corrupt * quaternion.getZ(); double qs = corrupt * quaternion.getS(); quaternion.setUnsafe(qx, qy, qz, qs); tupleActual.set(tupleExpected); Quaternion quaternion = new Quaternion(); quaternion.setUnsafe(0.0, 0.0, 0.0, 0.0); QuaternionTools.subTransform(quaternion, tupleExpected, tupleActual); EuclidCoreTestTools.assertTuple3DEquals(tupleExpected, tupleActual, EPSILON);
double qz = corrupt * quaternion.getZ(); double qs = corrupt * quaternion.getS(); quaternion.setUnsafe(qx, qy, qz, qs); tupleActual.set(tupleExpected); Quaternion quaternion = new Quaternion(); quaternion.setUnsafe(0.0, 0.0, 0.0, 0.0); QuaternionTools.addTransform(quaternion, tupleExpected, tupleActual); EuclidCoreTestTools.assertTuple3DEquals(tupleExpected, tupleActual, EPSILON);
double qz = corrupt * quaternion.getZ(); double qs = corrupt * quaternion.getS(); quaternion.setUnsafe(qx, qy, qz, qs); quaternion.setUnsafe(0.0, 0.0, 0.0, 0.0); QuaternionTools.transform(quaternion, vectorExpected, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPSILON);
double qz = corrupt * quaternion.getZ(); double qs = corrupt * quaternion.getS(); quaternion.setUnsafe(qx, qy, qz, qs);
double qz = corrupt * quaternion.getZ(); double qs = corrupt * quaternion.getS(); quaternion.setUnsafe(qx, qy, qz, qs); quaternion.setUnsafe(0.0, 0.0, 0.0, 0.0); QuaternionTools.transform(quaternion, matrixExpected, matrixActual); EuclidCoreTestTools.assertMatrix3DEquals(matrixExpected, matrixActual, EPSILON);
double qy = uy * Math.sin(angle / 2.0); double qz = uz * Math.sin(angle / 2.0); expectedQuaternion.setUnsafe(qx, qy, qz, qs);
double scale = EuclidCoreRandomTools.nextDouble(random, 0.1, 10.0); qExpected.setUnsafe(qExpected.getX() * scale, qExpected.getY() * scale, qExpected.getZ() * scale, qExpected.getS() * scale); qActual.set(qExpected); qExpected.setUnsafe(qExpected.getX() * scale, qExpected.getY() * scale, qExpected.getZ() * scale, qExpected.getS() * scale); qActual.set(qExpected);