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 018package org.apache.commons.math3.geometry.euclidean.threed; 019 020import java.io.Serializable; 021 022import org.apache.commons.math3.exception.MathArithmeticException; 023import org.apache.commons.math3.exception.MathIllegalArgumentException; 024import org.apache.commons.math3.exception.util.LocalizedFormats; 025import org.apache.commons.math3.util.FastMath; 026import org.apache.commons.math3.util.MathArrays; 027 028/** 029 * This class implements rotations in a three-dimensional space. 030 * 031 * <p>Rotations can be represented by several different mathematical 032 * entities (matrices, axe and angle, Cardan or Euler angles, 033 * quaternions). This class presents an higher level abstraction, more 034 * user-oriented and hiding this implementation details. Well, for the 035 * curious, we use quaternions for the internal representation. The 036 * user can build a rotation from any of these representations, and 037 * any of these representations can be retrieved from a 038 * <code>Rotation</code> instance (see the various constructors and 039 * getters). In addition, a rotation can also be built implicitly 040 * from a set of vectors and their image.</p> 041 * <p>This implies that this class can be used to convert from one 042 * representation to another one. For example, converting a rotation 043 * matrix into a set of Cardan angles from can be done using the 044 * following single line of code:</p> 045 * <pre> 046 * double[] angles = new Rotation(matrix, 1.0e-10).getAngles(RotationOrder.XYZ); 047 * </pre> 048 * <p>Focus is oriented on what a rotation <em>do</em> rather than on its 049 * underlying representation. Once it has been built, and regardless of its 050 * internal representation, a rotation is an <em>operator</em> which basically 051 * transforms three dimensional {@link Vector3D vectors} into other three 052 * dimensional {@link Vector3D vectors}. Depending on the application, the 053 * meaning of these vectors may vary and the semantics of the rotation also.</p> 054 * <p>For example in an spacecraft attitude simulation tool, users will often 055 * consider the vectors are fixed (say the Earth direction for example) and the 056 * frames change. The rotation transforms the coordinates of the vector in inertial 057 * frame into the coordinates of the same vector in satellite frame. In this 058 * case, the rotation implicitly defines the relation between the two frames.</p> 059 * <p>Another example could be a telescope control application, where the rotation 060 * would transform the sighting direction at rest into the desired observing 061 * direction when the telescope is pointed towards an object of interest. In this 062 * case the rotation transforms the direction at rest in a topocentric frame 063 * into the sighting direction in the same topocentric frame. This implies in this 064 * case the frame is fixed and the vector moves.</p> 065 * <p>In many case, both approaches will be combined. In our telescope example, 066 * we will probably also need to transform the observing direction in the topocentric 067 * frame into the observing direction in inertial frame taking into account the observatory 068 * location and the Earth rotation, which would essentially be an application of the 069 * first approach.</p> 070 * 071 * <p>These examples show that a rotation is what the user wants it to be. This 072 * class does not push the user towards one specific definition and hence does not 073 * provide methods like <code>projectVectorIntoDestinationFrame</code> or 074 * <code>computeTransformedDirection</code>. It provides simpler and more generic 075 * methods: {@link #applyTo(Vector3D) applyTo(Vector3D)} and {@link 076 * #applyInverseTo(Vector3D) applyInverseTo(Vector3D)}.</p> 077 * 078 * <p>Since a rotation is basically a vectorial operator, several rotations can be 079 * composed together and the composite operation <code>r = r<sub>1</sub> o 080 * r<sub>2</sub></code> (which means that for each vector <code>u</code>, 081 * <code>r(u) = r<sub>1</sub>(r<sub>2</sub>(u))</code>) is also a rotation. Hence 082 * we can consider that in addition to vectors, a rotation can be applied to other 083 * rotations as well (or to itself). With our previous notations, we would say we 084 * can apply <code>r<sub>1</sub></code> to <code>r<sub>2</sub></code> and the result 085 * we get is <code>r = r<sub>1</sub> o r<sub>2</sub></code>. For this purpose, the 086 * class provides the methods: {@link #applyTo(Rotation) applyTo(Rotation)} and 087 * {@link #applyInverseTo(Rotation) applyInverseTo(Rotation)}.</p> 088 * 089 * <p>Rotations are guaranteed to be immutable objects.</p> 090 * 091 * @see Vector3D 092 * @see RotationOrder 093 * @since 1.2 094 */ 095 096public class Rotation implements Serializable { 097 098 /** Identity rotation. */ 099 public static final Rotation IDENTITY = new Rotation(1.0, 0.0, 0.0, 0.0, false); 100 101 /** Serializable version identifier */ 102 private static final long serialVersionUID = -2153622329907944313L; 103 104 /** Scalar coordinate of the quaternion. */ 105 private final double q0; 106 107 /** First coordinate of the vectorial part of the quaternion. */ 108 private final double q1; 109 110 /** Second coordinate of the vectorial part of the quaternion. */ 111 private final double q2; 112 113 /** Third coordinate of the vectorial part of the quaternion. */ 114 private final double q3; 115 116 /** Build a rotation from the quaternion coordinates. 117 * <p>A rotation can be built from a <em>normalized</em> quaternion, 118 * i.e. a quaternion for which q<sub>0</sub><sup>2</sup> + 119 * q<sub>1</sub><sup>2</sup> + q<sub>2</sub><sup>2</sup> + 120 * q<sub>3</sub><sup>2</sup> = 1. If the quaternion is not normalized, 121 * the constructor can normalize it in a preprocessing step.</p> 122 * <p>Note that some conventions put the scalar part of the quaternion 123 * as the 4<sup>th</sup> component and the vector part as the first three 124 * components. This is <em>not</em> our convention. We put the scalar part 125 * as the first component.</p> 126 * @param q0 scalar part of the quaternion 127 * @param q1 first coordinate of the vectorial part of the quaternion 128 * @param q2 second coordinate of the vectorial part of the quaternion 129 * @param q3 third coordinate of the vectorial part of the quaternion 130 * @param needsNormalization if true, the coordinates are considered 131 * not to be normalized, a normalization preprocessing step is performed 132 * before using them 133 */ 134 public Rotation(double q0, double q1, double q2, double q3, 135 boolean needsNormalization) { 136 137 if (needsNormalization) { 138 // normalization preprocessing 139 double inv = 1.0 / FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 140 q0 *= inv; 141 q1 *= inv; 142 q2 *= inv; 143 q3 *= inv; 144 } 145 146 this.q0 = q0; 147 this.q1 = q1; 148 this.q2 = q2; 149 this.q3 = q3; 150 151 } 152 153 /** Build a rotation from an axis and an angle. 154 * <p> 155 * Calling this constructor is equivalent to call 156 * {@link #Rotation(Vector3D, double, RotationConvention) 157 * new Rotation(axis, angle, RotationConvention.VECTOR_OPERATOR)} 158 * </p> 159 * @param axis axis around which to rotate 160 * @param angle rotation angle. 161 * @exception MathIllegalArgumentException if the axis norm is zero 162 * @deprecated as of 3.6, replaced with {@link #Rotation(Vector3D, double, RotationConvention)} 163 */ 164 @Deprecated 165 public Rotation(Vector3D axis, double angle) throws MathIllegalArgumentException { 166 this(axis, angle, RotationConvention.VECTOR_OPERATOR); 167 } 168 169 /** Build a rotation from an axis and an angle. 170 * @param axis axis around which to rotate 171 * @param angle rotation angle 172 * @param convention convention to use for the semantics of the angle 173 * @exception MathIllegalArgumentException if the axis norm is zero 174 * @since 3.6 175 */ 176 public Rotation(final Vector3D axis, final double angle, final RotationConvention convention) 177 throws MathIllegalArgumentException { 178 179 double norm = axis.getNorm(); 180 if (norm == 0) { 181 throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_AXIS); 182 } 183 184 double halfAngle = convention == RotationConvention.VECTOR_OPERATOR ? -0.5 * angle : +0.5 * angle; 185 double coeff = FastMath.sin(halfAngle) / norm; 186 187 q0 = FastMath.cos (halfAngle); 188 q1 = coeff * axis.getX(); 189 q2 = coeff * axis.getY(); 190 q3 = coeff * axis.getZ(); 191 192 } 193 194 /** Build a rotation from a 3X3 matrix. 195 196 * <p>Rotation matrices are orthogonal matrices, i.e. unit matrices 197 * (which are matrices for which m.m<sup>T</sup> = I) with real 198 * coefficients. The module of the determinant of unit matrices is 199 * 1, among the orthogonal 3X3 matrices, only the ones having a 200 * positive determinant (+1) are rotation matrices.</p> 201 202 * <p>When a rotation is defined by a matrix with truncated values 203 * (typically when it is extracted from a technical sheet where only 204 * four to five significant digits are available), the matrix is not 205 * orthogonal anymore. This constructor handles this case 206 * transparently by using a copy of the given matrix and applying a 207 * correction to the copy in order to perfect its orthogonality. If 208 * the Frobenius norm of the correction needed is above the given 209 * threshold, then the matrix is considered to be too far from a 210 * true rotation matrix and an exception is thrown.<p> 211 212 * @param m rotation matrix 213 * @param threshold convergence threshold for the iterative 214 * orthogonality correction (convergence is reached when the 215 * difference between two steps of the Frobenius norm of the 216 * correction is below this threshold) 217 218 * @exception NotARotationMatrixException if the matrix is not a 3X3 219 * matrix, or if it cannot be transformed into an orthogonal matrix 220 * with the given threshold, or if the determinant of the resulting 221 * orthogonal matrix is negative 222 223 */ 224 public Rotation(double[][] m, double threshold) 225 throws NotARotationMatrixException { 226 227 // dimension check 228 if ((m.length != 3) || (m[0].length != 3) || 229 (m[1].length != 3) || (m[2].length != 3)) { 230 throw new NotARotationMatrixException( 231 LocalizedFormats.ROTATION_MATRIX_DIMENSIONS, 232 m.length, m[0].length); 233 } 234 235 // compute a "close" orthogonal matrix 236 double[][] ort = orthogonalizeMatrix(m, threshold); 237 238 // check the sign of the determinant 239 double det = ort[0][0] * (ort[1][1] * ort[2][2] - ort[2][1] * ort[1][2]) - 240 ort[1][0] * (ort[0][1] * ort[2][2] - ort[2][1] * ort[0][2]) + 241 ort[2][0] * (ort[0][1] * ort[1][2] - ort[1][1] * ort[0][2]); 242 if (det < 0.0) { 243 throw new NotARotationMatrixException( 244 LocalizedFormats.CLOSEST_ORTHOGONAL_MATRIX_HAS_NEGATIVE_DETERMINANT, 245 det); 246 } 247 248 double[] quat = mat2quat(ort); 249 q0 = quat[0]; 250 q1 = quat[1]; 251 q2 = quat[2]; 252 q3 = quat[3]; 253 254 } 255 256 /** Build the rotation that transforms a pair of vectors into another pair. 257 258 * <p>Except for possible scale factors, if the instance were applied to 259 * the pair (u<sub>1</sub>, u<sub>2</sub>) it will produce the pair 260 * (v<sub>1</sub>, v<sub>2</sub>).</p> 261 262 * <p>If the angular separation between u<sub>1</sub> and u<sub>2</sub> is 263 * not the same as the angular separation between v<sub>1</sub> and 264 * v<sub>2</sub>, then a corrected v'<sub>2</sub> will be used rather than 265 * v<sub>2</sub>, the corrected vector will be in the (±v<sub>1</sub>, 266 * +v<sub>2</sub>) half-plane.</p> 267 268 * @param u1 first vector of the origin pair 269 * @param u2 second vector of the origin pair 270 * @param v1 desired image of u1 by the rotation 271 * @param v2 desired image of u2 by the rotation 272 * @exception MathArithmeticException if the norm of one of the vectors is zero, 273 * or if one of the pair is degenerated (i.e. the vectors of the pair are collinear) 274 */ 275 public Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2) 276 throws MathArithmeticException { 277 278 // build orthonormalized base from u1, u2 279 // this fails when vectors are null or collinear, which is forbidden to define a rotation 280 final Vector3D u3 = u1.crossProduct(u2).normalize(); 281 u2 = u3.crossProduct(u1).normalize(); 282 u1 = u1.normalize(); 283 284 // build an orthonormalized base from v1, v2 285 // this fails when vectors are null or collinear, which is forbidden to define a rotation 286 final Vector3D v3 = v1.crossProduct(v2).normalize(); 287 v2 = v3.crossProduct(v1).normalize(); 288 v1 = v1.normalize(); 289 290 // buid a matrix transforming the first base into the second one 291 final double[][] m = new double[][] { 292 { 293 MathArrays.linearCombination(u1.getX(), v1.getX(), u2.getX(), v2.getX(), u3.getX(), v3.getX()), 294 MathArrays.linearCombination(u1.getY(), v1.getX(), u2.getY(), v2.getX(), u3.getY(), v3.getX()), 295 MathArrays.linearCombination(u1.getZ(), v1.getX(), u2.getZ(), v2.getX(), u3.getZ(), v3.getX()) 296 }, 297 { 298 MathArrays.linearCombination(u1.getX(), v1.getY(), u2.getX(), v2.getY(), u3.getX(), v3.getY()), 299 MathArrays.linearCombination(u1.getY(), v1.getY(), u2.getY(), v2.getY(), u3.getY(), v3.getY()), 300 MathArrays.linearCombination(u1.getZ(), v1.getY(), u2.getZ(), v2.getY(), u3.getZ(), v3.getY()) 301 }, 302 { 303 MathArrays.linearCombination(u1.getX(), v1.getZ(), u2.getX(), v2.getZ(), u3.getX(), v3.getZ()), 304 MathArrays.linearCombination(u1.getY(), v1.getZ(), u2.getY(), v2.getZ(), u3.getY(), v3.getZ()), 305 MathArrays.linearCombination(u1.getZ(), v1.getZ(), u2.getZ(), v2.getZ(), u3.getZ(), v3.getZ()) 306 } 307 }; 308 309 double[] quat = mat2quat(m); 310 q0 = quat[0]; 311 q1 = quat[1]; 312 q2 = quat[2]; 313 q3 = quat[3]; 314 315 } 316 317 /** Build one of the rotations that transform one vector into another one. 318 319 * <p>Except for a possible scale factor, if the instance were 320 * applied to the vector u it will produce the vector v. There is an 321 * infinite number of such rotations, this constructor choose the 322 * one with the smallest associated angle (i.e. the one whose axis 323 * is orthogonal to the (u, v) plane). If u and v are collinear, an 324 * arbitrary rotation axis is chosen.</p> 325 326 * @param u origin vector 327 * @param v desired image of u by the rotation 328 * @exception MathArithmeticException if the norm of one of the vectors is zero 329 */ 330 public Rotation(Vector3D u, Vector3D v) throws MathArithmeticException { 331 332 double normProduct = u.getNorm() * v.getNorm(); 333 if (normProduct == 0) { 334 throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR); 335 } 336 337 double dot = u.dotProduct(v); 338 339 if (dot < ((2.0e-15 - 1.0) * normProduct)) { 340 // special case u = -v: we select a PI angle rotation around 341 // an arbitrary vector orthogonal to u 342 Vector3D w = u.orthogonal(); 343 q0 = 0.0; 344 q1 = -w.getX(); 345 q2 = -w.getY(); 346 q3 = -w.getZ(); 347 } else { 348 // general case: (u, v) defines a plane, we select 349 // the shortest possible rotation: axis orthogonal to this plane 350 q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct)); 351 double coeff = 1.0 / (2.0 * q0 * normProduct); 352 Vector3D q = v.crossProduct(u); 353 q1 = coeff * q.getX(); 354 q2 = coeff * q.getY(); 355 q3 = coeff * q.getZ(); 356 } 357 358 } 359 360 /** Build a rotation from three Cardan or Euler elementary rotations. 361 362 * <p> 363 * Calling this constructor is equivalent to call 364 * {@link #Rotation(RotationOrder, RotationConvention, double, double, double) 365 * new Rotation(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3)} 366 * </p> 367 368 * @param order order of rotations to use 369 * @param alpha1 angle of the first elementary rotation 370 * @param alpha2 angle of the second elementary rotation 371 * @param alpha3 angle of the third elementary rotation 372 * @deprecated as of 3.6, replaced with {@link 373 * #Rotation(RotationOrder, RotationConvention, double, double, double)} 374 */ 375 @Deprecated 376 public Rotation(RotationOrder order, 377 double alpha1, double alpha2, double alpha3) { 378 this(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3); 379 } 380 381 /** Build a rotation from three Cardan or Euler elementary rotations. 382 383 * <p>Cardan rotations are three successive rotations around the 384 * canonical axes X, Y and Z, each axis being used once. There are 385 * 6 such sets of rotations (XYZ, XZY, YXZ, YZX, ZXY and ZYX). Euler 386 * rotations are three successive rotations around the canonical 387 * axes X, Y and Z, the first and last rotations being around the 388 * same axis. There are 6 such sets of rotations (XYX, XZX, YXY, 389 * YZY, ZXZ and ZYZ), the most popular one being ZXZ.</p> 390 * <p>Beware that many people routinely use the term Euler angles even 391 * for what really are Cardan angles (this confusion is especially 392 * widespread in the aerospace business where Roll, Pitch and Yaw angles 393 * are often wrongly tagged as Euler angles).</p> 394 395 * @param order order of rotations to compose, from left to right 396 * (i.e. we will use {@code r1.compose(r2.compose(r3, convention), convention)}) 397 * @param convention convention to use for the semantics of the angle 398 * @param alpha1 angle of the first elementary rotation 399 * @param alpha2 angle of the second elementary rotation 400 * @param alpha3 angle of the third elementary rotation 401 * @since 3.6 402 */ 403 public Rotation(RotationOrder order, RotationConvention convention, 404 double alpha1, double alpha2, double alpha3) { 405 Rotation r1 = new Rotation(order.getA1(), alpha1, convention); 406 Rotation r2 = new Rotation(order.getA2(), alpha2, convention); 407 Rotation r3 = new Rotation(order.getA3(), alpha3, convention); 408 Rotation composed = r1.compose(r2.compose(r3, convention), convention); 409 q0 = composed.q0; 410 q1 = composed.q1; 411 q2 = composed.q2; 412 q3 = composed.q3; 413 } 414 415 /** Convert an orthogonal rotation matrix to a quaternion. 416 * @param ort orthogonal rotation matrix 417 * @return quaternion corresponding to the matrix 418 */ 419 private static double[] mat2quat(final double[][] ort) { 420 421 final double[] quat = new double[4]; 422 423 // There are different ways to compute the quaternions elements 424 // from the matrix. They all involve computing one element from 425 // the diagonal of the matrix, and computing the three other ones 426 // using a formula involving a division by the first element, 427 // which unfortunately can be zero. Since the norm of the 428 // quaternion is 1, we know at least one element has an absolute 429 // value greater or equal to 0.5, so it is always possible to 430 // select the right formula and avoid division by zero and even 431 // numerical inaccuracy. Checking the elements in turn and using 432 // the first one greater than 0.45 is safe (this leads to a simple 433 // test since qi = 0.45 implies 4 qi^2 - 1 = -0.19) 434 double s = ort[0][0] + ort[1][1] + ort[2][2]; 435 if (s > -0.19) { 436 // compute q0 and deduce q1, q2 and q3 437 quat[0] = 0.5 * FastMath.sqrt(s + 1.0); 438 double inv = 0.25 / quat[0]; 439 quat[1] = inv * (ort[1][2] - ort[2][1]); 440 quat[2] = inv * (ort[2][0] - ort[0][2]); 441 quat[3] = inv * (ort[0][1] - ort[1][0]); 442 } else { 443 s = ort[0][0] - ort[1][1] - ort[2][2]; 444 if (s > -0.19) { 445 // compute q1 and deduce q0, q2 and q3 446 quat[1] = 0.5 * FastMath.sqrt(s + 1.0); 447 double inv = 0.25 / quat[1]; 448 quat[0] = inv * (ort[1][2] - ort[2][1]); 449 quat[2] = inv * (ort[0][1] + ort[1][0]); 450 quat[3] = inv * (ort[0][2] + ort[2][0]); 451 } else { 452 s = ort[1][1] - ort[0][0] - ort[2][2]; 453 if (s > -0.19) { 454 // compute q2 and deduce q0, q1 and q3 455 quat[2] = 0.5 * FastMath.sqrt(s + 1.0); 456 double inv = 0.25 / quat[2]; 457 quat[0] = inv * (ort[2][0] - ort[0][2]); 458 quat[1] = inv * (ort[0][1] + ort[1][0]); 459 quat[3] = inv * (ort[2][1] + ort[1][2]); 460 } else { 461 // compute q3 and deduce q0, q1 and q2 462 s = ort[2][2] - ort[0][0] - ort[1][1]; 463 quat[3] = 0.5 * FastMath.sqrt(s + 1.0); 464 double inv = 0.25 / quat[3]; 465 quat[0] = inv * (ort[0][1] - ort[1][0]); 466 quat[1] = inv * (ort[0][2] + ort[2][0]); 467 quat[2] = inv * (ort[2][1] + ort[1][2]); 468 } 469 } 470 } 471 472 return quat; 473 474 } 475 476 /** Revert a rotation. 477 * Build a rotation which reverse the effect of another 478 * rotation. This means that if r(u) = v, then r.revert(v) = u. The 479 * instance is not changed. 480 * @return a new rotation whose effect is the reverse of the effect 481 * of the instance 482 */ 483 public Rotation revert() { 484 return new Rotation(-q0, q1, q2, q3, false); 485 } 486 487 /** Get the scalar coordinate of the quaternion. 488 * @return scalar coordinate of the quaternion 489 */ 490 public double getQ0() { 491 return q0; 492 } 493 494 /** Get the first coordinate of the vectorial part of the quaternion. 495 * @return first coordinate of the vectorial part of the quaternion 496 */ 497 public double getQ1() { 498 return q1; 499 } 500 501 /** Get the second coordinate of the vectorial part of the quaternion. 502 * @return second coordinate of the vectorial part of the quaternion 503 */ 504 public double getQ2() { 505 return q2; 506 } 507 508 /** Get the third coordinate of the vectorial part of the quaternion. 509 * @return third coordinate of the vectorial part of the quaternion 510 */ 511 public double getQ3() { 512 return q3; 513 } 514 515 /** Get the normalized axis of the rotation. 516 * <p> 517 * Calling this method is equivalent to call 518 * {@link #getAxis(RotationConvention) getAxis(RotationConvention.VECTOR_OPERATOR)} 519 * </p> 520 * @return normalized axis of the rotation 521 * @see #Rotation(Vector3D, double, RotationConvention) 522 * @deprecated as of 3.6, replaced with {@link #getAxis(RotationConvention)} 523 */ 524 @Deprecated 525 public Vector3D getAxis() { 526 return getAxis(RotationConvention.VECTOR_OPERATOR); 527 } 528 529 /** Get the normalized axis of the rotation. 530 * <p> 531 * Note that as {@link #getAngle()} always returns an angle 532 * between 0 and π, changing the convention changes the 533 * direction of the axis, not the sign of the angle. 534 * </p> 535 * @param convention convention to use for the semantics of the angle 536 * @return normalized axis of the rotation 537 * @see #Rotation(Vector3D, double, RotationConvention) 538 * @since 3.6 539 */ 540 public Vector3D getAxis(final RotationConvention convention) { 541 final double squaredSine = q1 * q1 + q2 * q2 + q3 * q3; 542 if (squaredSine == 0) { 543 return convention == RotationConvention.VECTOR_OPERATOR ? Vector3D.PLUS_I : Vector3D.MINUS_I; 544 } else { 545 final double sgn = convention == RotationConvention.VECTOR_OPERATOR ? +1 : -1; 546 if (q0 < 0) { 547 final double inverse = sgn / FastMath.sqrt(squaredSine); 548 return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); 549 } 550 final double inverse = -sgn / FastMath.sqrt(squaredSine); 551 return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); 552 } 553 } 554 555 /** Get the angle of the rotation. 556 * @return angle of the rotation (between 0 and π) 557 * @see #Rotation(Vector3D, double) 558 */ 559 public double getAngle() { 560 if ((q0 < -0.1) || (q0 > 0.1)) { 561 return 2 * FastMath.asin(FastMath.sqrt(q1 * q1 + q2 * q2 + q3 * q3)); 562 } else if (q0 < 0) { 563 return 2 * FastMath.acos(-q0); 564 } 565 return 2 * FastMath.acos(q0); 566 } 567 568 /** Get the Cardan or Euler angles corresponding to the instance. 569 570 * <p> 571 * Calling this method is equivalent to call 572 * {@link #getAngles(RotationOrder, RotationConvention) 573 * getAngles(order, RotationConvention.VECTOR_OPERATOR)} 574 * </p> 575 576 * @param order rotation order to use 577 * @return an array of three angles, in the order specified by the set 578 * @exception CardanEulerSingularityException if the rotation is 579 * singular with respect to the angles set specified 580 * @deprecated as of 3.6, replaced with {@link #getAngles(RotationOrder, RotationConvention)} 581 */ 582 @Deprecated 583 public double[] getAngles(RotationOrder order) 584 throws CardanEulerSingularityException { 585 return getAngles(order, RotationConvention.VECTOR_OPERATOR); 586 } 587 588 /** Get the Cardan or Euler angles corresponding to the instance. 589 590 * <p>The equations show that each rotation can be defined by two 591 * different values of the Cardan or Euler angles set. For example 592 * if Cardan angles are used, the rotation defined by the angles 593 * a<sub>1</sub>, a<sub>2</sub> and a<sub>3</sub> is the same as 594 * the rotation defined by the angles π + a<sub>1</sub>, π 595 * - a<sub>2</sub> and π + a<sub>3</sub>. This method implements 596 * the following arbitrary choices:</p> 597 * <ul> 598 * <li>for Cardan angles, the chosen set is the one for which the 599 * second angle is between -π/2 and π/2 (i.e its cosine is 600 * positive),</li> 601 * <li>for Euler angles, the chosen set is the one for which the 602 * second angle is between 0 and π (i.e its sine is positive).</li> 603 * </ul> 604 605 * <p>Cardan and Euler angle have a very disappointing drawback: all 606 * of them have singularities. This means that if the instance is 607 * too close to the singularities corresponding to the given 608 * rotation order, it will be impossible to retrieve the angles. For 609 * Cardan angles, this is often called gimbal lock. There is 610 * <em>nothing</em> to do to prevent this, it is an intrinsic problem 611 * with Cardan and Euler representation (but not a problem with the 612 * rotation itself, which is perfectly well defined). For Cardan 613 * angles, singularities occur when the second angle is close to 614 * -π/2 or +π/2, for Euler angle singularities occur when the 615 * second angle is close to 0 or π, this implies that the identity 616 * rotation is always singular for Euler angles!</p> 617 618 * @param order rotation order to use 619 * @param convention convention to use for the semantics of the angle 620 * @return an array of three angles, in the order specified by the set 621 * @exception CardanEulerSingularityException if the rotation is 622 * singular with respect to the angles set specified 623 * @since 3.6 624 */ 625 public double[] getAngles(RotationOrder order, RotationConvention convention) 626 throws CardanEulerSingularityException { 627 628 if (convention == RotationConvention.VECTOR_OPERATOR) { 629 if (order == RotationOrder.XYZ) { 630 631 // r (Vector3D.plusK) coordinates are : 632 // sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi) 633 // (-r) (Vector3D.plusI) coordinates are : 634 // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta) 635 // and we can choose to have theta in the interval [-PI/2 ; +PI/2] 636 Vector3D v1 = applyTo(Vector3D.PLUS_K); 637 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 638 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 639 throw new CardanEulerSingularityException(true); 640 } 641 return new double[] { 642 FastMath.atan2(-(v1.getY()), v1.getZ()), 643 FastMath.asin(v2.getZ()), 644 FastMath.atan2(-(v2.getY()), v2.getX()) 645 }; 646 647 } else if (order == RotationOrder.XZY) { 648 649 // r (Vector3D.plusJ) coordinates are : 650 // -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi) 651 // (-r) (Vector3D.plusI) coordinates are : 652 // cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi) 653 // and we can choose to have psi in the interval [-PI/2 ; +PI/2] 654 Vector3D v1 = applyTo(Vector3D.PLUS_J); 655 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 656 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 657 throw new CardanEulerSingularityException(true); 658 } 659 return new double[] { 660 FastMath.atan2(v1.getZ(), v1.getY()), 661 -FastMath.asin(v2.getY()), 662 FastMath.atan2(v2.getZ(), v2.getX()) 663 }; 664 665 } else if (order == RotationOrder.YXZ) { 666 667 // r (Vector3D.plusK) coordinates are : 668 // cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta) 669 // (-r) (Vector3D.plusJ) coordinates are : 670 // sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi) 671 // and we can choose to have phi in the interval [-PI/2 ; +PI/2] 672 Vector3D v1 = applyTo(Vector3D.PLUS_K); 673 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 674 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 675 throw new CardanEulerSingularityException(true); 676 } 677 return new double[] { 678 FastMath.atan2(v1.getX(), v1.getZ()), 679 -FastMath.asin(v2.getZ()), 680 FastMath.atan2(v2.getX(), v2.getY()) 681 }; 682 683 } else if (order == RotationOrder.YZX) { 684 685 // r (Vector3D.plusI) coordinates are : 686 // cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta) 687 // (-r) (Vector3D.plusJ) coordinates are : 688 // sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi) 689 // and we can choose to have psi in the interval [-PI/2 ; +PI/2] 690 Vector3D v1 = applyTo(Vector3D.PLUS_I); 691 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 692 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 693 throw new CardanEulerSingularityException(true); 694 } 695 return new double[] { 696 FastMath.atan2(-(v1.getZ()), v1.getX()), 697 FastMath.asin(v2.getX()), 698 FastMath.atan2(-(v2.getZ()), v2.getY()) 699 }; 700 701 } else if (order == RotationOrder.ZXY) { 702 703 // r (Vector3D.plusJ) coordinates are : 704 // -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi) 705 // (-r) (Vector3D.plusK) coordinates are : 706 // -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi) 707 // and we can choose to have phi in the interval [-PI/2 ; +PI/2] 708 Vector3D v1 = applyTo(Vector3D.PLUS_J); 709 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 710 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 711 throw new CardanEulerSingularityException(true); 712 } 713 return new double[] { 714 FastMath.atan2(-(v1.getX()), v1.getY()), 715 FastMath.asin(v2.getY()), 716 FastMath.atan2(-(v2.getX()), v2.getZ()) 717 }; 718 719 } else if (order == RotationOrder.ZYX) { 720 721 // r (Vector3D.plusI) coordinates are : 722 // cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta) 723 // (-r) (Vector3D.plusK) coordinates are : 724 // -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta) 725 // and we can choose to have theta in the interval [-PI/2 ; +PI/2] 726 Vector3D v1 = applyTo(Vector3D.PLUS_I); 727 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 728 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 729 throw new CardanEulerSingularityException(true); 730 } 731 return new double[] { 732 FastMath.atan2(v1.getY(), v1.getX()), 733 -FastMath.asin(v2.getX()), 734 FastMath.atan2(v2.getY(), v2.getZ()) 735 }; 736 737 } else if (order == RotationOrder.XYX) { 738 739 // r (Vector3D.plusI) coordinates are : 740 // cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta) 741 // (-r) (Vector3D.plusI) coordinates are : 742 // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2) 743 // and we can choose to have theta in the interval [0 ; PI] 744 Vector3D v1 = applyTo(Vector3D.PLUS_I); 745 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 746 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 747 throw new CardanEulerSingularityException(false); 748 } 749 return new double[] { 750 FastMath.atan2(v1.getY(), -v1.getZ()), 751 FastMath.acos(v2.getX()), 752 FastMath.atan2(v2.getY(), v2.getZ()) 753 }; 754 755 } else if (order == RotationOrder.XZX) { 756 757 // r (Vector3D.plusI) coordinates are : 758 // cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi) 759 // (-r) (Vector3D.plusI) coordinates are : 760 // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2) 761 // and we can choose to have psi in the interval [0 ; PI] 762 Vector3D v1 = applyTo(Vector3D.PLUS_I); 763 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 764 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 765 throw new CardanEulerSingularityException(false); 766 } 767 return new double[] { 768 FastMath.atan2(v1.getZ(), v1.getY()), 769 FastMath.acos(v2.getX()), 770 FastMath.atan2(v2.getZ(), -v2.getY()) 771 }; 772 773 } else if (order == RotationOrder.YXY) { 774 775 // r (Vector3D.plusJ) coordinates are : 776 // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) 777 // (-r) (Vector3D.plusJ) coordinates are : 778 // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) 779 // and we can choose to have phi in the interval [0 ; PI] 780 Vector3D v1 = applyTo(Vector3D.PLUS_J); 781 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 782 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 783 throw new CardanEulerSingularityException(false); 784 } 785 return new double[] { 786 FastMath.atan2(v1.getX(), v1.getZ()), 787 FastMath.acos(v2.getY()), 788 FastMath.atan2(v2.getX(), -v2.getZ()) 789 }; 790 791 } else if (order == RotationOrder.YZY) { 792 793 // r (Vector3D.plusJ) coordinates are : 794 // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) 795 // (-r) (Vector3D.plusJ) coordinates are : 796 // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) 797 // and we can choose to have psi in the interval [0 ; PI] 798 Vector3D v1 = applyTo(Vector3D.PLUS_J); 799 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 800 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 801 throw new CardanEulerSingularityException(false); 802 } 803 return new double[] { 804 FastMath.atan2(v1.getZ(), -v1.getX()), 805 FastMath.acos(v2.getY()), 806 FastMath.atan2(v2.getZ(), v2.getX()) 807 }; 808 809 } else if (order == RotationOrder.ZXZ) { 810 811 // r (Vector3D.plusK) coordinates are : 812 // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) 813 // (-r) (Vector3D.plusK) coordinates are : 814 // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) 815 // and we can choose to have phi in the interval [0 ; PI] 816 Vector3D v1 = applyTo(Vector3D.PLUS_K); 817 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 818 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 819 throw new CardanEulerSingularityException(false); 820 } 821 return new double[] { 822 FastMath.atan2(v1.getX(), -v1.getY()), 823 FastMath.acos(v2.getZ()), 824 FastMath.atan2(v2.getX(), v2.getY()) 825 }; 826 827 } else { // last possibility is ZYZ 828 829 // r (Vector3D.plusK) coordinates are : 830 // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) 831 // (-r) (Vector3D.plusK) coordinates are : 832 // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) 833 // and we can choose to have theta in the interval [0 ; PI] 834 Vector3D v1 = applyTo(Vector3D.PLUS_K); 835 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 836 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 837 throw new CardanEulerSingularityException(false); 838 } 839 return new double[] { 840 FastMath.atan2(v1.getY(), v1.getX()), 841 FastMath.acos(v2.getZ()), 842 FastMath.atan2(v2.getY(), -v2.getX()) 843 }; 844 845 } 846 } else { 847 if (order == RotationOrder.XYZ) { 848 849 // r (Vector3D.plusI) coordinates are : 850 // cos (theta) cos (psi), -cos (theta) sin (psi), sin (theta) 851 // (-r) (Vector3D.plusK) coordinates are : 852 // sin (theta), -sin (phi) cos (theta), cos (phi) cos (theta) 853 // and we can choose to have theta in the interval [-PI/2 ; +PI/2] 854 Vector3D v1 = applyTo(Vector3D.PLUS_I); 855 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 856 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 857 throw new CardanEulerSingularityException(true); 858 } 859 return new double[] { 860 FastMath.atan2(-v2.getY(), v2.getZ()), 861 FastMath.asin(v2.getX()), 862 FastMath.atan2(-v1.getY(), v1.getX()) 863 }; 864 865 } else if (order == RotationOrder.XZY) { 866 867 // r (Vector3D.plusI) coordinates are : 868 // cos (psi) cos (theta), -sin (psi), cos (psi) sin (theta) 869 // (-r) (Vector3D.plusJ) coordinates are : 870 // -sin (psi), cos (phi) cos (psi), sin (phi) cos (psi) 871 // and we can choose to have psi in the interval [-PI/2 ; +PI/2] 872 Vector3D v1 = applyTo(Vector3D.PLUS_I); 873 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 874 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 875 throw new CardanEulerSingularityException(true); 876 } 877 return new double[] { 878 FastMath.atan2(v2.getZ(), v2.getY()), 879 -FastMath.asin(v2.getX()), 880 FastMath.atan2(v1.getZ(), v1.getX()) 881 }; 882 883 } else if (order == RotationOrder.YXZ) { 884 885 // r (Vector3D.plusJ) coordinates are : 886 // cos (phi) sin (psi), cos (phi) cos (psi), -sin (phi) 887 // (-r) (Vector3D.plusK) coordinates are : 888 // sin (theta) cos (phi), -sin (phi), cos (theta) cos (phi) 889 // and we can choose to have phi in the interval [-PI/2 ; +PI/2] 890 Vector3D v1 = applyTo(Vector3D.PLUS_J); 891 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 892 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 893 throw new CardanEulerSingularityException(true); 894 } 895 return new double[] { 896 FastMath.atan2(v2.getX(), v2.getZ()), 897 -FastMath.asin(v2.getY()), 898 FastMath.atan2(v1.getX(), v1.getY()) 899 }; 900 901 } else if (order == RotationOrder.YZX) { 902 903 // r (Vector3D.plusJ) coordinates are : 904 // sin (psi), cos (psi) cos (phi), -cos (psi) sin (phi) 905 // (-r) (Vector3D.plusI) coordinates are : 906 // cos (theta) cos (psi), sin (psi), -sin (theta) cos (psi) 907 // and we can choose to have psi in the interval [-PI/2 ; +PI/2] 908 Vector3D v1 = applyTo(Vector3D.PLUS_J); 909 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 910 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 911 throw new CardanEulerSingularityException(true); 912 } 913 return new double[] { 914 FastMath.atan2(-v2.getZ(), v2.getX()), 915 FastMath.asin(v2.getY()), 916 FastMath.atan2(-v1.getZ(), v1.getY()) 917 }; 918 919 } else if (order == RotationOrder.ZXY) { 920 921 // r (Vector3D.plusK) coordinates are : 922 // -cos (phi) sin (theta), sin (phi), cos (phi) cos (theta) 923 // (-r) (Vector3D.plusJ) coordinates are : 924 // -sin (psi) cos (phi), cos (psi) cos (phi), sin (phi) 925 // and we can choose to have phi in the interval [-PI/2 ; +PI/2] 926 Vector3D v1 = applyTo(Vector3D.PLUS_K); 927 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 928 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 929 throw new CardanEulerSingularityException(true); 930 } 931 return new double[] { 932 FastMath.atan2(-v2.getX(), v2.getY()), 933 FastMath.asin(v2.getZ()), 934 FastMath.atan2(-v1.getX(), v1.getZ()) 935 }; 936 937 } else if (order == RotationOrder.ZYX) { 938 939 // r (Vector3D.plusK) coordinates are : 940 // -sin (theta), cos (theta) sin (phi), cos (theta) cos (phi) 941 // (-r) (Vector3D.plusI) coordinates are : 942 // cos (psi) cos (theta), sin (psi) cos (theta), -sin (theta) 943 // and we can choose to have theta in the interval [-PI/2 ; +PI/2] 944 Vector3D v1 = applyTo(Vector3D.PLUS_K); 945 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 946 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 947 throw new CardanEulerSingularityException(true); 948 } 949 return new double[] { 950 FastMath.atan2(v2.getY(), v2.getX()), 951 -FastMath.asin(v2.getZ()), 952 FastMath.atan2(v1.getY(), v1.getZ()) 953 }; 954 955 } else if (order == RotationOrder.XYX) { 956 957 // r (Vector3D.plusI) coordinates are : 958 // cos (theta), sin (phi2) sin (theta), cos (phi2) sin (theta) 959 // (-r) (Vector3D.plusI) coordinates are : 960 // cos (theta), sin (theta) sin (phi1), -sin (theta) cos (phi1) 961 // and we can choose to have theta in the interval [0 ; PI] 962 Vector3D v1 = applyTo(Vector3D.PLUS_I); 963 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 964 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 965 throw new CardanEulerSingularityException(false); 966 } 967 return new double[] { 968 FastMath.atan2(v2.getY(), -v2.getZ()), 969 FastMath.acos(v2.getX()), 970 FastMath.atan2(v1.getY(), v1.getZ()) 971 }; 972 973 } else if (order == RotationOrder.XZX) { 974 975 // r (Vector3D.plusI) coordinates are : 976 // cos (psi), -cos (phi2) sin (psi), sin (phi2) sin (psi) 977 // (-r) (Vector3D.plusI) coordinates are : 978 // cos (psi), sin (psi) cos (phi1), sin (psi) sin (phi1) 979 // and we can choose to have psi in the interval [0 ; PI] 980 Vector3D v1 = applyTo(Vector3D.PLUS_I); 981 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 982 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 983 throw new CardanEulerSingularityException(false); 984 } 985 return new double[] { 986 FastMath.atan2(v2.getZ(), v2.getY()), 987 FastMath.acos(v2.getX()), 988 FastMath.atan2(v1.getZ(), -v1.getY()) 989 }; 990 991 } else if (order == RotationOrder.YXY) { 992 993 // r (Vector3D.plusJ) coordinates are : 994 // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) 995 // (-r) (Vector3D.plusJ) coordinates are : 996 // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) 997 // and we can choose to have phi in the interval [0 ; PI] 998 Vector3D v1 = applyTo(Vector3D.PLUS_J); 999 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 1000 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 1001 throw new CardanEulerSingularityException(false); 1002 } 1003 return new double[] { 1004 FastMath.atan2(v2.getX(), v2.getZ()), 1005 FastMath.acos(v2.getY()), 1006 FastMath.atan2(v1.getX(), -v1.getZ()) 1007 }; 1008 1009 } else if (order == RotationOrder.YZY) { 1010 1011 // r (Vector3D.plusJ) coordinates are : 1012 // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) 1013 // (-r) (Vector3D.plusJ) coordinates are : 1014 // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) 1015 // and we can choose to have psi in the interval [0 ; PI] 1016 Vector3D v1 = applyTo(Vector3D.PLUS_J); 1017 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 1018 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 1019 throw new CardanEulerSingularityException(false); 1020 } 1021 return new double[] { 1022 FastMath.atan2(v2.getZ(), -v2.getX()), 1023 FastMath.acos(v2.getY()), 1024 FastMath.atan2(v1.getZ(), v1.getX()) 1025 }; 1026 1027 } else if (order == RotationOrder.ZXZ) { 1028 1029 // r (Vector3D.plusK) coordinates are : 1030 // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) 1031 // (-r) (Vector3D.plusK) coordinates are : 1032 // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) 1033 // and we can choose to have phi in the interval [0 ; PI] 1034 Vector3D v1 = applyTo(Vector3D.PLUS_K); 1035 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 1036 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 1037 throw new CardanEulerSingularityException(false); 1038 } 1039 return new double[] { 1040 FastMath.atan2(v2.getX(), -v2.getY()), 1041 FastMath.acos(v2.getZ()), 1042 FastMath.atan2(v1.getX(), v1.getY()) 1043 }; 1044 1045 } else { // last possibility is ZYZ 1046 1047 // r (Vector3D.plusK) coordinates are : 1048 // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) 1049 // (-r) (Vector3D.plusK) coordinates are : 1050 // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) 1051 // and we can choose to have theta in the interval [0 ; PI] 1052 Vector3D v1 = applyTo(Vector3D.PLUS_K); 1053 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 1054 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 1055 throw new CardanEulerSingularityException(false); 1056 } 1057 return new double[] { 1058 FastMath.atan2(v2.getY(), v2.getX()), 1059 FastMath.acos(v2.getZ()), 1060 FastMath.atan2(v1.getY(), -v1.getX()) 1061 }; 1062 1063 } 1064 } 1065 1066 } 1067 1068 /** Get the 3X3 matrix corresponding to the instance 1069 * @return the matrix corresponding to the instance 1070 */ 1071 public double[][] getMatrix() { 1072 1073 // products 1074 double q0q0 = q0 * q0; 1075 double q0q1 = q0 * q1; 1076 double q0q2 = q0 * q2; 1077 double q0q3 = q0 * q3; 1078 double q1q1 = q1 * q1; 1079 double q1q2 = q1 * q2; 1080 double q1q3 = q1 * q3; 1081 double q2q2 = q2 * q2; 1082 double q2q3 = q2 * q3; 1083 double q3q3 = q3 * q3; 1084 1085 // create the matrix 1086 double[][] m = new double[3][]; 1087 m[0] = new double[3]; 1088 m[1] = new double[3]; 1089 m[2] = new double[3]; 1090 1091 m [0][0] = 2.0 * (q0q0 + q1q1) - 1.0; 1092 m [1][0] = 2.0 * (q1q2 - q0q3); 1093 m [2][0] = 2.0 * (q1q3 + q0q2); 1094 1095 m [0][1] = 2.0 * (q1q2 + q0q3); 1096 m [1][1] = 2.0 * (q0q0 + q2q2) - 1.0; 1097 m [2][1] = 2.0 * (q2q3 - q0q1); 1098 1099 m [0][2] = 2.0 * (q1q3 - q0q2); 1100 m [1][2] = 2.0 * (q2q3 + q0q1); 1101 m [2][2] = 2.0 * (q0q0 + q3q3) - 1.0; 1102 1103 return m; 1104 1105 } 1106 1107 /** Apply the rotation to a vector. 1108 * @param u vector to apply the rotation to 1109 * @return a new vector which is the image of u by the rotation 1110 */ 1111 public Vector3D applyTo(Vector3D u) { 1112 1113 double x = u.getX(); 1114 double y = u.getY(); 1115 double z = u.getZ(); 1116 1117 double s = q1 * x + q2 * y + q3 * z; 1118 1119 return new Vector3D(2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x, 1120 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y, 1121 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z); 1122 1123 } 1124 1125 /** Apply the rotation to a vector stored in an array. 1126 * @param in an array with three items which stores vector to rotate 1127 * @param out an array with three items to put result to (it can be the same 1128 * array as in) 1129 */ 1130 public void applyTo(final double[] in, final double[] out) { 1131 1132 final double x = in[0]; 1133 final double y = in[1]; 1134 final double z = in[2]; 1135 1136 final double s = q1 * x + q2 * y + q3 * z; 1137 1138 out[0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x; 1139 out[1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y; 1140 out[2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z; 1141 1142 } 1143 1144 /** Apply the inverse of the rotation to a vector. 1145 * @param u vector to apply the inverse of the rotation to 1146 * @return a new vector which such that u is its image by the rotation 1147 */ 1148 public Vector3D applyInverseTo(Vector3D u) { 1149 1150 double x = u.getX(); 1151 double y = u.getY(); 1152 double z = u.getZ(); 1153 1154 double s = q1 * x + q2 * y + q3 * z; 1155 double m0 = -q0; 1156 1157 return new Vector3D(2 * (m0 * (x * m0 - (q2 * z - q3 * y)) + s * q1) - x, 1158 2 * (m0 * (y * m0 - (q3 * x - q1 * z)) + s * q2) - y, 1159 2 * (m0 * (z * m0 - (q1 * y - q2 * x)) + s * q3) - z); 1160 1161 } 1162 1163 /** Apply the inverse of the rotation to a vector stored in an array. 1164 * @param in an array with three items which stores vector to rotate 1165 * @param out an array with three items to put result to (it can be the same 1166 * array as in) 1167 */ 1168 public void applyInverseTo(final double[] in, final double[] out) { 1169 1170 final double x = in[0]; 1171 final double y = in[1]; 1172 final double z = in[2]; 1173 1174 final double s = q1 * x + q2 * y + q3 * z; 1175 final double m0 = -q0; 1176 1177 out[0] = 2 * (m0 * (x * m0 - (q2 * z - q3 * y)) + s * q1) - x; 1178 out[1] = 2 * (m0 * (y * m0 - (q3 * x - q1 * z)) + s * q2) - y; 1179 out[2] = 2 * (m0 * (z * m0 - (q1 * y - q2 * x)) + s * q3) - z; 1180 1181 } 1182 1183 /** Apply the instance to another rotation. 1184 * <p> 1185 * Calling this method is equivalent to call 1186 * {@link #compose(Rotation, RotationConvention) 1187 * compose(r, RotationConvention.VECTOR_OPERATOR)}. 1188 * </p> 1189 * @param r rotation to apply the rotation to 1190 * @return a new rotation which is the composition of r by the instance 1191 */ 1192 public Rotation applyTo(Rotation r) { 1193 return compose(r, RotationConvention.VECTOR_OPERATOR); 1194 } 1195 1196 /** Compose the instance with another rotation. 1197 * <p> 1198 * If the semantics of the rotations composition corresponds to a 1199 * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention, 1200 * applying the instance to a rotation is computing the composition 1201 * in an order compliant with the following rule : let {@code u} be any 1202 * vector and {@code v} its image by {@code r1} (i.e. 1203 * {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by 1204 * rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then 1205 * {@code w = comp.applyTo(u)}, where 1206 * {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}. 1207 * </p> 1208 * <p> 1209 * If the semantics of the rotations composition corresponds to a 1210 * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention, 1211 * the application order will be reversed. So keeping the exact same 1212 * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w} 1213 * and {@code comp} as above, {@code comp} could also be computed as 1214 * {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}. 1215 * </p> 1216 * @param r rotation to apply the rotation to 1217 * @param convention convention to use for the semantics of the angle 1218 * @return a new rotation which is the composition of r by the instance 1219 */ 1220 public Rotation compose(final Rotation r, final RotationConvention convention) { 1221 return convention == RotationConvention.VECTOR_OPERATOR ? 1222 composeInternal(r) : r.composeInternal(this); 1223 } 1224 1225 /** Compose the instance with another rotation using vector operator convention. 1226 * @param r rotation to apply the rotation to 1227 * @return a new rotation which is the composition of r by the instance 1228 * using vector operator convention 1229 */ 1230 private Rotation composeInternal(final Rotation r) { 1231 return new Rotation(r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3), 1232 r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2), 1233 r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3), 1234 r.q3 * q0 + r.q0 * q3 + (r.q1 * q2 - r.q2 * q1), 1235 false); 1236 } 1237 1238 /** Apply the inverse of the instance to another rotation. 1239 * <p> 1240 * Calling this method is equivalent to call 1241 * {@link #composeInverse(Rotation, RotationConvention) 1242 * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}. 1243 * </p> 1244 * @param r rotation to apply the rotation to 1245 * @return a new rotation which is the composition of r by the inverse 1246 * of the instance 1247 */ 1248 public Rotation applyInverseTo(Rotation r) { 1249 return composeInverse(r, RotationConvention.VECTOR_OPERATOR); 1250 } 1251 1252 /** Compose the inverse of the instance with another rotation. 1253 * <p> 1254 * If the semantics of the rotations composition corresponds to a 1255 * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention, 1256 * applying the inverse of the instance to a rotation is computing 1257 * the composition in an order compliant with the following rule : 1258 * let {@code u} be any vector and {@code v} its image by {@code r1} 1259 * (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image 1260 * of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}). 1261 * Then {@code w = comp.applyTo(u)}, where 1262 * {@code comp = r2.composeInverse(r1)}. 1263 * </p> 1264 * <p> 1265 * If the semantics of the rotations composition corresponds to a 1266 * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention, 1267 * the application order will be reversed, which means it is the 1268 * <em>innermost</em> rotation that will be reversed. So keeping the exact same 1269 * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w} 1270 * and {@code comp} as above, {@code comp} could also be computed as 1271 * {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}. 1272 * </p> 1273 * @param r rotation to apply the rotation to 1274 * @param convention convention to use for the semantics of the angle 1275 * @return a new rotation which is the composition of r by the inverse 1276 * of the instance 1277 */ 1278 public Rotation composeInverse(final Rotation r, final RotationConvention convention) { 1279 return convention == RotationConvention.VECTOR_OPERATOR ? 1280 composeInverseInternal(r) : r.composeInternal(revert()); 1281 } 1282 1283 /** Compose the inverse of the instance with another rotation 1284 * using vector operator convention. 1285 * @param r rotation to apply the rotation to 1286 * @return a new rotation which is the composition of r by the inverse 1287 * of the instance using vector operator convention 1288 */ 1289 private Rotation composeInverseInternal(Rotation r) { 1290 return new Rotation(-r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3), 1291 -r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2), 1292 -r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3), 1293 -r.q3 * q0 + r.q0 * q3 + (r.q1 * q2 - r.q2 * q1), 1294 false); 1295 } 1296 1297 /** Perfect orthogonality on a 3X3 matrix. 1298 * @param m initial matrix (not exactly orthogonal) 1299 * @param threshold convergence threshold for the iterative 1300 * orthogonality correction (convergence is reached when the 1301 * difference between two steps of the Frobenius norm of the 1302 * correction is below this threshold) 1303 * @return an orthogonal matrix close to m 1304 * @exception NotARotationMatrixException if the matrix cannot be 1305 * orthogonalized with the given threshold after 10 iterations 1306 */ 1307 private double[][] orthogonalizeMatrix(double[][] m, double threshold) 1308 throws NotARotationMatrixException { 1309 double[] m0 = m[0]; 1310 double[] m1 = m[1]; 1311 double[] m2 = m[2]; 1312 double x00 = m0[0]; 1313 double x01 = m0[1]; 1314 double x02 = m0[2]; 1315 double x10 = m1[0]; 1316 double x11 = m1[1]; 1317 double x12 = m1[2]; 1318 double x20 = m2[0]; 1319 double x21 = m2[1]; 1320 double x22 = m2[2]; 1321 double fn = 0; 1322 double fn1; 1323 1324 double[][] o = new double[3][3]; 1325 double[] o0 = o[0]; 1326 double[] o1 = o[1]; 1327 double[] o2 = o[2]; 1328 1329 // iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M) 1330 int i = 0; 1331 while (++i < 11) { 1332 1333 // Mt.Xn 1334 double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20; 1335 double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20; 1336 double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20; 1337 double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21; 1338 double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21; 1339 double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21; 1340 double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22; 1341 double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22; 1342 double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22; 1343 1344 // Xn+1 1345 o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]); 1346 o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]); 1347 o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]); 1348 o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]); 1349 o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]); 1350 o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]); 1351 o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]); 1352 o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]); 1353 o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]); 1354 1355 // correction on each elements 1356 double corr00 = o0[0] - m0[0]; 1357 double corr01 = o0[1] - m0[1]; 1358 double corr02 = o0[2] - m0[2]; 1359 double corr10 = o1[0] - m1[0]; 1360 double corr11 = o1[1] - m1[1]; 1361 double corr12 = o1[2] - m1[2]; 1362 double corr20 = o2[0] - m2[0]; 1363 double corr21 = o2[1] - m2[1]; 1364 double corr22 = o2[2] - m2[2]; 1365 1366 // Frobenius norm of the correction 1367 fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 + 1368 corr10 * corr10 + corr11 * corr11 + corr12 * corr12 + 1369 corr20 * corr20 + corr21 * corr21 + corr22 * corr22; 1370 1371 // convergence test 1372 if (FastMath.abs(fn1 - fn) <= threshold) { 1373 return o; 1374 } 1375 1376 // prepare next iteration 1377 x00 = o0[0]; 1378 x01 = o0[1]; 1379 x02 = o0[2]; 1380 x10 = o1[0]; 1381 x11 = o1[1]; 1382 x12 = o1[2]; 1383 x20 = o2[0]; 1384 x21 = o2[1]; 1385 x22 = o2[2]; 1386 fn = fn1; 1387 1388 } 1389 1390 // the algorithm did not converge after 10 iterations 1391 throw new NotARotationMatrixException( 1392 LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX, 1393 i - 1); 1394 } 1395 1396 /** Compute the <i>distance</i> between two rotations. 1397 * <p>The <i>distance</i> is intended here as a way to check if two 1398 * rotations are almost similar (i.e. they transform vectors the same way) 1399 * or very different. It is mathematically defined as the angle of 1400 * the rotation r that prepended to one of the rotations gives the other 1401 * one:</p> 1402 * <pre> 1403 * r<sub>1</sub>(r) = r<sub>2</sub> 1404 * </pre> 1405 * <p>This distance is an angle between 0 and π. Its value is the smallest 1406 * possible upper bound of the angle in radians between r<sub>1</sub>(v) 1407 * and r<sub>2</sub>(v) for all possible vectors v. This upper bound is 1408 * reached for some v. The distance is equal to 0 if and only if the two 1409 * rotations are identical.</p> 1410 * <p>Comparing two rotations should always be done using this value rather 1411 * than for example comparing the components of the quaternions. It is much 1412 * more stable, and has a geometric meaning. Also comparing quaternions 1413 * components is error prone since for example quaternions (0.36, 0.48, -0.48, -0.64) 1414 * and (-0.36, -0.48, 0.48, 0.64) represent exactly the same rotation despite 1415 * their components are different (they are exact opposites).</p> 1416 * @param r1 first rotation 1417 * @param r2 second rotation 1418 * @return <i>distance</i> between r1 and r2 1419 */ 1420 public static double distance(Rotation r1, Rotation r2) { 1421 return r1.composeInverseInternal(r2).getAngle(); 1422 } 1423 1424}