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 }