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 * http://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 package org.apache.commons.numbers.quaternion;
19
20 import java.util.Arrays;
21 import java.util.function.ToDoubleFunction;
22 import java.util.function.BiPredicate;
23 import java.io.Serializable;
24 import org.apache.commons.numbers.core.Precision;
25
26 /**
27 * This class implements <a href="http://mathworld.wolfram.com/Quaternion.html">
28 * quaternions</a> (Hamilton's hypercomplex numbers).
29 *
30 * <p>Wherever quaternion components are listed in sequence, this class follows the
31 * convention of placing the scalar ({@code w}) component first, e.g. [{@code w, x, y, z}].
32 * Other libraries and textbooks may place the {@code w} component last.</p>
33 *
34 * <p>Instances of this class are guaranteed to be immutable.</p>
35 */
36 public final class Quaternion implements Serializable {
37 /** Zero quaternion. */
38 public static final Quaternion ZERO = of(0, 0, 0, 0);
39 /** Identity quaternion. */
40 public static final Quaternion ONE = new Quaternion(Type.POSITIVE_POLAR_FORM, 1, 0, 0, 0);
41 /** i. */
42 public static final Quaternion I = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 1, 0, 0);
43 /** j. */
44 public static final Quaternion J = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 0, 1, 0);
45 /** k. */
46 public static final Quaternion K = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 0, 0, 1);
47
48 /** Serializable version identifier. */
49 private static final long serialVersionUID = 20170118L;
50 /** Error message. */
51 private static final String ILLEGAL_NORM_MSG = "Illegal norm: ";
52
53 /** {@link #toString() String representation}. */
54 private static final String FORMAT_START = "[";
55 /** {@link #toString() String representation}. */
56 private static final String FORMAT_END = "]";
57 /** {@link #toString() String representation}. */
58 private static final String FORMAT_SEP = " ";
59
60 /** The number of dimensions for the vector part of the quaternion. */
61 private static final int VECTOR_DIMENSIONS = 3;
62 /** The number of parts when parsing a text representation of the quaternion. */
63 private static final int NUMBER_OF_PARTS = 4;
64
65 /** For enabling specialized method implementations. */
66 private final Type type;
67 /** First component (scalar part). */
68 private final double w;
69 /** Second component (first vector part). */
70 private final double x;
71 /** Third component (second vector part). */
72 private final double y;
73 /** Fourth component (third vector part). */
74 private final double z;
75
76 /**
77 * For enabling optimized implementations.
78 */
79 private enum Type {
80 /** Default implementation. */
81 DEFAULT(Default.NORMSQ,
82 Default.NORM,
83 Default.IS_UNIT),
84 /** Quaternion has unit norm. */
85 NORMALIZED(Normalized.NORM,
86 Normalized.NORM,
87 Normalized.IS_UNIT),
88 /** Quaternion has positive scalar part. */
89 POSITIVE_POLAR_FORM(Normalized.NORM,
90 Normalized.NORM,
91 Normalized.IS_UNIT);
92
93 /** {@link Quaternion#normSq()}. */
94 private final ToDoubleFunction<Quaternion> normSq;
95 /** {@link Quaternion#norm()}. */
96 private final ToDoubleFunction<Quaternion> norm;
97 /** {@link Quaternion#isUnit(double)}. */
98 private final BiPredicate<Quaternion, Double> testIsUnit;
99
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(double)}. */
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 }