private void computeAngularVelocity(Vector3D angularVelocityToPack, Quaternion startRotationQuaternion, Quaternion endRotationQuaternion, double alphaDot) { if (startRotationQuaternion.dot(endRotationQuaternion) < 0.0) endRotationQuaternion.negate(); // compute relative orientation: orientation of interpolated frame w.r.t. start frame relativeRotationQuaternion.set(startRotationQuaternion); // R_W_S: orientation of start w.r.t. world relativeRotationQuaternion.conjugate(); // R_S_W: orientation of world w.r.t. start relativeRotationQuaternion.multiply(endRotationQuaternion); // R_S_I = R_S_W * R_W_I: orientation of interpolated w.r.t. start quaternionCalculus.log(relativeRotationQuaternion, angularVelocityToPack); angularVelocityToPack.scale(alphaDot); endRotationQuaternion.transform(angularVelocityToPack); } }
@Test public void testTransformationsWithQuaternionReadOnly() throws Exception { Random random = new Random(2342L); for (int i = 0; i < ITERATIONS; i++) { QuaternionReadOnly quaternion = EuclidCoreRandomTools.nextQuaternion(random); Tuple3DReadOnly tupleOriginal = EuclidCoreRandomTools.nextPoint3D(random, 10.0); Tuple3DBasics tupleExpected = new Point3D(); quaternion.transform(tupleOriginal, tupleExpected); Tuple3DBasics tupleActual = new Point3D(); tupleActual.setX(TransformationTools.computeTransformedX(quaternion, false, tupleOriginal)); tupleActual.setY(TransformationTools.computeTransformedY(quaternion, false, tupleOriginal)); tupleActual.setZ(TransformationTools.computeTransformedZ(quaternion, false, tupleOriginal)); EuclidCoreTestTools.assertTuple3DEquals(tupleExpected, tupleActual, EPSILON); Quaternion conjugatedQuaternion = new Quaternion(quaternion); conjugatedQuaternion.conjugate(); conjugatedQuaternion.transform(tupleOriginal, tupleExpected); tupleActual.setX(TransformationTools.computeTransformedX(quaternion, true, tupleOriginal)); tupleActual.setY(TransformationTools.computeTransformedY(quaternion, true, tupleOriginal)); tupleActual.setZ(TransformationTools.computeTransformedZ(quaternion, true, tupleOriginal)); EuclidCoreTestTools.assertTuple3DEquals(tupleExpected, tupleActual, EPSILON); } }
assertTrue(qNeutral.equals(qNeutralCopy)); qExpected.conjugate(); QuaternionTools.multiplyConjugateLeft(q, qNeutral, qActual); EuclidCoreTestTools.assertQuaternionEquals(qExpected, qActual, EPSILON); qInv.conjugate(); QuaternionTools.multiplyConjugateLeft(q, qInv, qActual); qExpected.conjugate(); EuclidCoreTestTools.assertQuaternionEquals(qExpected, qActual, EPSILON); EuclidCoreTestTools.assertQuaternionEquals(qExpected, qActual, EPSILON); qExpected.conjugate(); QuaternionTools.multiplyConjugateLeft(q2, q1, qActual); EuclidCoreTestTools.assertQuaternionEquals(qExpected, qActual, EPSILON);
assertTrue(qNeutral.equals(qNeutralCopy)); qExpected.conjugate(); QuaternionTools.multiplyConjugateRight(qNeutral, q, qActual); EuclidCoreTestTools.assertQuaternionEquals(qExpected, qActual, EPSILON); qInv.conjugate(); qExpected.conjugate(); EuclidCoreTestTools.assertQuaternionEquals(qExpected, qActual, EPSILON); EuclidCoreTestTools.assertQuaternionEquals(qExpected, qActual, EPSILON); qExpected.conjugate(); QuaternionTools.multiplyConjugateRight(q2, q1, qActual); EuclidCoreTestTools.assertQuaternionEquals(qExpected, qActual, EPSILON);
qInv.conjugate(); qExpected.setToZero(); QuaternionTools.multiply(q, qInv, qActual);