Add renormalization and rotation extraction from general 2D and 3D matrices. (#549)

* Add From impls to convert between UnitQuaterion/UnitComplex and Rotation2/3

* Add rotation extraction from a Matrix2 or Matrix3.

* Add fast Taylor renormalization for Unit.

Fix 376.

* Add renormalization for Rotation3.

Renormalization for Rotation2 requires Complex to implement VectorSpace which will be fixed only on alga v0.9

* Update Changelog.

CHANGELOG.md

@@ -4,7 +4,16 @@ documented here.
 
 This project adheres to [Semantic Versioning](http://semver.org/).
 
-## [0.17.0] - WIP
+## [0.18.0] - WIP
+
+### Added
+  * Add `.renormalize` to `Unit<...>` and `Rotation3` to correct potential drift due to repeated operations.
+    Those drifts can cause them not to be pure rotations anymore.
+  * Add the `::from_matrix` constructor too all rotation types to extract a rotation from a raw matrix.
+  * Add the `::from_matrix_eps` constructor too all rotation types to extract a rotation from a raw matrix. This takes
+    more argument than `::from_matrix` to control the convergence of the underlying optimization algorithm.
+
+## [0.17.0]
 
 ### Added
   * Add swizzling up to dimension 3 for vectors. For example, you can do `v.zxy()` as an equivalent to `Vector3::new(v.z, v.x, v.y)`.

src/base/unit.rs

@@ -13,6 +13,8 @@ use abomonation::Abomonation;
 use alga::general::SubsetOf;
 use alga::linear::NormedSpace;
 
+use ::Real;
+
 /// A wrapper that ensures the underlying algebraic entity has a unit norm.
 ///
 /// Use `.as_ref()` or `.into_inner()` to obtain the underlying value by-reference or by-move.
@@ -91,11 +93,24 @@ impl<T: NormedSpace> Unit<T> {
     /// Normalizes this value again. This is useful when repeated computations
     /// might cause a drift in the norm because of float inaccuracies.
     ///
-    /// Returns the norm before re-normalization (should be close to `1.0`).
+    /// Returns the norm before re-normalization. See `.renormalize_fast` for a faster alternative
+    /// that may be slightly less accurate if `self` drifted significantly from having a unit length.
     #[inline]
     pub fn renormalize(&mut self) -> T::Field {
         self.value.normalize_mut()
     }
+
+    /// Normalizes this value again using a first-order Taylor approximation.
+    /// This is useful when repeated computations might cause a drift in the norm
+    /// because of float inaccuracies.
+    #[inline]
+    pub fn renormalize_fast(&mut self)
+        where T::Field: Real {
+        let sq_norm = self.value.norm_squared();
+        let _3: T::Field = ::convert(3.0);
+        let _0_5: T::Field = ::convert(0.5);
+        self.value *= _0_5 * (_3 - sq_norm);
+    }
 }
 
 impl<T> Unit<T> {

src/geometry/quaternion_construction.rs

@@ -15,9 +15,9 @@ use base::dimension::U3;
 use base::storage::Storage;
 #[cfg(feature = "arbitrary")]
 use base::Vector3;
-use base::{Unit, Vector, Vector4};
+use base::{Unit, Vector, Vector4, Matrix3};
 
-use geometry::{Quaternion, Rotation, UnitQuaternion};
+use geometry::{Quaternion, Rotation3, UnitQuaternion};
 
 impl<N: Real> Quaternion<N> {
     /// Creates a quaternion from a 4D vector. The quaternion scalar part corresponds to the `w`
@@ -245,7 +245,7 @@ impl<N: Real> UnitQuaternion<N> {
     /// assert_relative_eq!(q.angle(), rot.angle(), epsilon = 1.0e-6);
     /// ```
     #[inline]
-    pub fn from_rotation_matrix(rotmat: &Rotation<N, U3>) -> Self {
+    pub fn from_rotation_matrix(rotmat: &Rotation3<N>) -> Self {
         // Robust matrix to quaternion transformation.
         // See http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion
         let tr = rotmat[(0, 0)] + rotmat[(1, 1)] + rotmat[(2, 2)];
@@ -293,6 +293,32 @@ impl<N: Real> UnitQuaternion<N> {
         Self::new_unchecked(res)
     }
 
+    /// Builds an unit quaternion by extracting the rotation part of the given transformation `m`.
+    ///
+    /// This is an iterative method. See `.from_matrix_eps` to provide mover
+    /// convergence parameters and starting solution.
+    /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
+    pub fn from_matrix(m: &Matrix3<N>) -> Self {
+        Rotation3::from_matrix(m).into()
+    }
+
+    /// Builds an unit quaternion by extracting the rotation part of the given transformation `m`.
+    ///
+    /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
+    ///
+    /// # Parameters
+    ///
+    /// * `m`: the matrix from which the rotational part is to be extracted.
+    /// * `eps`: the angular errors tolerated between the current rotation and the optimal one.
+    /// * `max_iter`: the maximum number of iterations. Loops indefinitely until convergence if set to `0`.
+    /// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close
+    ///           to the actual solution is provided. Can be set to `UnitQuaternion::identity()` if no other
+    ///           guesses come to mind.
+    pub fn from_matrix_eps(m: &Matrix3<N>, eps: N, max_iter: usize, guess: Self) -> Self {
+        let guess = Rotation3::from(guess);
+        Rotation3::from_matrix_eps(m, eps, max_iter, guess).into()
+    }
+
     /// The unit quaternion needed to make `a` and `b` be collinear and point toward the same
     /// direction.
     ///
@@ -453,7 +479,7 @@ impl<N: Real> UnitQuaternion<N> {
         SB: Storage<N, U3>,
         SC: Storage<N, U3>,
     {
-        Self::from_rotation_matrix(&Rotation::<N, U3>::face_towards(dir, up))
+        Self::from_rotation_matrix(&Rotation3::face_towards(dir, up))
     }
 
     /// Deprecated: Use [UnitQuaternion::face_towards] instead.

src/geometry/quaternion_conversion.rs

@@ -225,6 +225,20 @@ impl<N: Real> From<UnitQuaternion<N>> for Matrix4<N> {
     }
 }
 
+impl<N: Real> From<UnitQuaternion<N>> for Rotation3<N> {
+    #[inline]
+    fn from(q: UnitQuaternion<N>) -> Self {
+        q.to_rotation_matrix()
+    }
+}
+
+impl<N: Real> From<Rotation3<N>> for UnitQuaternion<N> {
+    #[inline]
+    fn from(q: Rotation3<N>) -> Self {
+        Self::from_rotation_matrix(&q)
+    }
+}
+
 impl<N: Real> From<UnitQuaternion<N>> for Matrix3<N> {
     #[inline]
     fn from(q: UnitQuaternion<N>) -> Self {

src/geometry/rotation_specialization.rs

@@ -11,9 +11,9 @@ use std::ops::Neg;
 
 use base::dimension::{U1, U2, U3};
 use base::storage::Storage;
-use base::{MatrixN, Unit, Vector, Vector1, Vector3, VectorN};
+use base::{Matrix2, Matrix3, MatrixN, Unit, Vector, Vector1, Vector3, VectorN};
 
-use geometry::{Rotation2, Rotation3, UnitComplex};
+use geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion};
 
 /*
  *
@@ -35,7 +35,7 @@ impl<N: Real> Rotation2<N> {
     /// ```
     pub fn new(angle: N) -> Self {
         let (sia, coa) = angle.sin_cos();
-        Self::from_matrix_unchecked(MatrixN::<N, U2>::new(coa, -sia, sia, coa))
+        Self::from_matrix_unchecked(Matrix2::new(coa, -sia, sia, coa))
     }
 
     /// Builds a 2 dimensional rotation matrix from an angle in radian wrapped in a 1-dimensional vector.
@@ -48,6 +48,51 @@ impl<N: Real> Rotation2<N> {
         Self::new(axisangle[0])
     }
 
+    /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
+    ///
+    /// This is an iterative method. See `.from_matrix_eps` to provide mover
+    /// convergence parameters and starting solution.
+    /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
+    pub fn from_matrix(m: &Matrix2<N>) -> Self {
+        Self::from_matrix_eps(m, N::default_epsilon(), 0, Self::identity())
+    }
+
+    /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
+    ///
+    /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
+    ///
+    /// # Parameters
+    ///
+    /// * `m`: the matrix from which the rotational part is to be extracted.
+    /// * `eps`: the angular errors tolerated between the current rotation and the optimal one.
+    /// * `max_iter`: the maximum number of iterations. Loops indefinitely until convergence if set to `0`.
+    /// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close
+    ///           to the actual solution is provided. Can be set to `Rotation2::identity()` if no other
+    ///           guesses come to mind.
+    pub fn from_matrix_eps(m: &Matrix2<N>, eps: N, mut max_iter: usize, guess: Self) -> Self {
+        if max_iter == 0 {
+            max_iter = usize::max_value();
+        }
+
+        let mut rot = guess.into_inner();
+
+        for _ in 0..max_iter {
+            let axis = rot.column(0).perp(&m.column(0)) +
+                rot.column(1).perp(&m.column(1));
+            let denom = rot.column(0).dot(&m.column(0)) +
+                rot.column(1).dot(&m.column(1));
+
+            let angle = axis / (denom.abs() + N::default_epsilon());
+            if angle.abs() > eps {
+                rot = Self::new(angle) * rot;
+            } else {
+                break;
+            }
+        }
+
+        Self::from_matrix_unchecked(rot)
+    }
+
     /// The rotation matrix required to align `a` and `b` but with its angle.
     ///
     /// This is the rotation `R` such that `(R * a).angle(b) == 0 && (R * a).dot(b).is_positive()`.
@@ -97,9 +142,7 @@ impl<N: Real> Rotation2<N> {
     {
         ::convert(UnitComplex::scaled_rotation_between(a, b, s).to_rotation_matrix())
     }
-}
 
-impl<N: Real> Rotation2<N> {
     /// The rotation angle.
     ///
     /// # Example
@@ -149,6 +192,20 @@ impl<N: Real> Rotation2<N> {
         other * self.inverse()
     }
 
+
+    /* FIXME: requires alga v0.9 to be released so that Complex implements VectorSpace.
+    /// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated
+    /// computations might cause the matrix from progressively not being orthonormal anymore.
+    #[inline]
+    pub fn renormalize(&mut self) {
+        let mut c = UnitComplex::from(*self);
+        let _ = c.renormalize();
+
+        *self = Self::from_matrix_eps(self.matrix(), N::default_epsilon(), 0, c.into())
+    }
+    */
+
+
     /// Raise the quaternion to a given floating power, i.e., returns the rotation with the angle
     /// of `self` multiplied by `n`.
     ///
@@ -230,6 +287,54 @@ impl<N: Real> Rotation3<N> {
         Self::from_axis_angle(&axis, angle)
     }
 
+    /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
+    ///
+    /// This is an iterative method. See `.from_matrix_eps` to provide mover
+    /// convergence parameters and starting solution.
+    /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
+    pub fn from_matrix(m: &Matrix3<N>) -> Self {
+        Self::from_matrix_eps(m, N::default_epsilon(), 0, Self::identity())
+    }
+
+    /// Builds a rotation matrix by extracting the rotation part of the given transformation `m`.
+    ///
+    /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
+    ///
+    /// # Parameters
+    ///
+    /// * `m`: the matrix from which the rotational part is to be extracted.
+    /// * `eps`: the angular errors tolerated between the current rotation and the optimal one.
+    /// * `max_iter`: the maximum number of iterations. Loops indefinitely until convergence if set to `0`.
+    /// * `guess`: a guess of the solution. Convergence will be significantly faster if an initial solution close
+    ///           to the actual solution is provided. Can be set to `Rotation3::identity()` if no other
+    ///           guesses come to mind.
+    pub fn from_matrix_eps(m: &Matrix3<N>, eps: N, mut max_iter: usize, guess: Self) -> Self {
+        if max_iter == 0 {
+            max_iter = usize::max_value();
+        }
+
+        let mut rot = guess.into_inner();
+
+        for _ in 0..max_iter {
+            let axis = rot.column(0).cross(&m.column(0)) +
+                rot.column(1).cross(&m.column(1)) +
+                rot.column(2).cross(&m.column(2));
+            let denom = rot.column(0).dot(&m.column(0)) +
+                rot.column(1).dot(&m.column(1)) +
+                rot.column(2).dot(&m.column(2));
+
+            let axisangle = axis / (denom.abs() + N::default_epsilon());
+
+            if let Some((axis, angle)) = Unit::try_new_and_get(axisangle, eps) {
+                rot = Rotation3::from_axis_angle(&axis, angle) * rot;
+            } else {
+                break;
+            }
+        }
+
+        Self::from_matrix_unchecked(rot)
+    }
+
     /// Builds a 3D rotation matrix from an axis scaled by the rotation angle.
     ///
     /// This is the same as `Self::new(axisangle)`.
@@ -378,6 +483,16 @@ impl<N: Real> Rotation3<N> {
         }
     }
 
+    /// Ensure this rotation is an orthonormal rotation matrix. This is useful when repeated
+    /// computations might cause the matrix from progressively not being orthonormal anymore.
+    #[inline]
+    pub fn renormalize(&mut self) {
+        let mut c = UnitQuaternion::from(*self);
+        let _ = c.renormalize();
+
+        *self = Self::from_matrix_eps(self.matrix(), N::default_epsilon(), 0, c.into())
+    }
+
     /// Creates a rotation that corresponds to the local frame of an observer standing at the
     /// origin and looking toward `dir`.
     ///

src/geometry/unit_complex.rs

@@ -303,18 +303,4 @@ impl<N: Real> UlpsEq for UnitComplex<N> {
         self.re.ulps_eq(&other.re, epsilon, max_ulps)
             && self.im.ulps_eq(&other.im, epsilon, max_ulps)
     }
-}
+}
-
-impl<N: Real> From<UnitComplex<N>> for Matrix3<N> {
-    #[inline]
-    fn from(q: UnitComplex<N>) -> Matrix3<N> {
-        q.to_homogeneous()
-    }
-}
-
-impl<N: Real> From<UnitComplex<N>> for Matrix2<N> {
-    #[inline]
-    fn from(q: UnitComplex<N>) -> Self {
-        q.to_rotation_matrix().into_inner()
-    }
-}

src/geometry/unit_complex_construction.rs

@@ -9,7 +9,7 @@ use rand::Rng;
 use alga::general::Real;
 use base::dimension::{U1, U2};
 use base::storage::Storage;
-use base::{Unit, Vector};
+use base::{Unit, Vector, Matrix2};
 use geometry::{Rotation2, UnitComplex};
 
 impl<N: Real> UnitComplex<N> {
@@ -129,6 +129,32 @@ impl<N: Real> UnitComplex<N> {
         Self::new_unchecked(Complex::new(rotmat[(0, 0)], rotmat[(1, 0)]))
     }
 
+    /// Builds an unit complex by extracting the rotation part of the given transformation `m`.
+    ///
+    /// This is an iterative method. See `.from_matrix_eps` to provide mover
+    /// convergence parameters and starting solution.
+    /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
+    pub fn from_matrix(m: &Matrix2<N>) -> Self {
+        Rotation2::from_matrix(m).into()
+    }
+
+    /// Builds an unit complex by extracting the rotation part of the given transformation `m`.
+    ///
+    /// This implements "A Robust Method to Extract the Rotational Part of Deformations" by Müller et al.
+    ///
+    /// # Parameters
+    ///
+    /// * `m`: the matrix from which the rotational part is to be extracted.
+    /// * `eps`: the angular errors tolerated between the current rotation and the optimal one.
+    /// * `max_iter`: the maximum number of iterations. Loops indefinitely until convergence if set to `0`.
+    /// * `guess`: an estimate of the solution. Convergence will be significantly faster if an initial solution close
+    ///           to the actual solution is provided. Can be set to `UnitQuaternion::identity()` if no other
+    ///           guesses come to mind.
+    pub fn from_matrix_eps(m: &Matrix2<N>, eps: N, max_iter: usize, guess: Self) -> Self {
+        let guess = Rotation2::from(guess);
+        Rotation2::from_matrix_eps(m, eps, max_iter, guess).into()
+    }
+
     /// The unit complex needed to make `a` and `b` be collinear and point toward the same
     /// direction.
     ///

src/geometry/unit_complex_conversion.rs

@@ -5,10 +5,10 @@ use alga::general::{Real, SubsetOf, SupersetOf};
 use alga::linear::Rotation as AlgaRotation;
 
 use base::dimension::U2;
-use base::Matrix3;
+use base::{Matrix2, Matrix3};
 use geometry::{
     Isometry, Point2, Rotation2, Similarity, SuperTCategoryOf, TAffine, Transform, Translation,
-    UnitComplex,
+    UnitComplex
 };
 
 /*
@@ -153,3 +153,32 @@ impl<N1: Real, N2: Real + SupersetOf<N1>> SubsetOf<Matrix3<N2>> for UnitComplex<
         Self::from_rotation_matrix(&rot)
     }
 }
+
+
+impl<N: Real> From<UnitComplex<N>> for Rotation2<N> {
+    #[inline]
+    fn from(q: UnitComplex<N>) -> Self {
+        q.to_rotation_matrix()
+    }
+}
+
+impl<N: Real> From<Rotation2<N>> for UnitComplex<N> {
+    #[inline]
+    fn from(q: Rotation2<N>) -> Self {
+        Self::from_rotation_matrix(&q)
+    }
+}
+
+impl<N: Real> From<UnitComplex<N>> for Matrix3<N> {
+    #[inline]
+    fn from(q: UnitComplex<N>) -> Matrix3<N> {
+        q.to_homogeneous()
+    }
+}
+
+impl<N: Real> From<UnitComplex<N>> for Matrix2<N> {
+    #[inline]
+    fn from(q: UnitComplex<N>) -> Self {
+        q.to_rotation_matrix().into_inner()
+    }
+}