From c97dfaf381d9c2f185f2e522774084280bbf83ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Mon, 18 Feb 2019 22:41:46 +0100 Subject: [PATCH] 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 | 11 +- src/base/unit.rs | 17 ++- src/geometry/quaternion_construction.rs | 34 +++++- src/geometry/quaternion_conversion.rs | 14 +++ src/geometry/rotation_specialization.rs | 125 +++++++++++++++++++++- src/geometry/unit_complex.rs | 16 +-- src/geometry/unit_complex_construction.rs | 28 ++++- src/geometry/unit_complex_conversion.rs | 33 +++++- 8 files changed, 249 insertions(+), 29 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d76638f1..22aa582b 100644 --- a/CHANGELOG.md +++ b/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)`. diff --git a/src/base/unit.rs b/src/base/unit.rs index af400389..3d0a9c03 100644 --- a/src/base/unit.rs +++ b/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 Unit { /// 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 Unit { diff --git a/src/geometry/quaternion_construction.rs b/src/geometry/quaternion_construction.rs index 4098a5da..0a30fabe 100644 --- a/src/geometry/quaternion_construction.rs +++ b/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 Quaternion { /// Creates a quaternion from a 4D vector. The quaternion scalar part corresponds to the `w` @@ -245,7 +245,7 @@ impl UnitQuaternion { /// assert_relative_eq!(q.angle(), rot.angle(), epsilon = 1.0e-6); /// ``` #[inline] - pub fn from_rotation_matrix(rotmat: &Rotation) -> Self { + pub fn from_rotation_matrix(rotmat: &Rotation3) -> 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 UnitQuaternion { 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) -> 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, 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 UnitQuaternion { SB: Storage, SC: Storage, { - Self::from_rotation_matrix(&Rotation::::face_towards(dir, up)) + Self::from_rotation_matrix(&Rotation3::face_towards(dir, up)) } /// Deprecated: Use [UnitQuaternion::face_towards] instead. diff --git a/src/geometry/quaternion_conversion.rs b/src/geometry/quaternion_conversion.rs index 741c68da..a0b44390 100644 --- a/src/geometry/quaternion_conversion.rs +++ b/src/geometry/quaternion_conversion.rs @@ -225,6 +225,20 @@ impl From> for Matrix4 { } } +impl From> for Rotation3 { + #[inline] + fn from(q: UnitQuaternion) -> Self { + q.to_rotation_matrix() + } +} + +impl From> for UnitQuaternion { + #[inline] + fn from(q: Rotation3) -> Self { + Self::from_rotation_matrix(&q) + } +} + impl From> for Matrix3 { #[inline] fn from(q: UnitQuaternion) -> Self { diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index dde5bb82..0aeb3041 100644 --- a/src/geometry/rotation_specialization.rs +++ b/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 Rotation2 { /// ``` pub fn new(angle: N) -> Self { let (sia, coa) = angle.sin_cos(); - Self::from_matrix_unchecked(MatrixN::::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 Rotation2 { 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) -> 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, 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 Rotation2 { { ::convert(UnitComplex::scaled_rotation_between(a, b, s).to_rotation_matrix()) } -} -impl Rotation2 { /// The rotation angle. /// /// # Example @@ -149,6 +192,20 @@ impl Rotation2 { 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 Rotation3 { 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) -> 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, 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 Rotation3 { } } + /// 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`. /// diff --git a/src/geometry/unit_complex.rs b/src/geometry/unit_complex.rs index cae41483..fe1058af 100644 --- a/src/geometry/unit_complex.rs +++ b/src/geometry/unit_complex.rs @@ -303,18 +303,4 @@ impl UlpsEq for UnitComplex { self.re.ulps_eq(&other.re, epsilon, max_ulps) && self.im.ulps_eq(&other.im, epsilon, max_ulps) } -} - -impl From> for Matrix3 { - #[inline] - fn from(q: UnitComplex) -> Matrix3 { - q.to_homogeneous() - } -} - -impl From> for Matrix2 { - #[inline] - fn from(q: UnitComplex) -> Self { - q.to_rotation_matrix().into_inner() - } -} +} \ No newline at end of file diff --git a/src/geometry/unit_complex_construction.rs b/src/geometry/unit_complex_construction.rs index 37518b4f..ba1d6694 100644 --- a/src/geometry/unit_complex_construction.rs +++ b/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 UnitComplex { @@ -129,6 +129,32 @@ impl UnitComplex { 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) -> 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, 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. /// diff --git a/src/geometry/unit_complex_conversion.rs b/src/geometry/unit_complex_conversion.rs index fd4d1103..1751a67c 100644 --- a/src/geometry/unit_complex_conversion.rs +++ b/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> SubsetOf> for UnitComplex< Self::from_rotation_matrix(&rot) } } + + +impl From> for Rotation2 { + #[inline] + fn from(q: UnitComplex) -> Self { + q.to_rotation_matrix() + } +} + +impl From> for UnitComplex { + #[inline] + fn from(q: Rotation2) -> Self { + Self::from_rotation_matrix(&q) + } +} + +impl From> for Matrix3 { + #[inline] + fn from(q: UnitComplex) -> Matrix3 { + q.to_homogeneous() + } +} + +impl From> for Matrix2 { + #[inline] + fn from(q: UnitComplex) -> Self { + q.to_rotation_matrix().into_inner() + } +}