public void transform(Quat4d q, Quat4d qToTransform, Quat4d resultToPack) { resultToPack.mul(q, qToTransform); resultToPack.mulInverse(q); }
public static void assertQuaternionsEqualUsingDifference(Quat4d q1, Quat4d q2, double epsilon) { try { Quat4d qDifference = new Quat4d(); qDifference.mulInverse(q1, q2); AxisAngle4d axisAngle = new AxisAngle4d(); axisAngle.set(qDifference); assertEquals(0.0, axisAngle.getAngle(), epsilon); } catch (AssertionError e) { throw new AssertionError("expected:\n<" + q1 + ">\n but was:\n<" + q2 + ">"); } } }
private void computeOrientationError() { orientationEstimator.getEstimatedOrientation(estimatedOrientation); Quat4d estimatedOrientationQuat4d = new Quat4d(); estimatedOrientation.getQuaternion(estimatedOrientationQuat4d); Quat4d actualOrientation = new Quat4d(); estimationJoint.getRotationToWorld(actualOrientation); if (((estimatedOrientationQuat4d.getW() > 0.0) && (actualOrientation.getW() < 0.0)) || ((estimatedOrientationQuat4d.getW() < 0.0) && (actualOrientation.getW() > 0.0))) { actualOrientation.negate(); } perfectOrientation.set(actualOrientation); Quat4d orientationErrorQuat4d = new Quat4d(actualOrientation); orientationErrorQuat4d.mulInverse(estimatedOrientationQuat4d); AxisAngle4d orientationErrorAxisAngle = new AxisAngle4d(); orientationErrorAxisAngle.set(orientationErrorQuat4d); double errorAngle = AngleTools.trimAngleMinusPiToPi(orientationErrorAxisAngle.getAngle()); orientationError.set(Math.abs(errorAngle)); }