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