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 }