private static double getAngleDifference(Quaternion q1, Quaternion q2) { Quaternion qDifference = new Quaternion(q1); qDifference.multiplyConjugateOther(q2); AxisAngle axisAngle = new AxisAngle(); axisAngle.set(qDifference); return axisAngle.getAngle(); } }
private boolean isOrientationEqual(QuaternionReadOnly initialQuat, QuaternionReadOnly finalQuat, double angleEpsilon) { Quaternion quatDifference = new Quaternion(initialQuat); quatDifference.multiplyConjugateOther(finalQuat); AxisAngle angleDifference = new AxisAngle(); angleDifference.set(quatDifference); AngleTools.trimAngleMinusPiToPi(angleDifference.getAngle()); return Math.abs(angleDifference.getAngle()) < angleEpsilon; }
@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); } }
quaternionError.multiplyConjugateOther(quaternionTwo); quaternionShouldBeOne.multiplyConjugateOther(quaternionErrorCheck);
if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta); double velocityFD = delta.length() / dt;
if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta); double velocityFD = delta.length() / dt;
@Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) { RigidBodyTransform transformToRoot = originalFrame.getTransformToRoot(); if (previousRotation.containsNaN() || previousTranslation.containsNaN()) { transformToRoot.get(previousRotation, previousTranslation); angularVelocity.setToZero(); linearVelocity.setToZero(); } else { transformToRoot.get(rotation, translation); rotation.multiplyConjugateOther(previousRotation); rotation.getRotationVector(angularVelocity); angularVelocity.scale(1.0 / updateDT); linearVelocity.sub(translation, previousTranslation); linearVelocity.scale(1.0 / updateDT); transformToRoot.get(previousRotation, previousTranslation); transformToRoot.inverseTransform(angularVelocity); transformToRoot.inverseTransform(linearVelocity); } twistRelativeToParentToPack.setIncludingFrame(this, getParent(), this, angularVelocity, linearVelocity); } }
qExpected.set(q1); if (invert2) qExpected.multiplyConjugateOther(q2); else qExpected.multiply(q2);
if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta); double velocityFD = delta.length() / dt;
if (currentQuaternion.dot(previousQuaternion) < 0.0) previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta); double velocityFD = delta.length() / dt;
expected.multiplyConjugateOther(appended); actualOrientation.appendInvertOther(appended);