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 }