    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 &pi;/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 &theta; about the unit vector (x, y, z) is the same as the
    163       * rotation build from quaternion components { cos(-&theta;/2),
    164       * x * sin(-&theta;/2), y * sin(-&theta;/2), z * sin(-&theta;/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 &pi;)
    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 &pi; + a<sub>1</sub>, &pi;
    525       * - a<sub>2</sub> and &pi; + 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 -&pi;/2 and &pi;/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 &pi; (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       * -&pi;/2 or +&pi;/2, for Euler angle singularities occur when the
    545       * second angle is close to 0 or &pi;, 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 &pi;. 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    }