/** * 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); }
/** * Returns the conjugate quaternion of the instance. * * @return the conjugate quaternion */ public Quaternion getConjugate() { return new Quaternion(q0, -q1, -q2, -q3); }
/** * Returns the inverse of this instance. * The norm of the quaternion must not be zero. * * @return the inverse. * @throws ZeroException if the norm (squared) of the quaternion is zero. */ public Quaternion getInverse() { final double squareNorm = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; if (squareNorm < Precision.SAFE_MIN) { throw new ZeroException(LocalizedFormats.NORM, squareNorm); } return new Quaternion(q0 / squareNorm, -q1 / squareNorm, -q2 / squareNorm, -q3 / squareNorm); }
/** * 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); }
/** * 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()); }
/** * Returns the polar form of the quaternion. * * @return the unit quaternion with positive scalar part. */ public Quaternion getPositivePolarForm() { if (getQ0() < 0) { final Quaternion unitQ = normalize(); // The quaternion of rotation (normalized quaternion) q and -q // are equivalent (i.e. represent the same rotation). return new Quaternion(-unitQ.getQ0(), -unitQ.getQ1(), -unitQ.getQ2(), -unitQ.getQ3()); } else { return this.normalize(); } }
/** * 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()); }
/** * Returns the Hamilton product of two quaternions. * * @param q1 First quaternion. * @param q2 Second quaternion. * @return the product {@code q1} and {@code q2}, in that order. */ public static Quaternion multiply(final Quaternion q1, final Quaternion q2) { // Components of the first quaternion. final double q1a = q1.getQ0(); final double q1b = q1.getQ1(); final double q1c = q1.getQ2(); final double q1d = q1.getQ3(); // Components of the second quaternion. final double q2a = q2.getQ0(); final double q2b = q2.getQ1(); final double q2c = q2.getQ2(); final double q2d = q2.getQ3(); // Components of the product. final double w = q1a * q2a - q1b * q2b - q1c * q2c - q1d * q2d; final double x = q1a * q2b + q1b * q2a + q1c * q2d - q1d * q2c; final double y = q1a * q2c - q1b * q2d + q1c * q2a + q1d * q2b; final double z = q1a * q2d + q1b * q2c - q1c * q2b + q1d * q2a; return new Quaternion(w, x, y, z); }
/** * Returns the conjugate quaternion of the instance. * * @return the conjugate quaternion */ public Quaternion getConjugate() { return new Quaternion(q0, -q1, -q2, -q3); }
/** * 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); }
/** * Returns the conjugate quaternion of the instance. * * @return the conjugate quaternion */ public Quaternion getConjugate() { return new Quaternion(q0, -q1, -q2, -q3); }
/** * 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(); }
/** * Returns the inverse of this instance. * The norm of the quaternion must not be zero. * * @return the inverse. * @throws ZeroException if the norm (squared) of the quaternion is zero. */ public Quaternion getInverse() { final double squareNorm = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; if (squareNorm < Precision.SAFE_MIN) { throw new ZeroException(LocalizedFormats.NORM, squareNorm); } return new Quaternion(q0 / squareNorm, -q1 / squareNorm, -q2 / squareNorm, -q3 / squareNorm); }
/** * Returns the inverse of this instance. * The norm of the quaternion must not be zero. * * @return the inverse. * @throws ZeroException if the norm (squared) of the quaternion is zero. */ public Quaternion getInverse() { final double squareNorm = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; if (squareNorm < Precision.SAFE_MIN) { throw new ZeroException(LocalizedFormats.NORM, squareNorm); } return new Quaternion(q0 / squareNorm, -q1 / squareNorm, -q2 / squareNorm, -q3 / squareNorm); }
/** * 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); }
/** * 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 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()); }
/** * 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()); }
/** * 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()); }