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:
parent
df0407ef44
commit
c97dfaf381
11
CHANGELOG.md
11
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)`.
|
||||
|
@ -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> {
|
||||
|
@ -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.
|
||||
|
@ -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 {
|
||||
|
@ -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`.
|
||||
///
|
||||
|
@ -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()
|
||||
}
|
||||
}
|
||||
}
|
@ -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.
|
||||
///
|
||||
|
@ -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()
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user