Quaternion.java

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

  18. /*
  19.  * This is not the original file distributed by the Apache Software Foundation
  20.  * It has been modified by the Hipparchus project
  21.  */

  23. package org.hipparchus.complex;

  25. import java.io.Serializable;

  27. import org.hipparchus.exception.LocalizedCoreFormats;
  28. import org.hipparchus.exception.MathIllegalArgumentException;
  29. import org.hipparchus.util.FastMath;
  30. import org.hipparchus.util.MathUtils;
  31. import org.hipparchus.util.Precision;

  33. /**
  34.  * This class implements <a href="http://mathworld.wolfram.com/Quaternion.html">
  35.  * quaternions</a> (Hamilton's hypercomplex numbers).
  36.  * <p>
  37.  * Instance of this class are guaranteed to be immutable.
  38.  */
  39. public final class Quaternion implements Serializable {
  40.     /** Identity quaternion. */
  41.     public static final Quaternion IDENTITY = new Quaternion(1, 0, 0, 0);
  42.     /** Zero quaternion. */
  43.     public static final Quaternion ZERO = new Quaternion(0, 0, 0, 0);
  44.     /** i */
  45.     public static final Quaternion I = new Quaternion(0, 1, 0, 0);
  46.     /** j */
  47.     public static final Quaternion J = new Quaternion(0, 0, 1, 0);
  48.     /** k */
  49.     public static final Quaternion K = new Quaternion(0, 0, 0, 1);

  51.     /** Serializable version identifier. */
  52.     private static final long serialVersionUID = 20092012L;

  54.     /** First component (scalar part). */
  55.     private final double q0;
  56.     /** Second component (first vector part). */
  57.     private final double q1;
  58.     /** Third component (second vector part). */
  59.     private final double q2;
  60.     /** Fourth component (third vector part). */
  61.     private final double q3;

  63.     /**
  64.      * Builds a quaternion from its components.
  65.      *
  66.      * @param a Scalar component.
  67.      * @param b First vector component.
  68.      * @param c Second vector component.
  69.      * @param d Third vector component.
  70.      */
  71.     public Quaternion(final double a,
  72.                       final double b,
  73.                       final double c,
  74.                       final double d) {
  75.         this.q0 = a;
  76.         this.q1 = b;
  77.         this.q2 = c;
  78.         this.q3 = d;
  79.     }

  81.     /**
  82.      * Builds a quaternion from scalar and vector parts.
  83.      *
  84.      * @param scalar Scalar part of the quaternion.
  85.      * @param v Components of the vector part of the quaternion.
  86.      *
  87.      * @throws MathIllegalArgumentException if the array length is not 3.
  88.      */
  89.     public Quaternion(final double scalar, final double[] v)
  90.         throws MathIllegalArgumentException {
  91.         MathUtils.checkDimension(v.length, 3);
  92.         this.q0 = scalar;
  93.         this.q1 = v[0];
  94.         this.q2 = v[1];
  95.         this.q3 = v[2];
  96.     }

  98.     /**
  99.      * Builds a pure quaternion from a vector (assuming that the scalar
  100.      * part is zero).
  101.      *
  102.      * @param v Components of the vector part of the pure quaternion.
  103.      */
  104.     public Quaternion(final double[] v) {
  105.         this(0, v);
  106.     }

  108.     /**
  109.      * Returns the conjugate quaternion of the instance.
  110.      *
  111.      * @return the conjugate quaternion
  112.      */
  113.     public Quaternion getConjugate() {
  114.         return new Quaternion(q0, -q1, -q2, -q3);
  115.     }

  117.     /**
  118.      * Returns the Hamilton product of two quaternions.
  119.      *
  120.      * @param q1 First quaternion.
  121.      * @param q2 Second quaternion.
  122.      * @return the product {@code q1} and {@code q2}, in that order.
  123.      */
  124.     public static Quaternion multiply(final Quaternion q1, final Quaternion q2) {
  125.         // Components of the first quaternion.
  126.         final double q1a = q1.getQ0();
  127.         final double q1b = q1.getQ1();
  128.         final double q1c = q1.getQ2();
  129.         final double q1d = q1.getQ3();

  131.         // Components of the second quaternion.
  132.         final double q2a = q2.getQ0();
  133.         final double q2b = q2.getQ1();
  134.         final double q2c = q2.getQ2();
  135.         final double q2d = q2.getQ3();

  137.         // Components of the product.
  138.         final double w = q1a * q2a - q1b * q2b - q1c * q2c - q1d * q2d;
  139.         final double x = q1a * q2b + q1b * q2a + q1c * q2d - q1d * q2c;
  140.         final double y = q1a * q2c - q1b * q2d + q1c * q2a + q1d * q2b;
  141.         final double z = q1a * q2d + q1b * q2c - q1c * q2b + q1d * q2a;

  143.         return new Quaternion(w, x, y, z);
  144.     }

  146.     /**
  147.      * Returns the Hamilton product of the instance by a quaternion.
  148.      *
  149.      * @param q Quaternion.
  150.      * @return the product of this instance with {@code q}, in that order.
  151.      */
  152.     public Quaternion multiply(final Quaternion q) {
  153.         return multiply(this, q);
  154.     }

  156.     /**
  157.      * Computes the sum of two quaternions.
  158.      *
  159.      * @param q1 Quaternion.
  160.      * @param q2 Quaternion.
  161.      * @return the sum of {@code q1} and {@code q2}.
  162.      */
  163.     public static Quaternion add(final Quaternion q1,
  164.                                  final Quaternion q2) {
  165.         return new Quaternion(q1.getQ0() + q2.getQ0(),
  166.                               q1.getQ1() + q2.getQ1(),
  167.                               q1.getQ2() + q2.getQ2(),
  168.                               q1.getQ3() + q2.getQ3());
  169.     }

  171.     /**
  172.      * Computes the sum of the instance and another quaternion.
  173.      *
  174.      * @param q Quaternion.
  175.      * @return the sum of this instance and {@code q}
  176.      */
  177.     public Quaternion add(final Quaternion q) {
  178.         return add(this, q);
  179.     }

  181.     /**
  182.      * Subtracts two quaternions.
  183.      *
  184.      * @param q1 First Quaternion.
  185.      * @param q2 Second quaternion.
  186.      * @return the difference between {@code q1} and {@code q2}.
  187.      */
  188.     public static Quaternion subtract(final Quaternion q1,
  189.                                       final Quaternion q2) {
  190.         return new Quaternion(q1.getQ0() - q2.getQ0(),
  191.                               q1.getQ1() - q2.getQ1(),
  192.                               q1.getQ2() - q2.getQ2(),
  193.                               q1.getQ3() - q2.getQ3());
  194.     }

  196.     /**
  197.      * Subtracts a quaternion from the instance.
  198.      *
  199.      * @param q Quaternion.
  200.      * @return the difference between this instance and {@code q}.
  201.      */
  202.     public Quaternion subtract(final Quaternion q) {
  203.         return subtract(this, q);
  204.     }

  206.     /**
  207.      * Computes the dot-product of two quaternions.
  208.      *
  209.      * @param q1 Quaternion.
  210.      * @param q2 Quaternion.
  211.      * @return the dot product of {@code q1} and {@code q2}.
  212.      */
  213.     public static double dotProduct(final Quaternion q1,
  214.                                     final Quaternion q2) {
  215.         return q1.getQ0() * q2.getQ0() +
  216.             q1.getQ1() * q2.getQ1() +
  217.             q1.getQ2() * q2.getQ2() +
  218.             q1.getQ3() * q2.getQ3();
  219.     }

  221.     /**
  222.      * Computes the dot-product of the instance by a quaternion.
  223.      *
  224.      * @param q Quaternion.
  225.      * @return the dot product of this instance and {@code q}.
  226.      */
  227.     public double dotProduct(final Quaternion q) {
  228.         return dotProduct(this, q);
  229.     }

  231.     /**
  232.      * Computes the norm of the quaternion.
  233.      *
  234.      * @return the norm.
  235.      */
  236.     public double getNorm() {
  237.         return FastMath.sqrt(q0 * q0 +
  238.                              q1 * q1 +
  239.                              q2 * q2 +
  240.                              q3 * q3);
  241.     }

  243.     /**
  244.      * Computes the normalized quaternion (the versor of the instance).
  245.      * The norm of the quaternion must not be zero.
  246.      *
  247.      * @return a normalized quaternion.
  248.      * @throws MathIllegalArgumentException if the norm of the quaternion is zero.
  249.      */
  250.     public Quaternion normalize() {
  251.         final double norm = getNorm();

  253.         if (norm < Precision.SAFE_MIN) {
  254.             throw new MathIllegalArgumentException(LocalizedCoreFormats.NORM, norm);
  255.         }

  257.         return new Quaternion(q0 / norm,
  258.                               q1 / norm,
  259.                               q2 / norm,
  260.                               q3 / norm);
  261.     }

  263.     /**
  264.      * {@inheritDoc}
  265.      */
  266.     @Override
  267.     public boolean equals(Object other) {
  268.         if (this == other) {
  269.             return true;
  270.         }
  271.         if (other instanceof Quaternion) {
  272.             final Quaternion q = (Quaternion) other;
  273.             return q0 == q.getQ0() &&
  274.                 q1 == q.getQ1() &&
  275.                 q2 == q.getQ2() &&
  276.                 q3 == q.getQ3();
  277.         }

  279.         return false;
  280.     }

  282.     /**
  283.      * {@inheritDoc}
  284.      */
  285.     @Override
  286.     public int hashCode() {
  287.         // "Effective Java" (second edition, p. 47).
  288.         int result = 17;
  289.         for (double comp : new double[] { q0, q1, q2, q3 }) {
  290.             final int c = MathUtils.hash(comp);
  291.             result = 31 * result + c;
  292.         }
  293.         return result;
  294.     }

  296.     /**
  297.      * Checks whether this instance is equal to another quaternion
  298.      * within a given tolerance.
  299.      *
  300.      * @param q Quaternion with which to compare the current quaternion.
  301.      * @param eps Tolerance.
  302.      * @return {@code true} if the each of the components are equal
  303.      * within the allowed absolute error.
  304.      */
  305.     public boolean equals(final Quaternion q,
  306.                           final double eps) {
  307.         return Precision.equals(q0, q.getQ0(), eps) &&
  308.             Precision.equals(q1, q.getQ1(), eps) &&
  309.             Precision.equals(q2, q.getQ2(), eps) &&
  310.             Precision.equals(q3, q.getQ3(), eps);
  311.     }

  313.     /**
  314.      * Checks whether the instance is a unit quaternion within a given
  315.      * tolerance.
  316.      *
  317.      * @param eps Tolerance (absolute error).
  318.      * @return {@code true} if the norm is 1 within the given tolerance,
  319.      * {@code false} otherwise
  320.      */
  321.     public boolean isUnitQuaternion(double eps) {
  322.         return Precision.equals(getNorm(), 1d, eps);
  323.     }

  325.     /**
  326.      * Checks whether the instance is a pure quaternion within a given
  327.      * tolerance.
  328.      *
  329.      * @param eps Tolerance (absolute error).
  330.      * @return {@code true} if the scalar part of the quaternion is zero.
  331.      */
  332.     public boolean isPureQuaternion(double eps) {
  333.         return FastMath.abs(getQ0()) <= eps;
  334.     }

  336.     /**
  337.      * Returns the polar form of the quaternion.
  338.      *
  339.      * @return the unit quaternion with positive scalar part.
  340.      */
  341.     public Quaternion getPositivePolarForm() {
  342.         if (getQ0() < 0) {
  343.             final Quaternion unitQ = normalize();
  344.             // The quaternion of rotation (normalized quaternion) q and -q
  345.             // are equivalent (i.e. represent the same rotation).
  346.             return new Quaternion(-unitQ.getQ0(),
  347.                                   -unitQ.getQ1(),
  348.                                   -unitQ.getQ2(),
  349.                                   -unitQ.getQ3());
  350.         } else {
  351.             return this.normalize();
  352.         }
  353.     }

  355.     /**
  356.      * Returns the inverse of this instance.
  357.      * The norm of the quaternion must not be zero.
  358.      *
  359.      * @return the inverse.
  360.      * @throws MathIllegalArgumentException if the norm (squared) of the quaternion is zero.
  361.      */
  362.     public Quaternion getInverse() {
  363.         final double squareNorm = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3;
  364.         if (squareNorm < Precision.SAFE_MIN) {
  365.             throw new MathIllegalArgumentException(LocalizedCoreFormats.NORM, squareNorm);
  366.         }

  368.         return new Quaternion(q0 / squareNorm,
  369.                               -q1 / squareNorm,
  370.                               -q2 / squareNorm,
  371.                               -q3 / squareNorm);
  372.     }

  374.     /**
  375.      * Gets the first component of the quaternion (scalar part).
  376.      *
  377.      * @return the scalar part.
  378.      */
  379.     public double getQ0() {
  380.         return q0;
  381.     }

  383.     /**
  384.      * Gets the second component of the quaternion (first component
  385.      * of the vector part).
  386.      *
  387.      * @return the first component of the vector part.
  388.      */
  389.     public double getQ1() {
  390.         return q1;
  391.     }

  393.     /**
  394.      * Gets the third component of the quaternion (second component
  395.      * of the vector part).
  396.      *
  397.      * @return the second component of the vector part.
  398.      */
  399.     public double getQ2() {
  400.         return q2;
  401.     }

  403.     /**
  404.      * Gets the fourth component of the quaternion (third component
  405.      * of the vector part).
  406.      *
  407.      * @return the third component of the vector part.
  408.      */
  409.     public double getQ3() {
  410.         return q3;
  411.     }

  413.     /**
  414.      * Gets the scalar part of the quaternion.
  415.      *
  416.      * @return the scalar part.
  417.      * @see #getQ0()
  418.      */
  419.     public double getScalarPart() {
  420.         return getQ0();
  421.     }

  423.     /**
  424.      * Gets the three components of the vector part of the quaternion.
  425.      *
  426.      * @return the vector part.
  427.      * @see #getQ1()
  428.      * @see #getQ2()
  429.      * @see #getQ3()
  430.      */
  431.     public double[] getVectorPart() {
  432.         return new double[] { getQ1(), getQ2(), getQ3() };
  433.     }

  435.     /**
  436.      * Multiplies the instance by a scalar.
  437.      *
  438.      * @param alpha Scalar factor.
  439.      * @return a scaled quaternion.
  440.      */
  441.     public Quaternion multiply(final double alpha) {
  442.         return new Quaternion(alpha * q0,
  443.                               alpha * q1,
  444.                               alpha * q2,
  445.                               alpha * q3);
  446.     }

  448.     /**
  449.      * {@inheritDoc}
  450.      */
  451.     @Override
  452.     public String toString() {
  453.         final String sp = " ";
  454.         final StringBuilder s = new StringBuilder();
  455.         s.append('[')
  456.             .append(q0).append(sp)
  457.             .append(q1).append(sp)
  458.             .append(q2).append(sp)
  459.             .append(q3)
  460.             .append(']');

  462.         return s.toString();
  463.     }
  464. }
Created with JaCoCo 0.8.12.202403310830