    001    /*
    002     * Licensed to the Apache Software Foundation (ASF) under one or more
    003     * contributor license agreements.  See the NOTICE file distributed with
    004     * this work for additional information regarding copyright ownership.
    005     * The ASF licenses this file to You under the Apache License, Version 2.0
    006     * (the "License"); you may not use this file except in compliance with
    007     * the License.  You may obtain a copy of the License at
    008     *
    009     *      http://www.apache.org/licenses/LICENSE-2.0
    010     *
    011     * Unless required by applicable law or agreed to in writing, software
    012     * distributed under the License is distributed on an "AS IS" BASIS,
    013     * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    014     * See the License for the specific language governing permissions and
    015     * limitations under the License.
    016     */
    017
    018    package org.apache.commons.math3.complex;
    019
    020    import java.io.Serializable;
    021    import org.apache.commons.math3.util.FastMath;
    022    import org.apache.commons.math3.util.MathUtils;
    023    import org.apache.commons.math3.util.Precision;
    024    import org.apache.commons.math3.exception.DimensionMismatchException;
    025    import org.apache.commons.math3.exception.ZeroException;
    026    import org.apache.commons.math3.exception.util.LocalizedFormats;
    027
    028    /**
    029     * This class implements <a href="http://mathworld.wolfram.com/Quaternion.html">
    030     * quaternions</a> (Hamilton's hypercomplex numbers).
    031     * <br/>
    032     * Instance of this class are guaranteed to be immutable.
    033     *
    034     * @since 3.1
    035     * @version $Id: Quaternion.java 1421249 2012-12-13 12:32:03Z erans $
    036     */
    037    public final class Quaternion implements Serializable {
    038        /** Identity quaternion. */
    039        public static final Quaternion IDENTITY = new Quaternion(1, 0, 0, 0);
    040        /** Zero quaternion. */
    041        public static final Quaternion ZERO = new Quaternion(0, 0, 0, 0);
    042        /** i */
    043        public static final Quaternion I = new Quaternion(0, 1, 0, 0);
    044        /** j */
    045        public static final Quaternion J = new Quaternion(0, 0, 1, 0);
    046        /** k */
    047        public static final Quaternion K = new Quaternion(0, 0, 0, 1);
    048
    049        /** Serializable version identifier. */
    050        private static final long serialVersionUID = 20092012L;
    051
    052        /** First component (scalar part). */
    053        private final double q0;
    054        /** Second component (first vector part). */
    055        private final double q1;
    056        /** Third component (second vector part). */
    057        private final double q2;
    058        /** Fourth component (third vector part). */
    059        private final double q3;
    060
    061        /**
    062         * Builds a quaternion from its components.
    063         *
    064         * @param a Scalar component.
    065         * @param b First vector component.
    066         * @param c Second vector component.
    067         * @param d Third vector component.
    068         */
    069        public Quaternion(final double a,
    070                          final double b,
    071                          final double c,
    072                          final double d) {
    073            this.q0 = a;
    074            this.q1 = b;
    075            this.q2 = c;
    076            this.q3 = d;
    077        }
    078
    079        /**
    080         * Builds a quaternion from scalar and vector parts.
    081         *
    082         * @param scalar Scalar part of the quaternion.
    083         * @param v Components of the vector part of the quaternion.
    084         *
    085         * @throws DimensionMismatchException if the array length is not 3.
    086         */
    087        public Quaternion(final double scalar,
    088                          final double[] v)
    089            throws DimensionMismatchException {
    090            if (v.length != 3) {
    091                throw new DimensionMismatchException(v.length, 3);
    092            }
    093            this.q0 = scalar;
    094            this.q1 = v[0];
    095            this.q2 = v[1];
    096            this.q3 = v[2];
    097        }
    098
    099        /**
    100         * Builds a pure quaternion from a vector (assuming that the scalar
    101         * part is zero).
    102         *
    103         * @param v Components of the vector part of the pure quaternion.
    104         */
    105        public Quaternion(final double[] v) {
    106            this(0, v);
    107        }
    108
    109        /**
    110         * Returns the conjugate quaternion of the instance.
    111         *
    112         * @return the conjugate quaternion
    113         */
    114        public Quaternion getConjugate() {
    115            return new Quaternion(q0, -q1, -q2, -q3);
    116        }
    117
    118        /**
    119         * Returns the Hamilton product of two quaternions.
    120         *
    121         * @param q1 First quaternion.
    122         * @param q2 Second quaternion.
    123         * @return the product {@code q1} and {@code q2}, in that order.
    124         */
    125        public static Quaternion multiply(final Quaternion q1, final Quaternion q2) {
    126            // Components of the first quaternion.
    127            final double q1a = q1.getQ0();
    128            final double q1b = q1.getQ1();
    129            final double q1c = q1.getQ2();
    130            final double q1d = q1.getQ3();
    131
    132            // Components of the second quaternion.
    133            final double q2a = q2.getQ0();
    134            final double q2b = q2.getQ1();
    135            final double q2c = q2.getQ2();
    136            final double q2d = q2.getQ3();
    137
    138            // Components of the product.
    139            final double w = q1a * q2a - q1b * q2b - q1c * q2c - q1d * q2d;
    140            final double x = q1a * q2b + q1b * q2a + q1c * q2d - q1d * q2c;
    141            final double y = q1a * q2c - q1b * q2d + q1c * q2a + q1d * q2b;
    142            final double z = q1a * q2d + q1b * q2c - q1c * q2b + q1d * q2a;
    143
    144            return new Quaternion(w, x, y, z);
    145        }
    146
    147        /**
    148         * Returns the Hamilton product of the instance by a quaternion.
    149         *
    150         * @param q Quaternion.
    151         * @return the product of this instance with {@code q}, in that order.
    152         */
    153        public Quaternion multiply(final Quaternion q) {
    154            return multiply(this, q);
    155        }
    156
    157        /**
    158         * Computes the sum of two quaternions.
    159         *
    160         * @param q1 Quaternion.
    161         * @param q2 Quaternion.
    162         * @return the sum of {@code q1} and {@code q2}.
    163         */
    164        public static Quaternion add(final Quaternion q1,
    165                                     final Quaternion q2) {
    166            return new Quaternion(q1.getQ0() + q2.getQ0(),
    167                                  q1.getQ1() + q2.getQ1(),
    168                                  q1.getQ2() + q2.getQ2(),
    169                                  q1.getQ3() + q2.getQ3());
    170        }
    171
    172        /**
    173         * Computes the sum of the instance and another quaternion.
    174         *
    175         * @param q Quaternion.
    176         * @return the sum of this instance and {@code q}
    177         */
    178        public Quaternion add(final Quaternion q) {
    179            return add(this, q);
    180        }
    181
    182        /**
    183         * Subtracts two quaternions.
    184         *
    185         * @param q1 First Quaternion.
    186         * @param q2 Second quaternion.
    187         * @return the difference between {@code q1} and {@code q2}.
    188         */
    189        public static Quaternion subtract(final Quaternion q1,
    190                                          final Quaternion q2) {
    191            return new Quaternion(q1.getQ0() - q2.getQ0(),
    192                                  q1.getQ1() - q2.getQ1(),
    193                                  q1.getQ2() - q2.getQ2(),
    194                                  q1.getQ3() - q2.getQ3());
    195        }
    196
    197        /**
    198         * Subtracts a quaternion from the instance.
    199         *
    200         * @param q Quaternion.
    201         * @return the difference between this instance and {@code q}.
    202         */
    203        public Quaternion subtract(final Quaternion q) {
    204            return subtract(this, q);
    205        }
    206
    207        /**
    208         * Computes the dot-product of two quaternions.
    209         *
    210         * @param q1 Quaternion.
    211         * @param q2 Quaternion.
    212         * @return the dot product of {@code q1} and {@code q2}.
    213         */
    214        public static double dotProduct(final Quaternion q1,
    215                                        final Quaternion q2) {
    216            return q1.getQ0() * q2.getQ0() +
    217                q1.getQ1() * q2.getQ1() +
    218                q1.getQ2() * q2.getQ2() +
    219                q1.getQ3() * q2.getQ3();
    220        }
    221
    222        /**
    223         * Computes the dot-product of the instance by a quaternion.
    224         *
    225         * @param q Quaternion.
    226         * @return the dot product of this instance and {@code q}.
    227         */
    228        public double dotProduct(final Quaternion q) {
    229            return dotProduct(this, q);
    230        }
    231
    232        /**
    233         * Computes the norm of the quaternion.
    234         *
    235         * @return the norm.
    236         */
    237        public double getNorm() {
    238            return FastMath.sqrt(q0 * q0 +
    239                                 q1 * q1 +
    240                                 q2 * q2 +
    241                                 q3 * q3);
    242        }
    243
    244        /**
    245         * Computes the normalized quaternion (the versor of the instance).
    246         * The norm of the quaternion must not be zero.
    247         *
    248         * @return a normalized quaternion.
    249         * @throws ZeroException if the norm of the quaternion is zero.
    250         */
    251        public Quaternion normalize() {
    252            final double norm = getNorm();
    253
    254            if (norm < Precision.SAFE_MIN) {
    255                throw new ZeroException(LocalizedFormats.NORM, norm);
    256            }
    257
    258            return new Quaternion(q0 / norm,
    259                                  q1 / norm,
    260                                  q2 / norm,
    261                                  q3 / norm);
    262        }
    263
    264        /**
    265         * {@inheritDoc}
    266         */
    267        @Override
    268        public boolean equals(Object other) {
    269            if (this == other) {
    270                return true;
    271            }
    272            if (other instanceof Quaternion) {
    273                final Quaternion q = (Quaternion) other;
    274                return q0 == q.getQ0() &&
    275                    q1 == q.getQ1() &&
    276                    q2 == q.getQ2() &&
    277                    q3 == q.getQ3();
    278            }
    279
    280            return false;
    281        }
    282
    283        /**
    284         * {@inheritDoc}
    285         */
    286        @Override
    287        public int hashCode() {
    288            // "Effective Java" (second edition, p. 47).
    289            int result = 17;
    290            for (double comp : new double[] { q0, q1, q2, q3 }) {
    291                final int c = MathUtils.hash(comp);
    292                result = 31 * result + c;
    293            }
    294            return result;
    295        }
    296
    297        /**
    298         * Checks whether this instance is equal to another quaternion
    299         * within a given tolerance.
    300         *
    301         * @param q Quaternion with which to compare the current quaternion.
    302         * @param eps Tolerance.
    303         * @return {@code true} if the each of the components are equal
    304         * within the allowed absolute error.
    305         */
    306        public boolean equals(final Quaternion q,
    307                              final double eps) {
    308            return Precision.equals(q0, q.getQ0(), eps) &&
    309                Precision.equals(q1, q.getQ1(), eps) &&
    310                Precision.equals(q2, q.getQ2(), eps) &&
    311                Precision.equals(q3, q.getQ3(), eps);
    312        }
    313
    314        /**
    315         * Checks whether the instance is a unit quaternion within a given
    316         * tolerance.
    317         *
    318         * @param eps Tolerance (absolute error).
    319         * @return {@code true} if the norm is 1 within the given tolerance,
    320         * {@code false} otherwise
    321         */
    322        public boolean isUnitQuaternion(double eps) {
    323            return Precision.equals(getNorm(), 1d, eps);
    324        }
    325
    326        /**
    327         * Checks whether the instance is a pure quaternion within a given
    328         * tolerance.
    329         *
    330         * @param eps Tolerance (absolute error).
    331         * @return {@code true} if the scalar part of the quaternion is zero.
    332         */
    333        public boolean isPureQuaternion(double eps) {
    334            return FastMath.abs(getQ0()) <= eps;
    335        }
    336
    337        /**
    338         * Returns the polar form of the quaternion.
    339         *
    340         * @return the unit quaternion with positive scalar part.
    341         */
    342        public Quaternion getPositivePolarForm() {
    343            if (getQ0() < 0) {
    344                final Quaternion unitQ = normalize();
    345                // The quaternion of rotation (normalized quaternion) q and -q
    346                // are equivalent (i.e. represent the same rotation).
    347                return new Quaternion(-unitQ.getQ0(),
    348                                      -unitQ.getQ1(),
    349                                      -unitQ.getQ2(),
    350                                      -unitQ.getQ3());
    351            } else {
    352                return this.normalize();
    353            }
    354        }
    355
    356        /**
    357         * Returns the inverse of this instance.
    358         * The norm of the quaternion must not be zero.
    359         *
    360         * @return the inverse.
    361         * @throws ZeroException if the norm (squared) of the quaternion is zero.
    362         */
    363        public Quaternion getInverse() {
    364            final double squareNorm = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3;
    365            if (squareNorm < Precision.SAFE_MIN) {
    366                throw new ZeroException(LocalizedFormats.NORM, squareNorm);
    367            }
    368
    369            return new Quaternion(q0 / squareNorm,
    370                                  -q1 / squareNorm,
    371                                  -q2 / squareNorm,
    372                                  -q3 / squareNorm);
    373        }
    374
    375        /**
    376         * Gets the first component of the quaternion (scalar part).
    377         *
    378         * @return the scalar part.
    379         */
    380        public double getQ0() {
    381            return q0;
    382        }
    383
    384        /**
    385         * Gets the second component of the quaternion (first component
    386         * of the vector part).
    387         *
    388         * @return the first component of the vector part.
    389         */
    390        public double getQ1() {
    391            return q1;
    392        }
    393
    394        /**
    395         * Gets the third component of the quaternion (second component
    396         * of the vector part).
    397         *
    398         * @return the second component of the vector part.
    399         */
    400        public double getQ2() {
    401            return q2;
    402        }
    403
    404        /**
    405         * Gets the fourth component of the quaternion (third component
    406         * of the vector part).
    407         *
    408         * @return the third component of the vector part.
    409         */
    410        public double getQ3() {
    411            return q3;
    412        }
    413
    414        /**
    415         * Gets the scalar part of the quaternion.
    416         *
    417         * @return the scalar part.
    418         * @see #getQ0()
    419         */
    420        public double getScalarPart() {
    421            return getQ0();
    422        }
    423
    424        /**
    425         * Gets the three components of the vector part of the quaternion.
    426         *
    427         * @return the vector part.
    428         * @see #getQ1()
    429         * @see #getQ2()
    430         * @see #getQ3()
    431         */
    432        public double[] getVectorPart() {
    433            return new double[] { getQ1(), getQ2(), getQ3() };
    434        }
    435
    436        /**
    437         * Multiplies the instance by a scalar.
    438         *
    439         * @param alpha Scalar factor.
    440         * @return a scaled quaternion.
    441         */
    442        public Quaternion multiply(final double alpha) {
    443            return new Quaternion(alpha * q0,
    444                                  alpha * q1,
    445                                  alpha * q2,
    446                                  alpha * q3);
    447        }
    448
    449        /**
    450         * {@inheritDoc}
    451         */
    452        @Override
    453        public String toString() {
    454            final String sp = " ";
    455            final StringBuilder s = new StringBuilder();
    456            s.append("[")
    457                .append(q0).append(sp)
    458                .append(q1).append(sp)
    459                .append(q2).append(sp)
    460                .append(q3)
    461                .append("]");
    462
    463            return s.toString();
    464        }
    465    }