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    }