001 /* 002 * Licensed to the Apache Software Foundation (ASF) under one or more 003 * contributor license agreements. See the NOTICE file distributed with 004 * this work for additional information regarding copyright ownership. 005 * The ASF licenses this file to You under the Apache License, Version 2.0 006 * (the "License"); you may not use this file except in compliance with 007 * the License. You may obtain a copy of the License at 008 * 009 * http://www.apache.org/licenses/LICENSE-2.0 010 * 011 * Unless required by applicable law or agreed to in writing, software 012 * distributed under the License is distributed on an "AS IS" BASIS, 013 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 014 * See the License for the specific language governing permissions and 015 * limitations under the License. 016 */ 017 018 package org.apache.commons.math3.geometry.euclidean.threed; 019 020 import java.io.Serializable; 021 022 import org.apache.commons.math3.exception.MathArithmeticException; 023 import org.apache.commons.math3.exception.MathIllegalArgumentException; 024 import org.apache.commons.math3.exception.util.LocalizedFormats; 025 import org.apache.commons.math3.util.FastMath; 026 import 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 * @version $Id: Rotation.java 1416643 2012-12-03 19:37:14Z tn $ 092 * @see Vector3D 093 * @see RotationOrder 094 * @since 1.2 095 */ 096 097 public class Rotation implements Serializable { 098 099 /** Identity rotation. */ 100 public static final Rotation IDENTITY = new Rotation(1.0, 0.0, 0.0, 0.0, false); 101 102 /** Serializable version identifier */ 103 private static final long serialVersionUID = -2153622329907944313L; 104 105 /** Scalar coordinate of the quaternion. */ 106 private final double q0; 107 108 /** First coordinate of the vectorial part of the quaternion. */ 109 private final double q1; 110 111 /** Second coordinate of the vectorial part of the quaternion. */ 112 private final double q2; 113 114 /** Third coordinate of the vectorial part of the quaternion. */ 115 private final double q3; 116 117 /** Build a rotation from the quaternion coordinates. 118 * <p>A rotation can be built from a <em>normalized</em> quaternion, 119 * i.e. a quaternion for which q<sub>0</sub><sup>2</sup> + 120 * q<sub>1</sub><sup>2</sup> + q<sub>2</sub><sup>2</sup> + 121 * q<sub>3</sub><sup>2</sup> = 1. If the quaternion is not normalized, 122 * the constructor can normalize it in a preprocessing step.</p> 123 * <p>Note that some conventions put the scalar part of the quaternion 124 * as the 4<sup>th</sup> component and the vector part as the first three 125 * components. This is <em>not</em> our convention. We put the scalar part 126 * as the first component.</p> 127 * @param q0 scalar part of the quaternion 128 * @param q1 first coordinate of the vectorial part of the quaternion 129 * @param q2 second coordinate of the vectorial part of the quaternion 130 * @param q3 third coordinate of the vectorial part of the quaternion 131 * @param needsNormalization if true, the coordinates are considered 132 * not to be normalized, a normalization preprocessing step is performed 133 * before using them 134 */ 135 public Rotation(double q0, double q1, double q2, double q3, 136 boolean needsNormalization) { 137 138 if (needsNormalization) { 139 // normalization preprocessing 140 double inv = 1.0 / FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 141 q0 *= inv; 142 q1 *= inv; 143 q2 *= inv; 144 q3 *= inv; 145 } 146 147 this.q0 = q0; 148 this.q1 = q1; 149 this.q2 = q2; 150 this.q3 = q3; 151 152 } 153 154 /** Build a rotation from an axis and an angle. 155 * <p>We use the convention that angles are oriented according to 156 * the effect of the rotation on vectors around the axis. That means 157 * that if (i, j, k) is a direct frame and if we first provide +k as 158 * the axis and π/2 as the angle to this constructor, and then 159 * {@link #applyTo(Vector3D) apply} the instance to +i, we will get 160 * +j.</p> 161 * <p>Another way to represent our convention is to say that a rotation 162 * of angle θ about the unit vector (x, y, z) is the same as the 163 * rotation build from quaternion components { cos(-θ/2), 164 * x * sin(-θ/2), y * sin(-θ/2), z * sin(-θ/2) }. 165 * Note the minus sign on the angle!</p> 166 * <p>On the one hand this convention is consistent with a vectorial 167 * perspective (moving vectors in fixed frames), on the other hand it 168 * is different from conventions with a frame perspective (fixed vectors 169 * viewed from different frames) like the ones used for example in spacecraft 170 * attitude community or in the graphics community.</p> 171 * @param axis axis around which to rotate 172 * @param angle rotation angle. 173 * @exception MathIllegalArgumentException if the axis norm is zero 174 */ 175 public Rotation(Vector3D axis, double angle) throws MathIllegalArgumentException { 176 177 double norm = axis.getNorm(); 178 if (norm == 0) { 179 throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_AXIS); 180 } 181 182 double halfAngle = -0.5 * angle; 183 double coeff = FastMath.sin(halfAngle) / norm; 184 185 q0 = FastMath.cos (halfAngle); 186 q1 = coeff * axis.getX(); 187 q2 = coeff * axis.getY(); 188 q3 = coeff * axis.getZ(); 189 190 } 191 192 /** Build a rotation from a 3X3 matrix. 193 194 * <p>Rotation matrices are orthogonal matrices, i.e. unit matrices 195 * (which are matrices for which m.m<sup>T</sup> = I) with real 196 * coefficients. The module of the determinant of unit matrices is 197 * 1, among the orthogonal 3X3 matrices, only the ones having a 198 * positive determinant (+1) are rotation matrices.</p> 199 200 * <p>When a rotation is defined by a matrix with truncated values 201 * (typically when it is extracted from a technical sheet where only 202 * four to five significant digits are available), the matrix is not 203 * orthogonal anymore. This constructor handles this case 204 * transparently by using a copy of the given matrix and applying a 205 * correction to the copy in order to perfect its orthogonality. If 206 * the Frobenius norm of the correction needed is above the given 207 * threshold, then the matrix is considered to be too far from a 208 * true rotation matrix and an exception is thrown.<p> 209 210 * @param m rotation matrix 211 * @param threshold convergence threshold for the iterative 212 * orthogonality correction (convergence is reached when the 213 * difference between two steps of the Frobenius norm of the 214 * correction is below this threshold) 215 216 * @exception NotARotationMatrixException if the matrix is not a 3X3 217 * matrix, or if it cannot be transformed into an orthogonal matrix 218 * with the given threshold, or if the determinant of the resulting 219 * orthogonal matrix is negative 220 221 */ 222 public Rotation(double[][] m, double threshold) 223 throws NotARotationMatrixException { 224 225 // dimension check 226 if ((m.length != 3) || (m[0].length != 3) || 227 (m[1].length != 3) || (m[2].length != 3)) { 228 throw new NotARotationMatrixException( 229 LocalizedFormats.ROTATION_MATRIX_DIMENSIONS, 230 m.length, m[0].length); 231 } 232 233 // compute a "close" orthogonal matrix 234 double[][] ort = orthogonalizeMatrix(m, threshold); 235 236 // check the sign of the determinant 237 double det = ort[0][0] * (ort[1][1] * ort[2][2] - ort[2][1] * ort[1][2]) - 238 ort[1][0] * (ort[0][1] * ort[2][2] - ort[2][1] * ort[0][2]) + 239 ort[2][0] * (ort[0][1] * ort[1][2] - ort[1][1] * ort[0][2]); 240 if (det < 0.0) { 241 throw new NotARotationMatrixException( 242 LocalizedFormats.CLOSEST_ORTHOGONAL_MATRIX_HAS_NEGATIVE_DETERMINANT, 243 det); 244 } 245 246 double[] quat = mat2quat(ort); 247 q0 = quat[0]; 248 q1 = quat[1]; 249 q2 = quat[2]; 250 q3 = quat[3]; 251 252 } 253 254 /** Build the rotation that transforms a pair of vector into another pair. 255 256 * <p>Except for possible scale factors, if the instance were applied to 257 * the pair (u<sub>1</sub>, u<sub>2</sub>) it will produce the pair 258 * (v<sub>1</sub>, v<sub>2</sub>).</p> 259 260 * <p>If the angular separation between u<sub>1</sub> and u<sub>2</sub> is 261 * not the same as the angular separation between v<sub>1</sub> and 262 * v<sub>2</sub>, then a corrected v'<sub>2</sub> will be used rather than 263 * v<sub>2</sub>, the corrected vector will be in the (v<sub>1</sub>, 264 * v<sub>2</sub>) plane.</p> 265 266 * @param u1 first vector of the origin pair 267 * @param u2 second vector of the origin pair 268 * @param v1 desired image of u1 by the rotation 269 * @param v2 desired image of u2 by the rotation 270 * @exception MathArithmeticException if the norm of one of the vectors is zero, 271 * or if one of the pair is degenerated (i.e. the vectors of the pair are colinear) 272 */ 273 public Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2) 274 throws MathArithmeticException { 275 276 // build orthonormalized base from u1, u2 277 // this fails when vectors are null or colinear, which is forbidden to define a rotation 278 final Vector3D u3 = u1.crossProduct(u2).normalize(); 279 u2 = u3.crossProduct(u1).normalize(); 280 u1 = u1.normalize(); 281 282 // build an orthonormalized base from v1, v2 283 // this fails when vectors are null or colinear, which is forbidden to define a rotation 284 final Vector3D v3 = v1.crossProduct(v2).normalize(); 285 v2 = v3.crossProduct(v1).normalize(); 286 v1 = v1.normalize(); 287 288 // buid a matrix transforming the first base into the second one 289 final double[][] m = new double[][] { 290 { 291 MathArrays.linearCombination(u1.getX(), v1.getX(), u2.getX(), v2.getX(), u3.getX(), v3.getX()), 292 MathArrays.linearCombination(u1.getY(), v1.getX(), u2.getY(), v2.getX(), u3.getY(), v3.getX()), 293 MathArrays.linearCombination(u1.getZ(), v1.getX(), u2.getZ(), v2.getX(), u3.getZ(), v3.getX()) 294 }, 295 { 296 MathArrays.linearCombination(u1.getX(), v1.getY(), u2.getX(), v2.getY(), u3.getX(), v3.getY()), 297 MathArrays.linearCombination(u1.getY(), v1.getY(), u2.getY(), v2.getY(), u3.getY(), v3.getY()), 298 MathArrays.linearCombination(u1.getZ(), v1.getY(), u2.getZ(), v2.getY(), u3.getZ(), v3.getY()) 299 }, 300 { 301 MathArrays.linearCombination(u1.getX(), v1.getZ(), u2.getX(), v2.getZ(), u3.getX(), v3.getZ()), 302 MathArrays.linearCombination(u1.getY(), v1.getZ(), u2.getY(), v2.getZ(), u3.getY(), v3.getZ()), 303 MathArrays.linearCombination(u1.getZ(), v1.getZ(), u2.getZ(), v2.getZ(), u3.getZ(), v3.getZ()) 304 } 305 }; 306 307 double[] quat = mat2quat(m); 308 q0 = quat[0]; 309 q1 = quat[1]; 310 q2 = quat[2]; 311 q3 = quat[3]; 312 313 } 314 315 /** Build one of the rotations that transform one vector into another one. 316 317 * <p>Except for a possible scale factor, if the instance were 318 * applied to the vector u it will produce the vector v. There is an 319 * infinite number of such rotations, this constructor choose the 320 * one with the smallest associated angle (i.e. the one whose axis 321 * is orthogonal to the (u, v) plane). If u and v are colinear, an 322 * arbitrary rotation axis is chosen.</p> 323 324 * @param u origin vector 325 * @param v desired image of u by the rotation 326 * @exception MathArithmeticException if the norm of one of the vectors is zero 327 */ 328 public Rotation(Vector3D u, Vector3D v) throws MathArithmeticException { 329 330 double normProduct = u.getNorm() * v.getNorm(); 331 if (normProduct == 0) { 332 throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR); 333 } 334 335 double dot = u.dotProduct(v); 336 337 if (dot < ((2.0e-15 - 1.0) * normProduct)) { 338 // special case u = -v: we select a PI angle rotation around 339 // an arbitrary vector orthogonal to u 340 Vector3D w = u.orthogonal(); 341 q0 = 0.0; 342 q1 = -w.getX(); 343 q2 = -w.getY(); 344 q3 = -w.getZ(); 345 } else { 346 // general case: (u, v) defines a plane, we select 347 // the shortest possible rotation: axis orthogonal to this plane 348 q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct)); 349 double coeff = 1.0 / (2.0 * q0 * normProduct); 350 Vector3D q = v.crossProduct(u); 351 q1 = coeff * q.getX(); 352 q2 = coeff * q.getY(); 353 q3 = coeff * q.getZ(); 354 } 355 356 } 357 358 /** Build a rotation from three Cardan or Euler elementary rotations. 359 360 * <p>Cardan rotations are three successive rotations around the 361 * canonical axes X, Y and Z, each axis being used once. There are 362 * 6 such sets of rotations (XYZ, XZY, YXZ, YZX, ZXY and ZYX). Euler 363 * rotations are three successive rotations around the canonical 364 * axes X, Y and Z, the first and last rotations being around the 365 * same axis. There are 6 such sets of rotations (XYX, XZX, YXY, 366 * YZY, ZXZ and ZYZ), the most popular one being ZXZ.</p> 367 * <p>Beware that many people routinely use the term Euler angles even 368 * for what really are Cardan angles (this confusion is especially 369 * widespread in the aerospace business where Roll, Pitch and Yaw angles 370 * are often wrongly tagged as Euler angles).</p> 371 372 * @param order order of rotations to use 373 * @param alpha1 angle of the first elementary rotation 374 * @param alpha2 angle of the second elementary rotation 375 * @param alpha3 angle of the third elementary rotation 376 */ 377 public Rotation(RotationOrder order, 378 double alpha1, double alpha2, double alpha3) { 379 Rotation r1 = new Rotation(order.getA1(), alpha1); 380 Rotation r2 = new Rotation(order.getA2(), alpha2); 381 Rotation r3 = new Rotation(order.getA3(), alpha3); 382 Rotation composed = r1.applyTo(r2.applyTo(r3)); 383 q0 = composed.q0; 384 q1 = composed.q1; 385 q2 = composed.q2; 386 q3 = composed.q3; 387 } 388 389 /** Convert an orthogonal rotation matrix to a quaternion. 390 * @param ort orthogonal rotation matrix 391 * @return quaternion corresponding to the matrix 392 */ 393 private static double[] mat2quat(final double[][] ort) { 394 395 final double[] quat = new double[4]; 396 397 // There are different ways to compute the quaternions elements 398 // from the matrix. They all involve computing one element from 399 // the diagonal of the matrix, and computing the three other ones 400 // using a formula involving a division by the first element, 401 // which unfortunately can be zero. Since the norm of the 402 // quaternion is 1, we know at least one element has an absolute 403 // value greater or equal to 0.5, so it is always possible to 404 // select the right formula and avoid division by zero and even 405 // numerical inaccuracy. Checking the elements in turn and using 406 // the first one greater than 0.45 is safe (this leads to a simple 407 // test since qi = 0.45 implies 4 qi^2 - 1 = -0.19) 408 double s = ort[0][0] + ort[1][1] + ort[2][2]; 409 if (s > -0.19) { 410 // compute q0 and deduce q1, q2 and q3 411 quat[0] = 0.5 * FastMath.sqrt(s + 1.0); 412 double inv = 0.25 / quat[0]; 413 quat[1] = inv * (ort[1][2] - ort[2][1]); 414 quat[2] = inv * (ort[2][0] - ort[0][2]); 415 quat[3] = inv * (ort[0][1] - ort[1][0]); 416 } else { 417 s = ort[0][0] - ort[1][1] - ort[2][2]; 418 if (s > -0.19) { 419 // compute q1 and deduce q0, q2 and q3 420 quat[1] = 0.5 * FastMath.sqrt(s + 1.0); 421 double inv = 0.25 / quat[1]; 422 quat[0] = inv * (ort[1][2] - ort[2][1]); 423 quat[2] = inv * (ort[0][1] + ort[1][0]); 424 quat[3] = inv * (ort[0][2] + ort[2][0]); 425 } else { 426 s = ort[1][1] - ort[0][0] - ort[2][2]; 427 if (s > -0.19) { 428 // compute q2 and deduce q0, q1 and q3 429 quat[2] = 0.5 * FastMath.sqrt(s + 1.0); 430 double inv = 0.25 / quat[2]; 431 quat[0] = inv * (ort[2][0] - ort[0][2]); 432 quat[1] = inv * (ort[0][1] + ort[1][0]); 433 quat[3] = inv * (ort[2][1] + ort[1][2]); 434 } else { 435 // compute q3 and deduce q0, q1 and q2 436 s = ort[2][2] - ort[0][0] - ort[1][1]; 437 quat[3] = 0.5 * FastMath.sqrt(s + 1.0); 438 double inv = 0.25 / quat[3]; 439 quat[0] = inv * (ort[0][1] - ort[1][0]); 440 quat[1] = inv * (ort[0][2] + ort[2][0]); 441 quat[2] = inv * (ort[2][1] + ort[1][2]); 442 } 443 } 444 } 445 446 return quat; 447 448 } 449 450 /** Revert a rotation. 451 * Build a rotation which reverse the effect of another 452 * rotation. This means that if r(u) = v, then r.revert(v) = u. The 453 * instance is not changed. 454 * @return a new rotation whose effect is the reverse of the effect 455 * of the instance 456 */ 457 public Rotation revert() { 458 return new Rotation(-q0, q1, q2, q3, false); 459 } 460 461 /** Get the scalar coordinate of the quaternion. 462 * @return scalar coordinate of the quaternion 463 */ 464 public double getQ0() { 465 return q0; 466 } 467 468 /** Get the first coordinate of the vectorial part of the quaternion. 469 * @return first coordinate of the vectorial part of the quaternion 470 */ 471 public double getQ1() { 472 return q1; 473 } 474 475 /** Get the second coordinate of the vectorial part of the quaternion. 476 * @return second coordinate of the vectorial part of the quaternion 477 */ 478 public double getQ2() { 479 return q2; 480 } 481 482 /** Get the third coordinate of the vectorial part of the quaternion. 483 * @return third coordinate of the vectorial part of the quaternion 484 */ 485 public double getQ3() { 486 return q3; 487 } 488 489 /** Get the normalized axis of the rotation. 490 * @return normalized axis of the rotation 491 * @see #Rotation(Vector3D, double) 492 */ 493 public Vector3D getAxis() { 494 double squaredSine = q1 * q1 + q2 * q2 + q3 * q3; 495 if (squaredSine == 0) { 496 return new Vector3D(1, 0, 0); 497 } else if (q0 < 0) { 498 double inverse = 1 / FastMath.sqrt(squaredSine); 499 return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); 500 } 501 double inverse = -1 / FastMath.sqrt(squaredSine); 502 return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); 503 } 504 505 /** Get the angle of the rotation. 506 * @return angle of the rotation (between 0 and π) 507 * @see #Rotation(Vector3D, double) 508 */ 509 public double getAngle() { 510 if ((q0 < -0.1) || (q0 > 0.1)) { 511 return 2 * FastMath.asin(FastMath.sqrt(q1 * q1 + q2 * q2 + q3 * q3)); 512 } else if (q0 < 0) { 513 return 2 * FastMath.acos(-q0); 514 } 515 return 2 * FastMath.acos(q0); 516 } 517 518 /** Get the Cardan or Euler angles corresponding to the instance. 519 520 * <p>The equations show that each rotation can be defined by two 521 * different values of the Cardan or Euler angles set. For example 522 * if Cardan angles are used, the rotation defined by the angles 523 * a<sub>1</sub>, a<sub>2</sub> and a<sub>3</sub> is the same as 524 * the rotation defined by the angles π + a<sub>1</sub>, π 525 * - a<sub>2</sub> and π + a<sub>3</sub>. This method implements 526 * the following arbitrary choices:</p> 527 * <ul> 528 * <li>for Cardan angles, the chosen set is the one for which the 529 * second angle is between -π/2 and π/2 (i.e its cosine is 530 * positive),</li> 531 * <li>for Euler angles, the chosen set is the one for which the 532 * second angle is between 0 and π (i.e its sine is positive).</li> 533 * </ul> 534 535 * <p>Cardan and Euler angle have a very disappointing drawback: all 536 * of them have singularities. This means that if the instance is 537 * too close to the singularities corresponding to the given 538 * rotation order, it will be impossible to retrieve the angles. For 539 * Cardan angles, this is often called gimbal lock. There is 540 * <em>nothing</em> to do to prevent this, it is an intrinsic problem 541 * with Cardan and Euler representation (but not a problem with the 542 * rotation itself, which is perfectly well defined). For Cardan 543 * angles, singularities occur when the second angle is close to 544 * -π/2 or +π/2, for Euler angle singularities occur when the 545 * second angle is close to 0 or π, this implies that the identity 546 * rotation is always singular for Euler angles!</p> 547 548 * @param order rotation order to use 549 * @return an array of three angles, in the order specified by the set 550 * @exception CardanEulerSingularityException if the rotation is 551 * singular with respect to the angles set specified 552 */ 553 public double[] getAngles(RotationOrder order) 554 throws CardanEulerSingularityException { 555 556 if (order == RotationOrder.XYZ) { 557 558 // r (Vector3D.plusK) coordinates are : 559 // sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi) 560 // (-r) (Vector3D.plusI) coordinates are : 561 // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta) 562 // and we can choose to have theta in the interval [-PI/2 ; +PI/2] 563 Vector3D v1 = applyTo(Vector3D.PLUS_K); 564 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 565 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 566 throw new CardanEulerSingularityException(true); 567 } 568 return new double[] { 569 FastMath.atan2(-(v1.getY()), v1.getZ()), 570 FastMath.asin(v2.getZ()), 571 FastMath.atan2(-(v2.getY()), v2.getX()) 572 }; 573 574 } else if (order == RotationOrder.XZY) { 575 576 // r (Vector3D.plusJ) coordinates are : 577 // -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi) 578 // (-r) (Vector3D.plusI) coordinates are : 579 // cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi) 580 // and we can choose to have psi in the interval [-PI/2 ; +PI/2] 581 Vector3D v1 = applyTo(Vector3D.PLUS_J); 582 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 583 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 584 throw new CardanEulerSingularityException(true); 585 } 586 return new double[] { 587 FastMath.atan2(v1.getZ(), v1.getY()), 588 -FastMath.asin(v2.getY()), 589 FastMath.atan2(v2.getZ(), v2.getX()) 590 }; 591 592 } else if (order == RotationOrder.YXZ) { 593 594 // r (Vector3D.plusK) coordinates are : 595 // cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta) 596 // (-r) (Vector3D.plusJ) coordinates are : 597 // sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi) 598 // and we can choose to have phi in the interval [-PI/2 ; +PI/2] 599 Vector3D v1 = applyTo(Vector3D.PLUS_K); 600 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 601 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 602 throw new CardanEulerSingularityException(true); 603 } 604 return new double[] { 605 FastMath.atan2(v1.getX(), v1.getZ()), 606 -FastMath.asin(v2.getZ()), 607 FastMath.atan2(v2.getX(), v2.getY()) 608 }; 609 610 } else if (order == RotationOrder.YZX) { 611 612 // r (Vector3D.plusI) coordinates are : 613 // cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta) 614 // (-r) (Vector3D.plusJ) coordinates are : 615 // sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi) 616 // and we can choose to have psi in the interval [-PI/2 ; +PI/2] 617 Vector3D v1 = applyTo(Vector3D.PLUS_I); 618 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 619 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 620 throw new CardanEulerSingularityException(true); 621 } 622 return new double[] { 623 FastMath.atan2(-(v1.getZ()), v1.getX()), 624 FastMath.asin(v2.getX()), 625 FastMath.atan2(-(v2.getZ()), v2.getY()) 626 }; 627 628 } else if (order == RotationOrder.ZXY) { 629 630 // r (Vector3D.plusJ) coordinates are : 631 // -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi) 632 // (-r) (Vector3D.plusK) coordinates are : 633 // -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi) 634 // and we can choose to have phi in the interval [-PI/2 ; +PI/2] 635 Vector3D v1 = applyTo(Vector3D.PLUS_J); 636 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 637 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 638 throw new CardanEulerSingularityException(true); 639 } 640 return new double[] { 641 FastMath.atan2(-(v1.getX()), v1.getY()), 642 FastMath.asin(v2.getY()), 643 FastMath.atan2(-(v2.getX()), v2.getZ()) 644 }; 645 646 } else if (order == RotationOrder.ZYX) { 647 648 // r (Vector3D.plusI) coordinates are : 649 // cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta) 650 // (-r) (Vector3D.plusK) coordinates are : 651 // -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta) 652 // and we can choose to have theta in the interval [-PI/2 ; +PI/2] 653 Vector3D v1 = applyTo(Vector3D.PLUS_I); 654 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 655 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 656 throw new CardanEulerSingularityException(true); 657 } 658 return new double[] { 659 FastMath.atan2(v1.getY(), v1.getX()), 660 -FastMath.asin(v2.getX()), 661 FastMath.atan2(v2.getY(), v2.getZ()) 662 }; 663 664 } else if (order == RotationOrder.XYX) { 665 666 // r (Vector3D.plusI) coordinates are : 667 // cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta) 668 // (-r) (Vector3D.plusI) coordinates are : 669 // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2) 670 // and we can choose to have theta in the interval [0 ; PI] 671 Vector3D v1 = applyTo(Vector3D.PLUS_I); 672 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 673 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 674 throw new CardanEulerSingularityException(false); 675 } 676 return new double[] { 677 FastMath.atan2(v1.getY(), -v1.getZ()), 678 FastMath.acos(v2.getX()), 679 FastMath.atan2(v2.getY(), v2.getZ()) 680 }; 681 682 } else if (order == RotationOrder.XZX) { 683 684 // r (Vector3D.plusI) coordinates are : 685 // cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi) 686 // (-r) (Vector3D.plusI) coordinates are : 687 // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2) 688 // and we can choose to have psi in the interval [0 ; PI] 689 Vector3D v1 = applyTo(Vector3D.PLUS_I); 690 Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); 691 if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { 692 throw new CardanEulerSingularityException(false); 693 } 694 return new double[] { 695 FastMath.atan2(v1.getZ(), v1.getY()), 696 FastMath.acos(v2.getX()), 697 FastMath.atan2(v2.getZ(), -v2.getY()) 698 }; 699 700 } else if (order == RotationOrder.YXY) { 701 702 // r (Vector3D.plusJ) coordinates are : 703 // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) 704 // (-r) (Vector3D.plusJ) coordinates are : 705 // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) 706 // and we can choose to have phi in the interval [0 ; PI] 707 Vector3D v1 = applyTo(Vector3D.PLUS_J); 708 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 709 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 710 throw new CardanEulerSingularityException(false); 711 } 712 return new double[] { 713 FastMath.atan2(v1.getX(), v1.getZ()), 714 FastMath.acos(v2.getY()), 715 FastMath.atan2(v2.getX(), -v2.getZ()) 716 }; 717 718 } else if (order == RotationOrder.YZY) { 719 720 // r (Vector3D.plusJ) coordinates are : 721 // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) 722 // (-r) (Vector3D.plusJ) coordinates are : 723 // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) 724 // and we can choose to have psi in the interval [0 ; PI] 725 Vector3D v1 = applyTo(Vector3D.PLUS_J); 726 Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); 727 if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { 728 throw new CardanEulerSingularityException(false); 729 } 730 return new double[] { 731 FastMath.atan2(v1.getZ(), -v1.getX()), 732 FastMath.acos(v2.getY()), 733 FastMath.atan2(v2.getZ(), v2.getX()) 734 }; 735 736 } else if (order == RotationOrder.ZXZ) { 737 738 // r (Vector3D.plusK) coordinates are : 739 // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) 740 // (-r) (Vector3D.plusK) coordinates are : 741 // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) 742 // and we can choose to have phi in the interval [0 ; PI] 743 Vector3D v1 = applyTo(Vector3D.PLUS_K); 744 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 745 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 746 throw new CardanEulerSingularityException(false); 747 } 748 return new double[] { 749 FastMath.atan2(v1.getX(), -v1.getY()), 750 FastMath.acos(v2.getZ()), 751 FastMath.atan2(v2.getX(), v2.getY()) 752 }; 753 754 } else { // last possibility is ZYZ 755 756 // r (Vector3D.plusK) coordinates are : 757 // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) 758 // (-r) (Vector3D.plusK) coordinates are : 759 // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) 760 // and we can choose to have theta in the interval [0 ; PI] 761 Vector3D v1 = applyTo(Vector3D.PLUS_K); 762 Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); 763 if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { 764 throw new CardanEulerSingularityException(false); 765 } 766 return new double[] { 767 FastMath.atan2(v1.getY(), v1.getX()), 768 FastMath.acos(v2.getZ()), 769 FastMath.atan2(v2.getY(), -v2.getX()) 770 }; 771 772 } 773 774 } 775 776 /** Get the 3X3 matrix corresponding to the instance 777 * @return the matrix corresponding to the instance 778 */ 779 public double[][] getMatrix() { 780 781 // products 782 double q0q0 = q0 * q0; 783 double q0q1 = q0 * q1; 784 double q0q2 = q0 * q2; 785 double q0q3 = q0 * q3; 786 double q1q1 = q1 * q1; 787 double q1q2 = q1 * q2; 788 double q1q3 = q1 * q3; 789 double q2q2 = q2 * q2; 790 double q2q3 = q2 * q3; 791 double q3q3 = q3 * q3; 792 793 // create the matrix 794 double[][] m = new double[3][]; 795 m[0] = new double[3]; 796 m[1] = new double[3]; 797 m[2] = new double[3]; 798 799 m [0][0] = 2.0 * (q0q0 + q1q1) - 1.0; 800 m [1][0] = 2.0 * (q1q2 - q0q3); 801 m [2][0] = 2.0 * (q1q3 + q0q2); 802 803 m [0][1] = 2.0 * (q1q2 + q0q3); 804 m [1][1] = 2.0 * (q0q0 + q2q2) - 1.0; 805 m [2][1] = 2.0 * (q2q3 - q0q1); 806 807 m [0][2] = 2.0 * (q1q3 - q0q2); 808 m [1][2] = 2.0 * (q2q3 + q0q1); 809 m [2][2] = 2.0 * (q0q0 + q3q3) - 1.0; 810 811 return m; 812 813 } 814 815 /** Apply the rotation to a vector. 816 * @param u vector to apply the rotation to 817 * @return a new vector which is the image of u by the rotation 818 */ 819 public Vector3D applyTo(Vector3D u) { 820 821 double x = u.getX(); 822 double y = u.getY(); 823 double z = u.getZ(); 824 825 double s = q1 * x + q2 * y + q3 * z; 826 827 return new Vector3D(2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x, 828 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y, 829 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z); 830 831 } 832 833 /** Apply the rotation to a vector stored in an array. 834 * @param in an array with three items which stores vector to rotate 835 * @param out an array with three items to put result to (it can be the same 836 * array as in) 837 */ 838 public void applyTo(final double[] in, final double[] out) { 839 840 final double x = in[0]; 841 final double y = in[1]; 842 final double z = in[2]; 843 844 final double s = q1 * x + q2 * y + q3 * z; 845 846 out[0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x; 847 out[1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y; 848 out[2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z; 849 850 } 851 852 /** Apply the inverse of the rotation to a vector. 853 * @param u vector to apply the inverse of the rotation to 854 * @return a new vector which such that u is its image by the rotation 855 */ 856 public Vector3D applyInverseTo(Vector3D u) { 857 858 double x = u.getX(); 859 double y = u.getY(); 860 double z = u.getZ(); 861 862 double s = q1 * x + q2 * y + q3 * z; 863 double m0 = -q0; 864 865 return new Vector3D(2 * (m0 * (x * m0 - (q2 * z - q3 * y)) + s * q1) - x, 866 2 * (m0 * (y * m0 - (q3 * x - q1 * z)) + s * q2) - y, 867 2 * (m0 * (z * m0 - (q1 * y - q2 * x)) + s * q3) - z); 868 869 } 870 871 /** Apply the inverse of the rotation to a vector stored in an array. 872 * @param in an array with three items which stores vector to rotate 873 * @param out an array with three items to put result to (it can be the same 874 * array as in) 875 */ 876 public void applyInverseTo(final double[] in, final double[] out) { 877 878 final double x = in[0]; 879 final double y = in[1]; 880 final double z = in[2]; 881 882 final double s = q1 * x + q2 * y + q3 * z; 883 final double m0 = -q0; 884 885 out[0] = 2 * (m0 * (x * m0 - (q2 * z - q3 * y)) + s * q1) - x; 886 out[1] = 2 * (m0 * (y * m0 - (q3 * x - q1 * z)) + s * q2) - y; 887 out[2] = 2 * (m0 * (z * m0 - (q1 * y - q2 * x)) + s * q3) - z; 888 889 } 890 891 /** Apply the instance to another rotation. 892 * Applying the instance to a rotation is computing the composition 893 * in an order compliant with the following rule : let u be any 894 * vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image 895 * of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u), 896 * where comp = applyTo(r). 897 * @param r rotation to apply the rotation to 898 * @return a new rotation which is the composition of r by the instance 899 */ 900 public Rotation applyTo(Rotation r) { 901 return new Rotation(r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3), 902 r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2), 903 r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3), 904 r.q3 * q0 + r.q0 * q3 + (r.q1 * q2 - r.q2 * q1), 905 false); 906 } 907 908 /** Apply the inverse of the instance to another rotation. 909 * Applying the inverse of the instance to a rotation is computing 910 * the composition in an order compliant with the following rule : 911 * let u be any vector and v its image by r (i.e. r.applyTo(u) = v), 912 * let w be the inverse image of v by the instance 913 * (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where 914 * comp = applyInverseTo(r). 915 * @param r rotation to apply the rotation to 916 * @return a new rotation which is the composition of r by the inverse 917 * of the instance 918 */ 919 public Rotation applyInverseTo(Rotation r) { 920 return new Rotation(-r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3), 921 -r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2), 922 -r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3), 923 -r.q3 * q0 + r.q0 * q3 + (r.q1 * q2 - r.q2 * q1), 924 false); 925 } 926 927 /** Perfect orthogonality on a 3X3 matrix. 928 * @param m initial matrix (not exactly orthogonal) 929 * @param threshold convergence threshold for the iterative 930 * orthogonality correction (convergence is reached when the 931 * difference between two steps of the Frobenius norm of the 932 * correction is below this threshold) 933 * @return an orthogonal matrix close to m 934 * @exception NotARotationMatrixException if the matrix cannot be 935 * orthogonalized with the given threshold after 10 iterations 936 */ 937 private double[][] orthogonalizeMatrix(double[][] m, double threshold) 938 throws NotARotationMatrixException { 939 double[] m0 = m[0]; 940 double[] m1 = m[1]; 941 double[] m2 = m[2]; 942 double x00 = m0[0]; 943 double x01 = m0[1]; 944 double x02 = m0[2]; 945 double x10 = m1[0]; 946 double x11 = m1[1]; 947 double x12 = m1[2]; 948 double x20 = m2[0]; 949 double x21 = m2[1]; 950 double x22 = m2[2]; 951 double fn = 0; 952 double fn1; 953 954 double[][] o = new double[3][3]; 955 double[] o0 = o[0]; 956 double[] o1 = o[1]; 957 double[] o2 = o[2]; 958 959 // iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M) 960 int i = 0; 961 while (++i < 11) { 962 963 // Mt.Xn 964 double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20; 965 double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20; 966 double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20; 967 double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21; 968 double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21; 969 double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21; 970 double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22; 971 double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22; 972 double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22; 973 974 // Xn+1 975 o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]); 976 o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]); 977 o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]); 978 o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]); 979 o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]); 980 o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]); 981 o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]); 982 o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]); 983 o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]); 984 985 // correction on each elements 986 double corr00 = o0[0] - m0[0]; 987 double corr01 = o0[1] - m0[1]; 988 double corr02 = o0[2] - m0[2]; 989 double corr10 = o1[0] - m1[0]; 990 double corr11 = o1[1] - m1[1]; 991 double corr12 = o1[2] - m1[2]; 992 double corr20 = o2[0] - m2[0]; 993 double corr21 = o2[1] - m2[1]; 994 double corr22 = o2[2] - m2[2]; 995 996 // Frobenius norm of the correction 997 fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 + 998 corr10 * corr10 + corr11 * corr11 + corr12 * corr12 + 999 corr20 * corr20 + corr21 * corr21 + corr22 * corr22; 1000 1001 // convergence test 1002 if (FastMath.abs(fn1 - fn) <= threshold) { 1003 return o; 1004 } 1005 1006 // prepare next iteration 1007 x00 = o0[0]; 1008 x01 = o0[1]; 1009 x02 = o0[2]; 1010 x10 = o1[0]; 1011 x11 = o1[1]; 1012 x12 = o1[2]; 1013 x20 = o2[0]; 1014 x21 = o2[1]; 1015 x22 = o2[2]; 1016 fn = fn1; 1017 1018 } 1019 1020 // the algorithm did not converge after 10 iterations 1021 throw new NotARotationMatrixException( 1022 LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX, 1023 i - 1); 1024 } 1025 1026 /** Compute the <i>distance</i> between two rotations. 1027 * <p>The <i>distance</i> is intended here as a way to check if two 1028 * rotations are almost similar (i.e. they transform vectors the same way) 1029 * or very different. It is mathematically defined as the angle of 1030 * the rotation r that prepended to one of the rotations gives the other 1031 * one:</p> 1032 * <pre> 1033 * r<sub>1</sub>(r) = r<sub>2</sub> 1034 * </pre> 1035 * <p>This distance is an angle between 0 and π. Its value is the smallest 1036 * possible upper bound of the angle in radians between r<sub>1</sub>(v) 1037 * and r<sub>2</sub>(v) for all possible vectors v. This upper bound is 1038 * reached for some v. The distance is equal to 0 if and only if the two 1039 * rotations are identical.</p> 1040 * <p>Comparing two rotations should always be done using this value rather 1041 * than for example comparing the components of the quaternions. It is much 1042 * more stable, and has a geometric meaning. Also comparing quaternions 1043 * components is error prone since for example quaternions (0.36, 0.48, -0.48, -0.64) 1044 * and (-0.36, -0.48, 0.48, 0.64) represent exactly the same rotation despite 1045 * their components are different (they are exact opposites).</p> 1046 * @param r1 first rotation 1047 * @param r2 second rotation 1048 * @return <i>distance</i> between r1 and r2 1049 */ 1050 public static double distance(Rotation r1, Rotation r2) { 1051 return r1.applyInverseTo(r2).getAngle(); 1052 } 1053 1054 }