View Javadoc
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   */
17  
18  /*
19   * This is not the original file distributed by the Apache Software Foundation
20   * It has been modified by the Hipparchus project
21   */
22  
23  package org.hipparchus.complex;
24  
25  import java.io.Serializable;
26  
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;
32  
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);
50  
51      /** Serializable version identifier. */
52      private static final long serialVersionUID = 20092012L;
53  
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;
62  
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      }
80  
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      }
97  
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     }
107 
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     }
116 
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();
130 
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();
136 
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;
142 
143         return new Quaternion(w, x, y, z);
144     }
145 
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     }
155 
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     }
170 
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     }
180 
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     }
195 
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     }
205 
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     }
220 
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     }
230 
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     }
242 
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();
252 
253         if (norm < Precision.SAFE_MIN) {
254             throw new MathIllegalArgumentException(LocalizedCoreFormats.NORM, norm);
255         }
256 
257         return new Quaternion(q0 / norm,
258                               q1 / norm,
259                               q2 / norm,
260                               q3 / norm);
261     }
262 
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         }
278 
279         return false;
280     }
281 
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     }
295 
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     }
312 
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     }
324 
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     }
335 
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     }
354 
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         }
367 
368         return new Quaternion(q0 / squareNorm,
369                               -q1 / squareNorm,
370                               -q2 / squareNorm,
371                               -q3 / squareNorm);
372     }
373 
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     }
382 
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     }
392 
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     }
402 
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     }
412 
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     }
422 
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     }
434 
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     }
447 
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(']');
461 
462         return s.toString();
463     }
464 }