public void interpolate(double alpha, QuaternionReadOnly q0, QuaternionReadOnly q1, QuaternionBasics qInterpolatedToPack, boolean preventExtraSpin) { tempQ1ForInterpolation.set(q1); if (preventExtraSpin && q0.dot(tempQ1ForInterpolation) < 0.0) { tempQ1ForInterpolation.negate(); } computeQuaternionDifference(q0, tempQ1ForInterpolation, qInterpolatedToPack); pow(qInterpolatedToPack, alpha, qInterpolatedToPack); qInterpolatedToPack.multiply(q0, qInterpolatedToPack); }
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); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testSetQuaternionBasedOnMatrix3d() { Random random = new Random(1776L); Quaternion unitQuaternion = new Quaternion(0.0, 0.0, 0.0, 1.0); int numberOfTests = 100000; for (int i = 0; i < numberOfTests; i++) { Quaternion randomQuaternion = RandomGeometry.nextQuaternion(random); RotationMatrix rotationMatrix = new RotationMatrix(); rotationMatrix.set(randomQuaternion); Quaternion quaternionToPack = new Quaternion(rotationMatrix); quaternionToPack.multiplyConjugateOther(randomQuaternion); if (quaternionToPack.getS() < 0.0) quaternionToPack.negate(); boolean quaternionsAreEpsilonEquals = unitQuaternion.epsilonEquals(quaternionToPack, 1e-7); assertTrue(quaternionsAreEpsilonEquals); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAgainstInterpolation() throws Exception { for (int nTest = 0; nTest < 10; nTest++) { double epsilon = 1.0e-15; Random random = new Random(56416456L); Quaternion quat1 = RandomGeometry.nextQuaternion(random); Quaternion quat2 = RandomGeometry.nextQuaternion(random); Quaternion expectedAverageQuat = new Quaternion(); expectedAverageQuat.interpolate(quat1, quat2, 0.5); AverageQuaternionCalculator averageQuaternionCalculator = new AverageQuaternionCalculator(); averageQuaternionCalculator.queueQuaternion(quat1); averageQuaternionCalculator.queueQuaternion(quat2); averageQuaternionCalculator.compute(); Quaternion actualAverageQuat = new Quaternion(); averageQuaternionCalculator.getAverageQuaternion(actualAverageQuat); if (expectedAverageQuat.getS() * actualAverageQuat.getS() < 0.0) expectedAverageQuat.negate(); double[] expecteds = new double[4]; expectedAverageQuat.get(expecteds); double[] actuals = new double[4]; actualAverageQuat.get(actuals); assertArrayEquals(expecteds, actuals, epsilon); } }
previousQuaternion.set(previousOrientation); if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta);
previousQuaternion.set(previousOrientation); if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta);
expectedAverageQuat.negate();
previousQuaternion.set(previousOrientation); if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta);
previousQuaternion.set(previousOrientation); if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta);
qfCopy.negate(); qDiff.difference(q0, qfCopy);