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
018package org.apache.commons.numbers.quaternion;
019
020import java.util.Arrays;
021import java.util.function.ToDoubleFunction;
022import java.util.function.BiPredicate;
023import java.io.Serializable;
024import org.apache.commons.numbers.core.Precision;
025
026/**
027 * This class implements <a href="http://mathworld.wolfram.com/Quaternion.html">
028 * quaternions</a> (Hamilton's hypercomplex numbers).
029 *
030 * <p>Wherever quaternion components are listed in sequence, this class follows the
031 * convention of placing the scalar ({@code w}) component first, e.g. [{@code w, x, y, z}].
032 * Other libraries and textbooks may place the {@code w} component last.</p>
033 *
034 * <p>Instances of this class are guaranteed to be immutable.</p>
035 */
036public final class Quaternion implements Serializable {
037    /** Zero quaternion. */
038    public static final Quaternion ZERO = of(0, 0, 0, 0);
039    /** Identity quaternion. */
040    public static final Quaternion ONE = new Quaternion(Type.POSITIVE_POLAR_FORM, 1, 0, 0, 0);
041    /** i. */
042    public static final Quaternion I = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 1, 0, 0);
043    /** j. */
044    public static final Quaternion J = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 0, 1, 0);
045    /** k. */
046    public static final Quaternion K = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 0, 0, 1);
047
048    /** Serializable version identifier. */
049    private static final long serialVersionUID = 20170118L;
050    /** Error message. */
051    private static final String ILLEGAL_NORM_MSG = "Illegal norm: ";
052
053    /** {@link #toString() String representation}. */
054    private static final String FORMAT_START = "[";
055    /** {@link #toString() String representation}. */
056    private static final String FORMAT_END = "]";
057    /** {@link #toString() String representation}. */
058    private static final String FORMAT_SEP = " ";
059
060    /** The number of dimensions for the vector part of the quaternion. */
061    private static final int VECTOR_DIMENSIONS = 3;
062    /** The number of parts when parsing a text representation of the quaternion. */
063    private static final int NUMBER_OF_PARTS = 4;
064
065    /** For enabling specialized method implementations. */
066    private final Type type;
067    /** First component (scalar part). */
068    private final double w;
069    /** Second component (first vector part). */
070    private final double x;
071    /** Third component (second vector part). */
072    private final double y;
073    /** Fourth component (third vector part). */
074    private final double z;
075
076    /**
077     * For enabling optimized implementations.
078     */
079    private enum Type {
080        /** Default implementation. */
081        DEFAULT(Default.NORMSQ,
082                Default.NORM,
083                Default.IS_UNIT),
084        /** Quaternion has unit norm. */
085        NORMALIZED(Normalized.NORM,
086                   Normalized.NORM,
087                   Normalized.IS_UNIT),
088        /** Quaternion has positive scalar part. */
089        POSITIVE_POLAR_FORM(Normalized.NORM,
090                            Normalized.NORM,
091                            Normalized.IS_UNIT);
092
093        /** {@link Quaternion#normSq()}. */
094        private final ToDoubleFunction<Quaternion> normSq;
095        /** {@link Quaternion#norm()}. */
096        private final ToDoubleFunction<Quaternion> norm;
097        /** {@link Quaternion#isUnit()}. */
098        private final BiPredicate<Quaternion, Double> testIsUnit;
099
100        /** Default implementations. */
101        private static final class Default {
102            /** {@link Quaternion#normSq()}. */
103            static final ToDoubleFunction<Quaternion> NORMSQ = q ->
104                q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
105
106            /** {@link Quaternion#norm()}. */
107            private static final ToDoubleFunction<Quaternion> NORM = q ->
108                Math.sqrt(NORMSQ.applyAsDouble(q));
109
110            /** {@link Quaternion#isUnit()}. */
111            private static final BiPredicate<Quaternion, Double> IS_UNIT = (q, eps) ->
112                Precision.equals(NORM.applyAsDouble(q), 1d, eps);
113        }
114
115        /** Implementations for normalized quaternions. */
116        private static final class Normalized {
117            /** {@link Quaternion#norm()} returns 1. */
118            static final ToDoubleFunction<Quaternion> NORM = q -> 1;
119            /** {@link Quaternion#isUnit(double)} returns 1. */
120            static final BiPredicate<Quaternion, Double> IS_UNIT = (q, eps) -> true;
121        }
122
123        /**
124         * @param normSq {@code normSq} method.
125         * @param norm {@code norm} method.
126         * @param isUnit {@code isUnit} method.
127         */
128        Type(ToDoubleFunction<Quaternion> normSq,
129             ToDoubleFunction<Quaternion> norm,
130             BiPredicate<Quaternion, Double> isUnit)  {
131            this.normSq = normSq;
132            this.norm = norm;
133            this.testIsUnit = isUnit;
134        }
135
136        /**
137         * @param q Quaternion.
138         * @return the norm squared.
139         */
140        double normSq(Quaternion q) {
141            return normSq.applyAsDouble(q);
142        }
143        /**
144         * @param q Quaternion.
145         * @return the norm.
146         */
147        double norm(Quaternion q) {
148            return norm.applyAsDouble(q);
149        }
150        /**
151         * @param q Quaternion.
152         * @param eps Tolerance.
153         * @return whether {@code q} has unit norm within the allowed tolerance.
154         */
155        boolean isUnit(Quaternion q,
156                       double eps) {
157            return testIsUnit.test(q, eps);
158        }
159    }
160
161    /**
162     * Builds a quaternion from its components.
163     *
164     * @param type Quaternion type.
165     * @param w Scalar component.
166     * @param x First vector component.
167     * @param y Second vector component.
168     * @param z Third vector component.
169     */
170    private Quaternion(Type type,
171                       final double w,
172                       final double x,
173                       final double y,
174                       final double z) {
175        this.type = type;
176        this.w = w;
177        this.x = x;
178        this.y = y;
179        this.z = z;
180    }
181
182    /**
183     * Copies the given quaternion, but change its {@link Type}.
184     *
185     * @param type Quaternion type.
186     * @param q Quaternion whose components will be copied.
187     */
188    private Quaternion(Type type,
189                       Quaternion q) {
190        this.type = type;
191        w = q.w;
192        x = q.x;
193        y = q.y;
194        z = q.z;
195    }
196
197    /**
198     * Builds a quaternion from its components.
199     *
200     * @param w Scalar component.
201     * @param x First vector component.
202     * @param y Second vector component.
203     * @param z Third vector component.
204     * @return a quaternion instance.
205     */
206    public static Quaternion of(final double w,
207                                final double x,
208                                final double y,
209                                final double z) {
210        return new Quaternion(Type.DEFAULT,
211                              w, x, y, z);
212    }
213
214    /**
215     * Builds a quaternion from scalar and vector parts.
216     *
217     * @param scalar Scalar part of the quaternion.
218     * @param v Components of the vector part of the quaternion.
219     * @return a quaternion instance.
220     *
221     * @throws IllegalArgumentException if the array length is not 3.
222     */
223    public static Quaternion of(final double scalar,
224                                final double[] v) {
225        if (v.length != VECTOR_DIMENSIONS) {
226            throw new IllegalArgumentException("Size of array must be 3");
227        }
228
229        return of(scalar, v[0], v[1], v[2]);
230    }
231
232    /**
233     * Builds a pure quaternion from a vector (assuming that the scalar
234     * part is zero).
235     *
236     * @param v Components of the vector part of the pure quaternion.
237     * @return a quaternion instance.
238     */
239    public static Quaternion of(final double[] v) {
240        return of(0, v);
241    }
242
243    /**
244     * Returns the conjugate of this quaternion number.
245     * The conjugate of {@code a + bi + cj + dk} is {@code a - bi -cj -dk}.
246     *
247     * @return the conjugate of this quaternion object.
248     */
249    public Quaternion conjugate() {
250        return of(w, -x, -y, -z);
251    }
252
253    /**
254     * Returns the Hamilton product of two quaternions.
255     *
256     * @param q1 First quaternion.
257     * @param q2 Second quaternion.
258     * @return the product {@code q1} and {@code q2}, in that order.
259     */
260    public static Quaternion multiply(final Quaternion q1,
261                                      final Quaternion q2) {
262        // Components of the first quaternion.
263        final double q1a = q1.w;
264        final double q1b = q1.x;
265        final double q1c = q1.y;
266        final double q1d = q1.z;
267
268        // Components of the second quaternion.
269        final double q2a = q2.w;
270        final double q2b = q2.x;
271        final double q2c = q2.y;
272        final double q2d = q2.z;
273
274        // Components of the product.
275        final double w = q1a * q2a - q1b * q2b - q1c * q2c - q1d * q2d;
276        final double x = q1a * q2b + q1b * q2a + q1c * q2d - q1d * q2c;
277        final double y = q1a * q2c - q1b * q2d + q1c * q2a + q1d * q2b;
278        final double z = q1a * q2d + q1b * q2c - q1c * q2b + q1d * q2a;
279
280        return of(w, x, y, z);
281    }
282
283    /**
284     * Returns the Hamilton product of the instance by a quaternion.
285     *
286     * @param q Quaternion.
287     * @return the product of this instance with {@code q}, in that order.
288     */
289    public Quaternion multiply(final Quaternion q) {
290        return multiply(this, q);
291    }
292
293    /**
294     * Computes the sum of two quaternions.
295     *
296     * @param q1 Quaternion.
297     * @param q2 Quaternion.
298     * @return the sum of {@code q1} and {@code q2}.
299     */
300    public static Quaternion add(final Quaternion q1,
301                                 final Quaternion q2) {
302        return of(q1.w + q2.w,
303                  q1.x + q2.x,
304                  q1.y + q2.y,
305                  q1.z + q2.z);
306    }
307
308    /**
309     * Computes the sum of the instance and another quaternion.
310     *
311     * @param q Quaternion.
312     * @return the sum of this instance and {@code q}.
313     */
314    public Quaternion add(final Quaternion q) {
315        return add(this, q);
316    }
317
318    /**
319     * Subtracts two quaternions.
320     *
321     * @param q1 First Quaternion.
322     * @param q2 Second quaternion.
323     * @return the difference between {@code q1} and {@code q2}.
324     */
325    public static Quaternion subtract(final Quaternion q1,
326                                      final Quaternion q2) {
327        return of(q1.w - q2.w,
328                  q1.x - q2.x,
329                  q1.y - q2.y,
330                  q1.z - q2.z);
331    }
332
333    /**
334     * Subtracts a quaternion from the instance.
335     *
336     * @param q Quaternion.
337     * @return the difference between this instance and {@code q}.
338     */
339    public Quaternion subtract(final Quaternion q) {
340        return subtract(this, q);
341    }
342
343    /**
344     * Computes the dot-product of two quaternions.
345     *
346     * @param q1 Quaternion.
347     * @param q2 Quaternion.
348     * @return the dot product of {@code q1} and {@code q2}.
349     */
350    public static double dot(final Quaternion q1,
351                             final Quaternion q2) {
352        return q1.w * q2.w +
353            q1.x * q2.x +
354            q1.y * q2.y +
355            q1.z * q2.z;
356    }
357
358    /**
359     * Computes the dot-product of the instance by a quaternion.
360     *
361     * @param q Quaternion.
362     * @return the dot product of this instance and {@code q}.
363     */
364    public double dot(final Quaternion q) {
365        return dot(this, q);
366    }
367
368    /**
369     * Computes the norm of the quaternion.
370     *
371     * @return the norm.
372     */
373    public double norm() {
374        return type.norm(this);
375    }
376
377    /**
378     * Computes the square of the norm of the quaternion.
379     *
380     * @return the square of the norm.
381     */
382    public double normSq() {
383        return type.normSq(this);
384    }
385
386    /**
387     * Computes the normalized quaternion (the versor of the instance).
388     * The norm of the quaternion must not be near zero.
389     *
390     * @return a normalized quaternion.
391     * @throws IllegalStateException if the norm of the quaternion is NaN, infinite,
392     *      or near zero.
393     */
394    public Quaternion normalize() {
395        switch (type) {
396        case NORMALIZED:
397        case POSITIVE_POLAR_FORM:
398            return this;
399        case DEFAULT:
400            final double norm = norm();
401
402            if (norm < Precision.SAFE_MIN ||
403                !Double.isFinite(norm)) {
404                throw new IllegalStateException(ILLEGAL_NORM_MSG + norm);
405            }
406
407            final Quaternion unit = divide(norm);
408
409            return w >= 0 ?
410                new Quaternion(Type.POSITIVE_POLAR_FORM, unit) :
411                new Quaternion(Type.NORMALIZED, unit);
412        default:
413            throw new IllegalStateException(); // Should never happen.
414        }
415    }
416
417    /**
418     * {@inheritDoc}
419     */
420    @Override
421    public boolean equals(Object other) {
422        if (this == other) {
423            return true;
424        }
425        if (other instanceof Quaternion) {
426            final Quaternion q = (Quaternion) other;
427            return ((Double) w).equals(q.w) &&
428                ((Double) x).equals(q.x) &&
429                ((Double) y).equals(q.y) &&
430                ((Double) z).equals(q.z);
431        }
432
433        return false;
434    }
435
436    /**
437     * {@inheritDoc}
438     */
439    @Override
440    public int hashCode() {
441        return Arrays.hashCode(new double[] {w, x, y, z});
442    }
443
444    /**
445     * Checks whether this instance is equal to another quaternion
446     * within a given tolerance.
447     *
448     * @param q Quaternion with which to compare the current quaternion.
449     * @param eps Tolerance.
450     * @return {@code true} if the each of the components are equal
451     * within the allowed absolute error.
452     */
453    public boolean equals(final Quaternion q,
454                          final double eps) {
455        return Precision.equals(w, q.w, eps) &&
456            Precision.equals(x, q.x, eps) &&
457            Precision.equals(y, q.y, eps) &&
458            Precision.equals(z, q.z, eps);
459    }
460
461    /**
462     * Checks whether the instance is a unit quaternion within a given
463     * tolerance.
464     *
465     * @param eps Tolerance (absolute error).
466     * @return {@code true} if the norm is 1 within the given tolerance,
467     * {@code false} otherwise
468     */
469    public boolean isUnit(double eps) {
470        return type.isUnit(this, eps);
471    }
472
473    /**
474     * Checks whether the instance is a pure quaternion within a given
475     * tolerance.
476     *
477     * @param eps Tolerance (absolute error).
478     * @return {@code true} if the scalar part of the quaternion is zero.
479     */
480    public boolean isPure(double eps) {
481        return Math.abs(w) <= eps;
482    }
483
484    /**
485     * Returns the polar form of the quaternion.
486     *
487     * @return the unit quaternion with positive scalar part.
488     */
489    public Quaternion positivePolarForm() {
490        switch (type) {
491        case POSITIVE_POLAR_FORM:
492            return this;
493        case NORMALIZED:
494            return w >= 0 ?
495                new Quaternion(Type.POSITIVE_POLAR_FORM, this) :
496                new Quaternion(Type.POSITIVE_POLAR_FORM, negate());
497        case DEFAULT:
498            return w >= 0 ?
499                normalize() :
500                // The quaternion of rotation (normalized quaternion) q and -q
501                // are equivalent (i.e. represent the same rotation).
502                negate().normalize();
503        default:
504            throw new IllegalStateException(); // Should never happen.
505        }
506    }
507
508    /**
509     * Returns the opposite of this instance.
510     *
511     * @return the quaternion for which all components have an opposite
512     * sign to this one.
513     */
514    public Quaternion negate() {
515        switch (type) {
516        case POSITIVE_POLAR_FORM:
517        case NORMALIZED:
518            return new Quaternion(Type.NORMALIZED, -w, -x, -y, -z);
519        case DEFAULT:
520            return new Quaternion(Type.DEFAULT, -w, -x, -y, -z);
521        default:
522            throw new IllegalStateException(); // Should never happen.
523        }
524    }
525
526    /**
527     * Returns the inverse of this instance.
528     * The norm of the quaternion must not be zero.
529     *
530     * @return the inverse.
531     * @throws IllegalStateException if the norm (squared) of the quaternion is NaN,
532     *      infinite, or near zero.
533     */
534    public Quaternion inverse() {
535        switch (type) {
536        case POSITIVE_POLAR_FORM:
537        case NORMALIZED:
538            return new Quaternion(type, w, -x, -y, -z);
539        case DEFAULT:
540            final double squareNorm = normSq();
541            if (squareNorm < Precision.SAFE_MIN ||
542                !Double.isFinite(squareNorm)) {
543                throw new IllegalStateException(ILLEGAL_NORM_MSG + Math.sqrt(squareNorm));
544            }
545
546            return of(w / squareNorm,
547                      -x / squareNorm,
548                      -y / squareNorm,
549                      -z / squareNorm);
550        default:
551            throw new IllegalStateException(); // Should never happen.
552        }
553    }
554
555    /**
556     * Gets the first component of the quaternion (scalar part).
557     *
558     * @return the scalar part.
559     */
560    public double getW() {
561        return w;
562    }
563
564    /**
565     * Gets the second component of the quaternion (first component
566     * of the vector part).
567     *
568     * @return the first component of the vector part.
569     */
570    public double getX() {
571        return x;
572    }
573
574    /**
575     * Gets the third component of the quaternion (second component
576     * of the vector part).
577     *
578     * @return the second component of the vector part.
579     */
580    public double getY() {
581        return y;
582    }
583
584    /**
585     * Gets the fourth component of the quaternion (third component
586     * of the vector part).
587     *
588     * @return the third component of the vector part.
589     */
590    public double getZ() {
591        return z;
592    }
593
594    /**
595     * Gets the scalar part of the quaternion.
596     *
597     * @return the scalar part.
598     * @see #getW()
599     */
600    public double getScalarPart() {
601        return getW();
602    }
603
604    /**
605     * Gets the three components of the vector part of the quaternion.
606     *
607     * @return the vector part.
608     * @see #getX()
609     * @see #getY()
610     * @see #getZ()
611     */
612    public double[] getVectorPart() {
613        return new double[] {x, y, z};
614    }
615
616    /**
617     * Multiplies the instance by a scalar.
618     *
619     * @param alpha Scalar factor.
620     * @return a scaled quaternion.
621     */
622    public Quaternion multiply(final double alpha) {
623        return of(alpha * w,
624                  alpha * x,
625                  alpha * y,
626                  alpha * z);
627    }
628
629    /**
630     * Divides the instance by a scalar.
631     *
632     * @param alpha Scalar factor.
633     * @return a scaled quaternion.
634     */
635    public Quaternion divide(final double alpha) {
636        return of(w / alpha,
637                  x / alpha,
638                  y / alpha,
639                  z / alpha);
640    }
641
642    /**
643     * Parses a string that would be produced by {@link #toString()}
644     * and instantiates the corresponding object.
645     *
646     * @param s String representation.
647     * @return an instance.
648     * @throws NumberFormatException if the string does not conform
649     * to the specification.
650     */
651    public static Quaternion parse(String s) {
652        final int startBracket = s.indexOf(FORMAT_START);
653        if (startBracket != 0) {
654            throw new QuaternionParsingException("Expected start string: " + FORMAT_START);
655        }
656        final int len = s.length();
657        final int endBracket = s.indexOf(FORMAT_END);
658        if (endBracket != len - 1) {
659            throw new QuaternionParsingException("Expected end string: " + FORMAT_END);
660        }
661        final String[] elements = s.substring(1, s.length() - 1).split(FORMAT_SEP);
662        if (elements.length != NUMBER_OF_PARTS) {
663            throw new QuaternionParsingException("Incorrect number of parts: Expected 4 but was " +
664                                                 elements.length +
665                                                 " (separator is '" + FORMAT_SEP + "')");
666        }
667
668        final double a;
669        try {
670            a = Double.parseDouble(elements[0]);
671        } catch (NumberFormatException ex) {
672            throw new QuaternionParsingException("Could not parse scalar part" + elements[0], ex);
673        }
674        final double b;
675        try {
676            b = Double.parseDouble(elements[1]);
677        } catch (NumberFormatException ex) {
678            throw new QuaternionParsingException("Could not parse i part" + elements[1], ex);
679        }
680        final double c;
681        try {
682            c = Double.parseDouble(elements[2]);
683        } catch (NumberFormatException ex) {
684            throw new QuaternionParsingException("Could not parse j part" + elements[2], ex);
685        }
686        final double d;
687        try {
688            d = Double.parseDouble(elements[3]);
689        } catch (NumberFormatException ex) {
690            throw new QuaternionParsingException("Could not parse k part" + elements[3], ex);
691        }
692
693        return of(a, b, c, d);
694    }
695
696    /**
697     * {@inheritDoc}
698     */
699    @Override
700    public String toString() {
701        final StringBuilder s = new StringBuilder();
702        s.append(FORMAT_START)
703            .append(w).append(FORMAT_SEP)
704            .append(x).append(FORMAT_SEP)
705            .append(y).append(FORMAT_SEP)
706            .append(z)
707            .append(FORMAT_END);
708
709        return s.toString();
710    }
711
712    /** See {@link #parse(String)}. */
713    private static class QuaternionParsingException extends NumberFormatException {
714        /** Serializable version identifier. */
715        private static final long serialVersionUID = 20181128L;
716
717        /**
718         * @param msg Error message.
719         */
720        QuaternionParsingException(String msg) {
721            super(msg);
722        }
723
724        /**
725         * @param msg Error message.
726         * @param cause Cause of the exception.
727         */
728        QuaternionParsingException(String msg, Throwable cause) {
729            super(msg);
730            initCause(cause);
731        }
732    }
733}