Quaternion.java

/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package org.apache.commons.numbers.quaternion;

import java.util.Arrays;
import java.util.function.ToDoubleFunction;
import java.util.function.BiPredicate;
import java.io.Serializable;
import org.apache.commons.numbers.core.Precision;

/**
 * This class implements <a href="http://mathworld.wolfram.com/Quaternion.html">
 * quaternions</a> (Hamilton's hypercomplex numbers).
 *
 * <p>Wherever quaternion components are listed in sequence, this class follows the
 * convention of placing the scalar ({@code w}) component first, e.g. [{@code w, x, y, z}].
 * Other libraries and textbooks may place the {@code w} component last.</p>
 *
 * <p>Instances of this class are guaranteed to be immutable.</p>
 */
public final class Quaternion implements Serializable {
    /** Zero quaternion. */
    public static final Quaternion ZERO = of(0, 0, 0, 0);
    /** Identity quaternion. */
    public static final Quaternion ONE = new Quaternion(Type.POSITIVE_POLAR_FORM, 1, 0, 0, 0);
    /** i. */
    public static final Quaternion I = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 1, 0, 0);
    /** j. */
    public static final Quaternion J = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 0, 1, 0);
    /** k. */
    public static final Quaternion K = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 0, 0, 1);

    /** Serializable version identifier. */
    private static final long serialVersionUID = 20170118L;
    /** Error message. */
    private static final String ILLEGAL_NORM_MSG = "Illegal norm: ";

    /** {@link #toString() String representation}. */
    private static final String FORMAT_START = "[";
    /** {@link #toString() String representation}. */
    private static final String FORMAT_END = "]";
    /** {@link #toString() String representation}. */
    private static final String FORMAT_SEP = " ";

    /** The number of dimensions for the vector part of the quaternion. */
    private static final int VECTOR_DIMENSIONS = 3;
    /** The number of parts when parsing a text representation of the quaternion. */
    private static final int NUMBER_OF_PARTS = 4;

    /** For enabling specialized method implementations. */
    private final Type type;
    /** First component (scalar part). */
    private final double w;
    /** Second component (first vector part). */
    private final double x;
    /** Third component (second vector part). */
    private final double y;
    /** Fourth component (third vector part). */
    private final double z;

    /**
     * For enabling optimized implementations.
     */
    private enum Type {
        /** Default implementation. */
        DEFAULT(Default.NORMSQ,
                Default.NORM,
                Default.IS_UNIT),
        /** Quaternion has unit norm. */
        NORMALIZED(Normalized.NORM,
                   Normalized.NORM,
                   Normalized.IS_UNIT),
        /** Quaternion has positive scalar part. */
        POSITIVE_POLAR_FORM(Normalized.NORM,
                            Normalized.NORM,
                            Normalized.IS_UNIT);

        /** {@link Quaternion#normSq()}. */
        private final ToDoubleFunction<Quaternion> normSq;
        /** {@link Quaternion#norm()}. */
        private final ToDoubleFunction<Quaternion> norm;
        /** {@link Quaternion#isUnit(double)}. */
        private final BiPredicate<Quaternion, Double> testIsUnit;

        /** Default implementations. */
        private static final class Default {
            /** {@link Quaternion#normSq()}. */
            static final ToDoubleFunction<Quaternion> NORMSQ = q ->
                q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;

            /** {@link Quaternion#norm()}. */
            private static final ToDoubleFunction<Quaternion> NORM = q ->
                Math.sqrt(NORMSQ.applyAsDouble(q));

            /** {@link Quaternion#isUnit(double)}. */
            private static final BiPredicate<Quaternion, Double> IS_UNIT = (q, eps) ->
                Precision.equals(NORM.applyAsDouble(q), 1d, eps);
        }

        /** Implementations for normalized quaternions. */
        private static final class Normalized {
            /** {@link Quaternion#norm()} returns 1. */
            static final ToDoubleFunction<Quaternion> NORM = q -> 1;
            /** {@link Quaternion#isUnit(double)} returns 1. */
            static final BiPredicate<Quaternion, Double> IS_UNIT = (q, eps) -> true;
        }

        /**
         * @param normSq {@code normSq} method.
         * @param norm {@code norm} method.
         * @param isUnit {@code isUnit} method.
         */
        Type(ToDoubleFunction<Quaternion> normSq,
             ToDoubleFunction<Quaternion> norm,
             BiPredicate<Quaternion, Double> isUnit)  {
            this.normSq = normSq;
            this.norm = norm;
            this.testIsUnit = isUnit;
        }

        /**
         * @param q Quaternion.
         * @return the norm squared.
         */
        double normSq(Quaternion q) {
            return normSq.applyAsDouble(q);
        }
        /**
         * @param q Quaternion.
         * @return the norm.
         */
        double norm(Quaternion q) {
            return norm.applyAsDouble(q);
        }
        /**
         * @param q Quaternion.
         * @param eps Tolerance.
         * @return whether {@code q} has unit norm within the allowed tolerance.
         */
        boolean isUnit(Quaternion q,
                       double eps) {
            return testIsUnit.test(q, eps);
        }
    }

    /**
     * Builds a quaternion from its components.
     *
     * @param type Quaternion type.
     * @param w Scalar component.
     * @param x First vector component.
     * @param y Second vector component.
     * @param z Third vector component.
     */
    private Quaternion(Type type,
                       final double w,
                       final double x,
                       final double y,
                       final double z) {
        this.type = type;
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;
    }

    /**
     * Copies the given quaternion, but change its {@link Type}.
     *
     * @param type Quaternion type.
     * @param q Quaternion whose components will be copied.
     */
    private Quaternion(Type type,
                       Quaternion q) {
        this.type = type;
        w = q.w;
        x = q.x;
        y = q.y;
        z = q.z;
    }

    /**
     * Builds a quaternion from its components.
     *
     * @param w Scalar component.
     * @param x First vector component.
     * @param y Second vector component.
     * @param z Third vector component.
     * @return a quaternion instance.
     */
    public static Quaternion of(final double w,
                                final double x,
                                final double y,
                                final double z) {
        return new Quaternion(Type.DEFAULT,
                              w, x, y, z);
    }

    /**
     * Builds a quaternion from scalar and vector parts.
     *
     * @param scalar Scalar part of the quaternion.
     * @param v Components of the vector part of the quaternion.
     * @return a quaternion instance.
     *
     * @throws IllegalArgumentException if the array length is not 3.
     */
    public static Quaternion of(final double scalar,
                                final double[] v) {
        if (v.length != VECTOR_DIMENSIONS) {
            throw new IllegalArgumentException("Size of array must be 3");
        }

        return of(scalar, v[0], v[1], v[2]);
    }

    /**
     * Builds a pure quaternion from a vector (assuming that the scalar
     * part is zero).
     *
     * @param v Components of the vector part of the pure quaternion.
     * @return a quaternion instance.
     */
    public static Quaternion of(final double[] v) {
        return of(0, v);
    }

    /**
     * Returns the conjugate of this quaternion number.
     * The conjugate of {@code a + bi + cj + dk} is {@code a - bi -cj -dk}.
     *
     * @return the conjugate of this quaternion object.
     */
    public Quaternion conjugate() {
        return of(w, -x, -y, -z);
    }

    /**
     * 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.w;
        final double q1b = q1.x;
        final double q1c = q1.y;
        final double q1d = q1.z;

        // Components of the second quaternion.
        final double q2a = q2.w;
        final double q2b = q2.x;
        final double q2c = q2.y;
        final double q2d = q2.z;

        // 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 of(w, x, y, z);
    }

    /**
     * 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 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 of(q1.w + q2.w,
                  q1.x + q2.x,
                  q1.y + q2.y,
                  q1.z + q2.z);
    }

    /**
     * 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);
    }

    /**
     * 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 of(q1.w - q2.w,
                  q1.x - q2.x,
                  q1.y - q2.y,
                  q1.z - q2.z);
    }

    /**
     * Subtracts a quaternion from the instance.
     *
     * @param q Quaternion.
     * @return the difference between this instance and {@code q}.
     */
    public Quaternion subtract(final Quaternion q) {
        return subtract(this, q);
    }

    /**
     * Computes the dot-product of two quaternions.
     *
     * @param q1 Quaternion.
     * @param q2 Quaternion.
     * @return the dot product of {@code q1} and {@code q2}.
     */
    public static double dot(final Quaternion q1,
                             final Quaternion q2) {
        return q1.w * q2.w +
            q1.x * q2.x +
            q1.y * q2.y +
            q1.z * q2.z;
    }

    /**
     * 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 dot(final Quaternion q) {
        return dot(this, q);
    }

    /**
     * Computes the norm of the quaternion.
     *
     * @return the norm.
     */
    public double norm() {
        return type.norm(this);
    }

    /**
     * Computes the square of the norm of the quaternion.
     *
     * @return the square of the norm.
     */
    public double normSq() {
        return type.normSq(this);
    }

    /**
     * Computes the normalized quaternion (the versor of the instance).
     * The norm of the quaternion must not be near zero.
     *
     * @return a normalized quaternion.
     * @throws IllegalStateException if the norm of the quaternion is NaN, infinite,
     *      or near zero.
     */
    public Quaternion normalize() {
        switch (type) {
        case NORMALIZED:
        case POSITIVE_POLAR_FORM:
            return this;
        case DEFAULT:
            final double norm = norm();

            if (norm < Precision.SAFE_MIN ||
                !Double.isFinite(norm)) {
                throw new IllegalStateException(ILLEGAL_NORM_MSG + norm);
            }

            final Quaternion unit = divide(norm);

            return w >= 0 ?
                new Quaternion(Type.POSITIVE_POLAR_FORM, unit) :
                new Quaternion(Type.NORMALIZED, unit);
        default:
            throw new IllegalStateException(); // Should never happen.
        }
    }

    /**
     * {@inheritDoc}
     */
    @Override
    public boolean equals(Object other) {
        if (this == other) {
            return true;
        }
        if (other instanceof Quaternion) {
            final Quaternion q = (Quaternion) other;
            return ((Double) w).equals(q.w) &&
                ((Double) x).equals(q.x) &&
                ((Double) y).equals(q.y) &&
                ((Double) z).equals(q.z);
        }

        return false;
    }

    /**
     * {@inheritDoc}
     */
    @Override
    public int hashCode() {
        return Arrays.hashCode(new double[] {w, x, y, z});
    }

    /**
     * Checks whether this instance is equal to another quaternion
     * within a given tolerance.
     *
     * @param q Quaternion with which to compare the current quaternion.
     * @param eps Tolerance.
     * @return {@code true} if the each of the components are equal
     * within the allowed absolute error.
     */
    public boolean equals(final Quaternion q,
                          final double eps) {
        return Precision.equals(w, q.w, eps) &&
            Precision.equals(x, q.x, eps) &&
            Precision.equals(y, q.y, eps) &&
            Precision.equals(z, q.z, 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 isUnit(double eps) {
        return type.isUnit(this, eps);
    }

    /**
     * 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 isPure(double eps) {
        return Math.abs(w) <= eps;
    }

    /**
     * Returns the polar form of the quaternion.
     *
     * @return the unit quaternion with positive scalar part.
     */
    public Quaternion positivePolarForm() {
        switch (type) {
        case POSITIVE_POLAR_FORM:
            return this;
        case NORMALIZED:
            return w >= 0 ?
                new Quaternion(Type.POSITIVE_POLAR_FORM, this) :
                new Quaternion(Type.POSITIVE_POLAR_FORM, negate());
        case DEFAULT:
            return w >= 0 ?
                normalize() :
                // The quaternion of rotation (normalized quaternion) q and -q
                // are equivalent (i.e. represent the same rotation).
                negate().normalize();
        default:
            throw new IllegalStateException(); // Should never happen.
        }
    }

    /**
     * Returns the opposite of this instance.
     *
     * @return the quaternion for which all components have an opposite
     * sign to this one.
     */
    public Quaternion negate() {
        switch (type) {
        case POSITIVE_POLAR_FORM:
        case NORMALIZED:
            return new Quaternion(Type.NORMALIZED, -w, -x, -y, -z);
        case DEFAULT:
            return new Quaternion(Type.DEFAULT, -w, -x, -y, -z);
        default:
            throw new IllegalStateException(); // Should never happen.
        }
    }

    /**
     * Returns the inverse of this instance.
     * The norm of the quaternion must not be zero.
     *
     * @return the inverse.
     * @throws IllegalStateException if the norm (squared) of the quaternion is NaN,
     *      infinite, or near zero.
     */
    public Quaternion inverse() {
        switch (type) {
        case POSITIVE_POLAR_FORM:
        case NORMALIZED:
            return new Quaternion(type, w, -x, -y, -z);
        case DEFAULT:
            final double squareNorm = normSq();
            if (squareNorm < Precision.SAFE_MIN ||
                !Double.isFinite(squareNorm)) {
                throw new IllegalStateException(ILLEGAL_NORM_MSG + Math.sqrt(squareNorm));
            }

            return of(w / squareNorm,
                      -x / squareNorm,
                      -y / squareNorm,
                      -z / squareNorm);
        default:
            throw new IllegalStateException(); // Should never happen.
        }
    }

    /**
     * Gets the first component of the quaternion (scalar part).
     *
     * @return the scalar part.
     */
    public double getW() {
        return w;
    }

    /**
     * Gets the second component of the quaternion (first component
     * of the vector part).
     *
     * @return the first component of the vector part.
     */
    public double getX() {
        return x;
    }

    /**
     * Gets the third component of the quaternion (second component
     * of the vector part).
     *
     * @return the second component of the vector part.
     */
    public double getY() {
        return y;
    }

    /**
     * Gets the fourth component of the quaternion (third component
     * of the vector part).
     *
     * @return the third component of the vector part.
     */
    public double getZ() {
        return z;
    }

    /**
     * Gets the scalar part of the quaternion.
     *
     * @return the scalar part.
     * @see #getW()
     */
    public double getScalarPart() {
        return getW();
    }

    /**
     * Gets the three components of the vector part of the quaternion.
     *
     * @return the vector part.
     * @see #getX()
     * @see #getY()
     * @see #getZ()
     */
    public double[] getVectorPart() {
        return new double[] {x, y, z};
    }

    /**
     * Multiplies the instance by a scalar.
     *
     * @param alpha Scalar factor.
     * @return a scaled quaternion.
     */
    public Quaternion multiply(final double alpha) {
        return of(alpha * w,
                  alpha * x,
                  alpha * y,
                  alpha * z);
    }

    /**
     * Divides the instance by a scalar.
     *
     * @param alpha Scalar factor.
     * @return a scaled quaternion.
     */
    public Quaternion divide(final double alpha) {
        return of(w / alpha,
                  x / alpha,
                  y / alpha,
                  z / alpha);
    }

    /**
     * Parses a string that would be produced by {@link #toString()}
     * and instantiates the corresponding object.
     *
     * @param s String representation.
     * @return an instance.
     * @throws NumberFormatException if the string does not conform
     * to the specification.
     */
    public static Quaternion parse(String s) {
        final int startBracket = s.indexOf(FORMAT_START);
        if (startBracket != 0) {
            throw new QuaternionParsingException("Expected start string: " + FORMAT_START);
        }
        final int len = s.length();
        final int endBracket = s.indexOf(FORMAT_END);
        if (endBracket != len - 1) {
            throw new QuaternionParsingException("Expected end string: " + FORMAT_END);
        }
        final String[] elements = s.substring(1, s.length() - 1).split(FORMAT_SEP);
        if (elements.length != NUMBER_OF_PARTS) {
            throw new QuaternionParsingException("Incorrect number of parts: Expected 4 but was " +
                                                 elements.length +
                                                 " (separator is '" + FORMAT_SEP + "')");
        }

        final double a;
        try {
            a = Double.parseDouble(elements[0]);
        } catch (NumberFormatException ex) {
            throw new QuaternionParsingException("Could not parse scalar part" + elements[0], ex);
        }
        final double b;
        try {
            b = Double.parseDouble(elements[1]);
        } catch (NumberFormatException ex) {
            throw new QuaternionParsingException("Could not parse i part" + elements[1], ex);
        }
        final double c;
        try {
            c = Double.parseDouble(elements[2]);
        } catch (NumberFormatException ex) {
            throw new QuaternionParsingException("Could not parse j part" + elements[2], ex);
        }
        final double d;
        try {
            d = Double.parseDouble(elements[3]);
        } catch (NumberFormatException ex) {
            throw new QuaternionParsingException("Could not parse k part" + elements[3], ex);
        }

        return of(a, b, c, d);
    }

    /**
     * {@inheritDoc}
     */
    @Override
    public String toString() {
        final StringBuilder s = new StringBuilder();
        s.append(FORMAT_START)
            .append(w).append(FORMAT_SEP)
            .append(x).append(FORMAT_SEP)
            .append(y).append(FORMAT_SEP)
            .append(z)
            .append(FORMAT_END);

        return s.toString();
    }

    /** See {@link #parse(String)}. */
    private static class QuaternionParsingException extends NumberFormatException {
        /** Serializable version identifier. */
        private static final long serialVersionUID = 20181128L;

        /**
         * @param msg Error message.
         */
        QuaternionParsingException(String msg) {
            super(msg);
        }

        /**
         * @param msg Error message.
         * @param cause Cause of the exception.
         */
        QuaternionParsingException(String msg, Throwable cause) {
            super(msg);
            initCause(cause);
        }
    }
}