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