diff --git a/src/geometry/isometry.rs b/src/geometry/isometry.rs index b2228301..7a630bed 100755 --- a/src/geometry/isometry.rs +++ b/src/geometry/isometry.rs @@ -30,10 +30,6 @@ use crate::geometry::{AbstractRotation, Point, Translation}; /// - A translation part of type [`Translation3`](crate::Translation3) /// - A rotation part which can either be a [`UnitQuaternion`](crate::UnitQuaternion) or a [`Rotation3`](crate::Rotation3). /// -/// The [`Isometry2`](crate::Isometry2), [`Isometry3`](crate::Isometry3), [`IsometryMatrix2`](crate::IsometryMatrix2), -/// and [`IsometryMatrix3`](crate::IsometryMatrix3) type aliases are provided for convenience. All -/// their available methods are listed in this page and cant be grouped as follows. -/// /// Note that instead of using the [`Isometry`](crate::Isometry) type in your code directly, you should use one /// of its aliases: [`Isometry2`](crate::Isometry2), [`Isometry3`](crate::Isometry3), /// [`IsometryMatrix2`](crate::IsometryMatrix2), [`IsometryMatrix3`](crate::IsometryMatrix3). Though @@ -47,13 +43,12 @@ use crate::geometry::{AbstractRotation, Point, Translation}; /// * [From the translation and rotation parts `from_parts`…](#from-the-translation-and-rotation-parts) /// /// # Transformation and composition -/// Note that transforming vectors and points can be done bu multiplication, e.g., `isometry * point`. +/// Note that transforming vectors and points can be done by multiplication, e.g., `isometry * point`. /// Composing an isometry with another transformation can also be done by multiplication or division. /// /// * [Transformation of a vector or a point `transform_vector`, `inverse_transform_point`…](#transformation-of-a-vector-or-a-point) /// * [Inversion and in-place composition `inverse`, `append_rotation_wrt_point_mut`…](#inversion-and-in-place-composition) -/// * [2D interpolation `lerp_slerp`…](#2d-interpolation) -/// * [3D interpolation `lerp_slerp`…](#3d-interpolation) +/// * [Interpolation `lerp_slerp`…](#interpolation) /// /// # Conversion to a matrix /// * [Conversion to a matrix `to_matrix`…](#conversion-to-a-matrix) diff --git a/src/geometry/isometry_interpolation.rs b/src/geometry/isometry_interpolation.rs index 50e3f305..f58b4115 100644 --- a/src/geometry/isometry_interpolation.rs +++ b/src/geometry/isometry_interpolation.rs @@ -1,6 +1,6 @@ use crate::{Isometry2, Isometry3, IsometryMatrix2, IsometryMatrix3, RealField, SimdRealField}; -/// # 3D interpolation +/// # Interpolation impl Isometry3 { /// Interpolates between two isometries using a linear interpolation for the translation part, /// and a spherical interpolation for the rotation part. @@ -137,7 +137,6 @@ impl IsometryMatrix3 { } } -/// # 2D interpolation impl Isometry2 { /// Interpolates between two isometries using a linear interpolation for the translation part, /// and a spherical interpolation for the rotation part. diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 25ef89f1..d3e2236a 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -21,6 +21,7 @@ mod rotation_alga; mod rotation_alias; mod rotation_construction; mod rotation_conversion; +mod rotation_interpolation; mod rotation_ops; mod rotation_simba; // TODO: implement Rotation methods. mod rotation_specialization; @@ -58,6 +59,7 @@ mod isometry_alga; mod isometry_alias; mod isometry_construction; mod isometry_conversion; +mod isometry_interpolation; mod isometry_ops; mod isometry_simba; @@ -83,7 +85,6 @@ mod transform_simba; mod reflection; -mod isometry_interpolation; mod orthographic; mod perspective; diff --git a/src/geometry/rotation.rs b/src/geometry/rotation.rs index c30a3a97..20985093 100755 --- a/src/geometry/rotation.rs +++ b/src/geometry/rotation.rs @@ -23,6 +23,36 @@ use crate::base::{DefaultAllocator, MatrixN, Scalar, Unit, VectorN}; use crate::geometry::Point; /// A rotation matrix. +/// +/// This is also known as an element of a Special Orthogonal (SO) group. +/// The `Rotation` type can either represent a 2D or 3D rotation, represented as a matrix. +/// For a rotation based on quaternions, see [`UnitQuaternion`](crate::UnitQuaternion) instead. +/// +/// Note that instead of using the [`Rotation`](crate::Rotation) type in your code directly, you should use one +/// of its aliases: [`Rotation2`](crate::Rotation2), or [`Rotation3`](crate::Rotation3). Though +/// keep in mind that all the documentation of all the methods of these aliases will also appears on +/// this page. +/// +/// # Construction +/// * [Identity `identity`](#identity) +/// * [From a 2D rotation angle `new`…](#construction-from-a-2d-rotation-angle) +/// * [From an existing 2D matrix or rotations `from_matrix`, `rotation_between`, `powf`…](#construction-from-an-existing-2d-matrix-or-rotations) +/// * [From a 3D axis and/or angles `new`, `from_euler_angles`, `from_axis_angle`…](#construction-from-a-3d-axis-andor-angles) +/// * [From a 3D eye position and target point `look_at`, `look_at_lh`, `rotation_between`…](#construction-from-a-3d-eye-position-and-target-point) +/// * [From an existing 3D matrix or rotations `from_matrix`, `rotation_between`, `powf`…](#construction-from-an-existing-3d-matrix-or-rotations) +/// +/// # Transformation and composition +/// Note that transforming vectors and points can be done by multiplication, e.g., `rotation * point`. +/// Composing an rotation with another transformation can also be done by multiplication or division. +/// * [3D axis and angle extraction `angle`, `euler_angles`, `scaled_axis`, `angle_to`…](#3d-axis-and-angle-extraction) +/// * [2D angle extraction `angle`, `angle_to`…](#2d-angle-extraction) +/// * [Transformation of a vector or a point `transform_vector`, `inverse_transform_point`…](#transformation-of-a-vector-or-a-point) +/// * [Transposition and inversion `transpose`, `inverse`…](#transposition-and-inversion) +/// * [Interpolation `slerp`…](#interpolation) +/// +/// # Conversion to a matrix +/// * [Conversion to a matrix `matrix`, `to_homogeneous`…](#conversion-to-a-matrix) +/// #[repr(C)] #[derive(Debug)] pub struct Rotation @@ -111,6 +141,44 @@ where } } +impl Rotation +where + DefaultAllocator: Allocator, +{ + /// Creates a new rotation from the given square matrix. + /// + /// The matrix squareness is checked but not its orthonormality. + /// + /// # Example + /// ``` + /// # use nalgebra::{Rotation2, Rotation3, Matrix2, Matrix3}; + /// # use std::f32; + /// let mat = Matrix3::new(0.8660254, -0.5, 0.0, + /// 0.5, 0.8660254, 0.0, + /// 0.0, 0.0, 1.0); + /// let rot = Rotation3::from_matrix_unchecked(mat); + /// + /// assert_eq!(*rot.matrix(), mat); + /// + /// + /// let mat = Matrix2::new(0.8660254, -0.5, + /// 0.5, 0.8660254); + /// let rot = Rotation2::from_matrix_unchecked(mat); + /// + /// assert_eq!(*rot.matrix(), mat); + /// ``` + #[inline] + pub fn from_matrix_unchecked(matrix: MatrixN) -> Self { + assert!( + matrix.is_square(), + "Unable to create a rotation from a non-square matrix." + ); + + Self { matrix } + } +} + +/// # Conversion to a matrix impl Rotation where DefaultAllocator: Allocator, @@ -225,39 +293,13 @@ where res } +} - /// Creates a new rotation from the given square matrix. - /// - /// The matrix squareness is checked but not its orthonormality. - /// - /// # Example - /// ``` - /// # use nalgebra::{Rotation2, Rotation3, Matrix2, Matrix3}; - /// # use std::f32; - /// let mat = Matrix3::new(0.8660254, -0.5, 0.0, - /// 0.5, 0.8660254, 0.0, - /// 0.0, 0.0, 1.0); - /// let rot = Rotation3::from_matrix_unchecked(mat); - /// - /// assert_eq!(*rot.matrix(), mat); - /// - /// - /// let mat = Matrix2::new(0.8660254, -0.5, - /// 0.5, 0.8660254); - /// let rot = Rotation2::from_matrix_unchecked(mat); - /// - /// assert_eq!(*rot.matrix(), mat); - /// ``` - #[inline] - pub fn from_matrix_unchecked(matrix: MatrixN) -> Self { - assert!( - matrix.is_square(), - "Unable to create a rotation from a non-square matrix." - ); - - Self { matrix } - } - +/// # Transposition and inversion +impl Rotation +where + DefaultAllocator: Allocator, +{ /// Transposes `self`. /// /// Same as `.inverse()` because the inverse of a rotation matrix is its transform. @@ -361,6 +403,7 @@ where } } +/// # Transformation of a vector or a point impl Rotation where N::Element: SimdRealField, diff --git a/src/geometry/rotation_alias.rs b/src/geometry/rotation_alias.rs index 9fa5c2d0..f9e0230b 100644 --- a/src/geometry/rotation_alias.rs +++ b/src/geometry/rotation_alias.rs @@ -3,7 +3,11 @@ use crate::base::dimension::{U2, U3}; use crate::geometry::Rotation; /// A 2-dimensional rotation matrix. +/// +/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type too.** pub type Rotation2 = Rotation; /// A 3-dimensional rotation matrix. +/// +/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type too.** pub type Rotation3 = Rotation; diff --git a/src/geometry/rotation_construction.rs b/src/geometry/rotation_construction.rs index 12d93402..c351f373 100644 --- a/src/geometry/rotation_construction.rs +++ b/src/geometry/rotation_construction.rs @@ -8,6 +8,7 @@ use crate::base::{DefaultAllocator, MatrixN, Scalar}; use crate::geometry::Rotation; +/// # Identity impl Rotation where N: Scalar + Zero + One, diff --git a/src/geometry/rotation_interpolation.rs b/src/geometry/rotation_interpolation.rs new file mode 100644 index 00000000..5f861428 --- /dev/null +++ b/src/geometry/rotation_interpolation.rs @@ -0,0 +1,78 @@ +use crate::{RealField, Rotation2, Rotation3, SimdRealField, UnitComplex, UnitQuaternion}; + +/// # Interpolation +impl Rotation2 { + /// Spherical linear interpolation between two rotation matrices. + /// + /// # Examples: + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::geometry::Rotation2; + /// + /// let rot1 = Rotation2::new(std::f32::consts::FRAC_PI_4); + /// let rot2 = Rotation2::new(-std::f32::consts::PI); + /// + /// let rot = rot1.slerp(&rot2, 1.0 / 3.0); + /// + /// assert_relative_eq!(rot.angle(), std::f32::consts::FRAC_PI_2); + /// ``` + #[inline] + pub fn slerp(&self, other: &Self, t: N) -> Self + where + N::Element: SimdRealField, + { + let c1 = UnitComplex::from(*self); + let c2 = UnitComplex::from(*other); + c1.slerp(&c2, t).into() + } +} + +impl Rotation3 { + /// Spherical linear interpolation between two rotation matrices. + /// + /// Panics if the angle between both rotations is 180 degrees (in which case the interpolation + /// is not well-defined). Use `.try_slerp` instead to avoid the panic. + /// + /// # Examples: + /// + /// ``` + /// # use nalgebra::geometry::Rotation3; + /// + /// let q1 = Rotation3::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0); + /// let q2 = Rotation3::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0); + /// + /// let q = q1.slerp(&q2, 1.0 / 3.0); + /// + /// assert_eq!(q.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0)); + /// ``` + #[inline] + pub fn slerp(&self, other: &Self, t: N) -> Self + where + N: RealField, + { + let q1 = UnitQuaternion::from(*self); + let q2 = UnitQuaternion::from(*other); + q1.slerp(&q2, t).into() + } + + /// Computes the spherical linear interpolation between two rotation matrices or returns `None` + /// if both rotations are approximately 180 degrees apart (in which case the interpolation is + /// not well-defined). + /// + /// # Arguments + /// * `self`: the first rotation to interpolate from. + /// * `other`: the second rotation to interpolate toward. + /// * `t`: the interpolation parameter. Should be between 0 and 1. + /// * `epsilon`: the value below which the sinus of the angle separating both rotations + /// must be to return `None`. + #[inline] + pub fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option + where + N: RealField, + { + let q1 = Rotation3::from(*self); + let q2 = Rotation3::from(*other); + q1.try_slerp(&q2, t, epsilon).map(|q| q.into()) + } +} diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index 5cb7cb33..afc180d8 100644 --- a/src/geometry/rotation_specialization.rs +++ b/src/geometry/rotation_specialization.rs @@ -21,6 +21,7 @@ use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion}; * 2D Rotation matrix. * */ +/// # Construction from a 2D rotation angle impl Rotation2 { /// Builds a 2 dimensional rotation matrix from an angle in radian. /// @@ -48,7 +49,10 @@ impl Rotation2 { pub fn from_scaled_axis>(axisangle: Vector) -> Self { Self::new(axisangle[0]) } +} +/// # Construction from an existing 2D matrix or rotations +impl Rotation2 { /// 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 @@ -150,35 +154,6 @@ impl Rotation2 { crate::convert(UnitComplex::scaled_rotation_between(a, b, s).to_rotation_matrix()) } - /// The rotation angle. - /// - /// # Example - /// ``` - /// # #[macro_use] extern crate approx; - /// # use nalgebra::Rotation2; - /// let rot = Rotation2::new(1.78); - /// assert_relative_eq!(rot.angle(), 1.78); - /// ``` - #[inline] - pub fn angle(&self) -> N { - self.matrix()[(1, 0)].simd_atan2(self.matrix()[(0, 0)]) - } - - /// The rotation angle needed to make `self` and `other` coincide. - /// - /// # Example - /// ``` - /// # #[macro_use] extern crate approx; - /// # use nalgebra::Rotation2; - /// let rot1 = Rotation2::new(0.1); - /// let rot2 = Rotation2::new(1.7); - /// assert_relative_eq!(rot1.angle_to(&rot2), 1.6); - /// ``` - #[inline] - pub fn angle_to(&self, other: &Self) -> N { - self.rotation_to(other).angle() - } - /// The rotation matrix needed to make `self` and `other` coincide. /// /// The result is such that: `self.rotation_to(other) * self == other`. @@ -227,6 +202,38 @@ impl Rotation2 { pub fn powf(&self, n: N) -> Self { Self::new(self.angle() * n) } +} + +/// # 2D angle extraction +impl Rotation2 { + /// The rotation angle. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::Rotation2; + /// let rot = Rotation2::new(1.78); + /// assert_relative_eq!(rot.angle(), 1.78); + /// ``` + #[inline] + pub fn angle(&self) -> N { + self.matrix()[(1, 0)].simd_atan2(self.matrix()[(0, 0)]) + } + + /// The rotation angle needed to make `self` and `other` coincide. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::Rotation2; + /// let rot1 = Rotation2::new(0.1); + /// let rot2 = Rotation2::new(1.7); + /// assert_relative_eq!(rot1.angle_to(&rot2), 1.6); + /// ``` + #[inline] + pub fn angle_to(&self, other: &Self) -> N { + self.rotation_to(other).angle() + } /// The rotation angle returned as a 1-dimensional vector. /// @@ -236,31 +243,6 @@ impl Rotation2 { pub fn scaled_axis(&self) -> VectorN { Vector1::new(self.angle()) } - - /// Spherical linear interpolation between two rotation matrices. - /// - /// # Examples: - /// - /// ``` - /// # #[macro_use] extern crate approx; - /// # use nalgebra::geometry::Rotation2; - /// - /// let rot1 = Rotation2::new(std::f32::consts::FRAC_PI_4); - /// let rot2 = Rotation2::new(-std::f32::consts::PI); - /// - /// let rot = rot1.slerp(&rot2, 1.0 / 3.0); - /// - /// assert_relative_eq!(rot.angle(), std::f32::consts::FRAC_PI_2); - /// ``` - #[inline] - pub fn slerp(&self, other: &Self, t: N) -> Self - where - N::Element: SimdRealField, - { - let c1 = UnitComplex::from(*self); - let c2 = UnitComplex::from(*other); - c1.slerp(&c2, t).into() - } } impl Distribution> for Standard @@ -292,6 +274,7 @@ where * 3D Rotation matrix. * */ +/// # Construction from a 3D axis and/or angles impl Rotation3 where N::Element: SimdRealField, @@ -325,60 +308,6 @@ where 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 - where - N: RealField, - { - 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 - where - N: RealField, - { - 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)`. @@ -488,67 +417,13 @@ where cp * cr, )) } +} - /// Creates Euler angles from a rotation. - /// - /// The angles are produced in the form (roll, pitch, yaw). - #[deprecated(note = "This is renamed to use `.euler_angles()`.")] - pub fn to_euler_angles(&self) -> (N, N, N) - where - N: RealField, - { - self.euler_angles() - } - - /// Euler angles corresponding to this rotation from a rotation. - /// - /// The angles are produced in the form (roll, pitch, yaw). - /// - /// # Example - /// ``` - /// # #[macro_use] extern crate approx; - /// # use nalgebra::Rotation3; - /// let rot = Rotation3::from_euler_angles(0.1, 0.2, 0.3); - /// let euler = rot.euler_angles(); - /// assert_relative_eq!(euler.0, 0.1, epsilon = 1.0e-6); - /// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6); - /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6); - /// ``` - pub fn euler_angles(&self) -> (N, N, N) - where - N: RealField, - { - // Implementation informed by "Computing Euler angles from a rotation matrix", by Gregory G. Slabaugh - // https://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.371.6578 - if self[(2, 0)].abs() < N::one() { - let yaw = -self[(2, 0)].asin(); - let roll = (self[(2, 1)] / yaw.cos()).atan2(self[(2, 2)] / yaw.cos()); - let pitch = (self[(1, 0)] / yaw.cos()).atan2(self[(0, 0)] / yaw.cos()); - (roll, yaw, pitch) - } else if self[(2, 0)] <= -N::one() { - (self[(0, 1)].atan2(self[(0, 2)]), N::frac_pi_2(), N::zero()) - } else { - ( - -self[(0, 1)].atan2(-self[(0, 2)]), - -N::frac_pi_2(), - N::zero(), - ) - } - } - - /// 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) - where - N: RealField, - { - let mut c = UnitQuaternion::from(*self); - let _ = c.renormalize(); - - *self = Self::from_matrix_eps(self.matrix(), N::default_epsilon(), 0, c.into()) - } - +/// # Construction from a 3D eye position and target point +impl Rotation3 +where + N::Element: SimdRealField, +{ /// Creates a rotation that corresponds to the local frame of an observer standing at the /// origin and looking toward `dir`. /// @@ -656,7 +531,13 @@ where { Self::face_towards(dir, up).inverse() } +} +/// # Construction from an existing 3D matrix or rotations +impl Rotation3 +where + N::Element: SimdRealField, +{ /// 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()`. @@ -727,6 +608,123 @@ where Some(Self::identity()) } + /// The rotation matrix needed to make `self` and `other` coincide. + /// + /// The result is such that: `self.rotation_to(other) * self == other`. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{Rotation3, Vector3}; + /// let rot1 = Rotation3::from_axis_angle(&Vector3::y_axis(), 1.0); + /// let rot2 = Rotation3::from_axis_angle(&Vector3::x_axis(), 0.1); + /// let rot_to = rot1.rotation_to(&rot2); + /// assert_relative_eq!(rot_to * rot1, rot2, epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn rotation_to(&self, other: &Self) -> Self { + other * self.inverse() + } + + /// Raise the quaternion to a given floating power, i.e., returns the rotation with the same + /// axis as `self` and an angle equal to `self.angle()` multiplied by `n`. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{Rotation3, Vector3, Unit}; + /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0)); + /// let angle = 1.2; + /// let rot = Rotation3::from_axis_angle(&axis, angle); + /// let pow = rot.powf(2.0); + /// assert_relative_eq!(pow.axis().unwrap(), axis, epsilon = 1.0e-6); + /// assert_eq!(pow.angle(), 2.4); + /// ``` + #[inline] + pub fn powf(&self, n: N) -> Self + where + N: RealField, + { + if let Some(axis) = self.axis() { + Self::from_axis_angle(&axis, self.angle() * n) + } else if self.matrix()[(0, 0)] < N::zero() { + let minus_id = MatrixN::::from_diagonal_element(-N::one()); + Self::from_matrix_unchecked(minus_id) + } else { + Self::identity() + } + } + + /// 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 + where + N: RealField, + { + 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 + where + N: RealField, + { + 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) + } + + /// 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) + where + N: RealField, + { + let mut c = UnitQuaternion::from(*self); + let _ = c.renormalize(); + + *self = Self::from_matrix_eps(self.matrix(), N::default_epsilon(), 0, c.into()) + } +} + +/// # 3D axis and angle extraction +impl Rotation3 { /// The rotation angle in [0; pi]. /// /// # Example @@ -837,103 +835,59 @@ where /// assert_relative_eq!(rot1.angle_to(&rot2), 1.0045657, epsilon = 1.0e-6); /// ``` #[inline] - pub fn angle_to(&self, other: &Self) -> N { + pub fn angle_to(&self, other: &Self) -> N + where + N::Element: SimdRealField, + { self.rotation_to(other).angle() } - /// The rotation matrix needed to make `self` and `other` coincide. + /// Creates Euler angles from a rotation. /// - /// The result is such that: `self.rotation_to(other) * self == other`. - /// - /// # Example - /// ``` - /// # #[macro_use] extern crate approx; - /// # use nalgebra::{Rotation3, Vector3}; - /// let rot1 = Rotation3::from_axis_angle(&Vector3::y_axis(), 1.0); - /// let rot2 = Rotation3::from_axis_angle(&Vector3::x_axis(), 0.1); - /// let rot_to = rot1.rotation_to(&rot2); - /// assert_relative_eq!(rot_to * rot1, rot2, epsilon = 1.0e-6); - /// ``` - #[inline] - pub fn rotation_to(&self, other: &Self) -> Self { - other * self.inverse() - } - - /// Raise the quaternion to a given floating power, i.e., returns the rotation with the same - /// axis as `self` and an angle equal to `self.angle()` multiplied by `n`. - /// - /// # Example - /// ``` - /// # #[macro_use] extern crate approx; - /// # use nalgebra::{Rotation3, Vector3, Unit}; - /// let axis = Unit::new_normalize(Vector3::new(1.0, 2.0, 3.0)); - /// let angle = 1.2; - /// let rot = Rotation3::from_axis_angle(&axis, angle); - /// let pow = rot.powf(2.0); - /// assert_relative_eq!(pow.axis().unwrap(), axis, epsilon = 1.0e-6); - /// assert_eq!(pow.angle(), 2.4); - /// ``` - #[inline] - pub fn powf(&self, n: N) -> Self + /// The angles are produced in the form (roll, pitch, yaw). + #[deprecated(note = "This is renamed to use `.euler_angles()`.")] + pub fn to_euler_angles(&self) -> (N, N, N) where N: RealField, { - if let Some(axis) = self.axis() { - Self::from_axis_angle(&axis, self.angle() * n) - } else if self.matrix()[(0, 0)] < N::zero() { - let minus_id = MatrixN::::from_diagonal_element(-N::one()); - Self::from_matrix_unchecked(minus_id) + self.euler_angles() + } + + /// Euler angles corresponding to this rotation from a rotation. + /// + /// The angles are produced in the form (roll, pitch, yaw). + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::Rotation3; + /// let rot = Rotation3::from_euler_angles(0.1, 0.2, 0.3); + /// let euler = rot.euler_angles(); + /// assert_relative_eq!(euler.0, 0.1, epsilon = 1.0e-6); + /// assert_relative_eq!(euler.1, 0.2, epsilon = 1.0e-6); + /// assert_relative_eq!(euler.2, 0.3, epsilon = 1.0e-6); + /// ``` + pub fn euler_angles(&self) -> (N, N, N) + where + N: RealField, + { + // Implementation informed by "Computing Euler angles from a rotation matrix", by Gregory G. Slabaugh + // https://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.371.6578 + if self[(2, 0)].abs() < N::one() { + let yaw = -self[(2, 0)].asin(); + let roll = (self[(2, 1)] / yaw.cos()).atan2(self[(2, 2)] / yaw.cos()); + let pitch = (self[(1, 0)] / yaw.cos()).atan2(self[(0, 0)] / yaw.cos()); + (roll, yaw, pitch) + } else if self[(2, 0)] <= -N::one() { + (self[(0, 1)].atan2(self[(0, 2)]), N::frac_pi_2(), N::zero()) } else { - Self::identity() + ( + -self[(0, 1)].atan2(-self[(0, 2)]), + -N::frac_pi_2(), + N::zero(), + ) } } - - /// Spherical linear interpolation between two rotation matrices. - /// - /// Panics if the angle between both rotations is 180 degrees (in which case the interpolation - /// is not well-defined). Use `.try_slerp` instead to avoid the panic. - /// - /// # Examples: - /// - /// ``` - /// # use nalgebra::geometry::Rotation3; - /// - /// let q1 = Rotation3::from_euler_angles(std::f32::consts::FRAC_PI_4, 0.0, 0.0); - /// let q2 = Rotation3::from_euler_angles(-std::f32::consts::PI, 0.0, 0.0); - /// - /// let q = q1.slerp(&q2, 1.0 / 3.0); - /// - /// assert_eq!(q.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0)); - /// ``` - #[inline] - pub fn slerp(&self, other: &Self, t: N) -> Self - where - N: RealField, - { - let q1 = UnitQuaternion::from(*self); - let q2 = UnitQuaternion::from(*other); - q1.slerp(&q2, t).into() - } - - /// Computes the spherical linear interpolation between two rotation matrices or returns `None` - /// if both rotations are approximately 180 degrees apart (in which case the interpolation is - /// not well-defined). - /// - /// # Arguments - /// * `self`: the first rotation to interpolate from. - /// * `other`: the second rotation to interpolate toward. - /// * `t`: the interpolation parameter. Should be between 0 and 1. - /// * `epsilon`: the value below which the sinus of the angle separating both rotations - /// must be to return `None`. - #[inline] - pub fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option - where - N: RealField, - { - let q1 = Rotation3::from(*self); - let q2 = Rotation3::from(*other); - q1.try_slerp(&q2, t, epsilon).map(|q| q.into()) - } } impl Distribution> for Standard