/** * Computes the sum of two quaternions. * * @param q1 Quaternion. * @param q2 Quaternion. * @return the sum of {@code q1} and {@code q2}. */ public static Quaternion add(final Quaternion q1, final Quaternion q2) { return new Quaternion(q1.getQ0() + q2.getQ0(), q1.getQ1() + q2.getQ1(), q1.getQ2() + q2.getQ2(), q1.getQ3() + q2.getQ3()); }
/** * Computes the sum of the instance and another quaternion. * * @param q Quaternion. * @return the sum of this instance and {@code q} */ public Quaternion add(final Quaternion q) { return add(this, q); }
/** * Returns the Hamilton product of the instance by a quaternion. * * @param q Quaternion. * @return the product of this instance with {@code q}, in that order. */ public Quaternion multiply(final Quaternion q) { return multiply(this, q); }
/** * Computes the normalized quaternion (the versor of the instance). * The norm of the quaternion must not be zero. * * @return a normalized quaternion. * @throws ZeroException if the norm of the quaternion is zero. */ public Quaternion normalize() { final double norm = getNorm(); if (norm < Precision.SAFE_MIN) { throw new ZeroException(LocalizedFormats.NORM, norm); } return new Quaternion(q0 / norm, q1 / norm, q2 / norm, q3 / norm); }
Quaternion scaledRotationVectorAccelerationMagnetic = rotationVectorAccelerationMagnetic.multiply (oneMinusAlpha); Quaternion scaledRotationVectorGyroscope = rotationVectorGyroscope.multiply(alpha); rotationVectorGyroscope = scaledRotationVectorGyroscope.add (scaledRotationVectorAccelerationMagnetic); Rotation rotation = new Rotation(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(), rotationVectorGyroscope.getQ3(), true);
vectorGyroscope[0] = (float) rotationVectorGyroscope.getVectorPart()[0]; vectorGyroscope[1] = (float) rotationVectorGyroscope.getVectorPart()[1]; vectorGyroscope[2] = (float) rotationVectorGyroscope.getVectorPart()[2]; vectorGyroscope[3] = (float) rotationVectorGyroscope.getScalarPart(); vectorAccelerationMagnetic[0] = (float) rotationOrientation.getVectorPart()[0]; vectorAccelerationMagnetic[1] = (float) rotationOrientation.getVectorPart()[1]; vectorAccelerationMagnetic[2] = (float) rotationOrientation.getVectorPart()[2]; vectorAccelerationMagnetic[3] = (float) rotationOrientation.getScalarPart(); rotationVectorGyroscope = new Quaternion(kalmanFilter.getStateEstimation()[3], Arrays.copyOfRange(kalmanFilter.getStateEstimation(), 0, 3)); Rotation rotation = new Rotation(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(), rotationVectorGyroscope.getQ3(), true);
/** * Multiplies the instance by a scalar. * * @param alpha Scalar factor. * @return a scaled quaternion. */ public Quaternion multiply(final double alpha) { return new Quaternion(alpha * q0, alpha * q1, alpha * q2, alpha * q3); }
public static Quaternion createQuaternion(int[] quat, double scale) { // This article suggests QUAT_W is [0] // https://github.com/vmayoral/bb_mpu9150/blob/master/src/linux-mpu9150/mpu9150/mpu9150.c Quaternion quaterion = new Quaternion(quat[MPU9150_QUAT_W]*scale, quat[MPU9150_QUAT_X]*scale, quat[MPU9150_QUAT_Y]*scale, quat[MPU9150_QUAT_Z]*scale); return quaterion.normalize(); }
return previousRotationVector.multiply(new Quaternion(deltaVector[3], Arrays.copyOfRange( deltaVector, 0, 3)));
/** * Gets the scalar part of the quaternion. * * @return the scalar part. * @see #getQ0() */ public double getScalarPart() { return getQ0(); }
/** * Checks whether the instance is a unit quaternion within a given * tolerance. * * @param eps Tolerance (absolute error). * @return {@code true} if the norm is 1 within the given tolerance, * {@code false} otherwise */ public boolean isUnitQuaternion(double eps) { return Precision.equals(getNorm(), 1d, eps); }
/** * Computes the dot-product of the instance by a quaternion. * * @param q Quaternion. * @return the dot product of this instance and {@code q}. */ public double dotProduct(final Quaternion q) { return dotProduct(this, q); }
/** * Returns the conjugate quaternion of the instance. * * @return the conjugate quaternion */ public Quaternion getConjugate() { return new Quaternion(q0, -q1, -q2, -q3); }
/** * Computes the normalized quaternion (the versor of the instance). * The norm of the quaternion must not be zero. * * @return a normalized quaternion. * @throws ZeroException if the norm of the quaternion is zero. */ public Quaternion normalize() { final double norm = getNorm(); if (norm < Precision.SAFE_MIN) { throw new ZeroException(LocalizedFormats.NORM, norm); } return new Quaternion(q0 / norm, q1 / norm, q2 / norm, q3 / norm); }
/** * Checks whether the instance is a pure quaternion within a given * tolerance. * * @param eps Tolerance (absolute error). * @return {@code true} if the scalar part of the quaternion is zero. */ public boolean isPureQuaternion(double eps) { return FastMath.abs(getQ0()) <= eps; }
/** * Checks whether the instance is a unit quaternion within a given * tolerance. * * @param eps Tolerance (absolute error). * @return {@code true} if the norm is 1 within the given tolerance, * {@code false} otherwise */ public boolean isUnitQuaternion(double eps) { return Precision.equals(getNorm(), 1d, eps); }
/** * Computes the dot-product of the instance by a quaternion. * * @param q Quaternion. * @return the dot product of this instance and {@code q}. */ public double dotProduct(final Quaternion q) { return dotProduct(this, q); }
/** * Subtracts two quaternions. * * @param q1 First Quaternion. * @param q2 Second quaternion. * @return the difference between {@code q1} and {@code q2}. */ public static Quaternion subtract(final Quaternion q1, final Quaternion q2) { return new Quaternion(q1.getQ0() - q2.getQ0(), q1.getQ1() - q2.getQ1(), q1.getQ2() - q2.getQ2(), q1.getQ3() - q2.getQ3()); }