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); } }
if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion);
if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion);
if (quaternionFiltered.dot(quaternionUnfiltered) > 0.0)
if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion);
if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion);
assertTrue(qExpected.dot(qActual) < 0.0); assertTrue(qActual.getS() > 0.0); AxisAngleConversion.convertQuaternionToAxisAngle(qActual, axisAngle);