M-Labs
/
nalgebra
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Sébastien Crozet 2019-02-18 22:41:46 +01:00 committed by GitHub
parent df0407ef44
commit c97dfaf381
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 249 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
CHANGELOG.md
View File

@ -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)`.

17
src/base/unit.rs
View File

@ -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> {

34
src/geometry/quaternion_construction.rs
View File

@ -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.

14
src/geometry/quaternion_conversion.rs
View File

@ -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 {

125
src/geometry/rotation_specialization.rs
View File

@ -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`.
///

14
src/geometry/unit_complex.rs
View File

@ -304,17 +304,3 @@ impl<N: Real> UlpsEq for UnitComplex<N> {
&& 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()
}
}

28
src/geometry/unit_complex_construction.rs
View File

@ -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.
///

33
src/geometry/unit_complex_conversion.rs
View File

@ -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()
}
}