Add sections to the Rotation documentation
This commit is contained in:
parent
57723ef8fb
commit
99ac7a8e08
|
@ -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 <span style="float:right;">`from_parts`…</span>](#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 <span style="float:right;">`transform_vector`, `inverse_transform_point`…</span>](#transformation-of-a-vector-or-a-point)
|
||||
/// * [Inversion and in-place composition <span style="float:right;">`inverse`, `append_rotation_wrt_point_mut`…</span>](#inversion-and-in-place-composition)
|
||||
/// * [2D interpolation <span style="float:right;">`lerp_slerp`…</span>](#2d-interpolation)
|
||||
/// * [3D interpolation <span style="float:right;">`lerp_slerp`…</span>](#3d-interpolation)
|
||||
/// * [Interpolation <span style="float:right;">`lerp_slerp`…</span>](#interpolation)
|
||||
///
|
||||
/// # Conversion to a matrix
|
||||
/// * [Conversion to a matrix <span style="float:right;">`to_matrix`…</span>](#conversion-to-a-matrix)
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
use crate::{Isometry2, Isometry3, IsometryMatrix2, IsometryMatrix3, RealField, SimdRealField};
|
||||
|
||||
/// # 3D interpolation
|
||||
/// # Interpolation
|
||||
impl<N: SimdRealField> Isometry3<N> {
|
||||
/// 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<N: SimdRealField> IsometryMatrix3<N> {
|
|||
}
|
||||
}
|
||||
|
||||
/// # 2D interpolation
|
||||
impl<N: SimdRealField> Isometry2<N> {
|
||||
/// Interpolates between two isometries using a linear interpolation for the translation part,
|
||||
/// and a spherical interpolation for the rotation part.
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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 <span style="float:right;">`identity`</span>](#identity)
|
||||
/// * [From a 2D rotation angle <span style="float:right;">`new`…</span>](#construction-from-a-2d-rotation-angle)
|
||||
/// * [From an existing 2D matrix or rotations <span style="float:right;">`from_matrix`, `rotation_between`, `powf`…</span>](#construction-from-an-existing-2d-matrix-or-rotations)
|
||||
/// * [From a 3D axis and/or angles <span style="float:right;">`new`, `from_euler_angles`, `from_axis_angle`…</span>](#construction-from-a-3d-axis-andor-angles)
|
||||
/// * [From a 3D eye position and target point <span style="float:right;">`look_at`, `look_at_lh`, `rotation_between`…</span>](#construction-from-a-3d-eye-position-and-target-point)
|
||||
/// * [From an existing 3D matrix or rotations <span style="float:right;">`from_matrix`, `rotation_between`, `powf`…</span>](#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 <span style="float:right;">`angle`, `euler_angles`, `scaled_axis`, `angle_to`…</span>](#3d-axis-and-angle-extraction)
|
||||
/// * [2D angle extraction <span style="float:right;">`angle`, `angle_to`…</span>](#2d-angle-extraction)
|
||||
/// * [Transformation of a vector or a point <span style="float:right;">`transform_vector`, `inverse_transform_point`…</span>](#transformation-of-a-vector-or-a-point)
|
||||
/// * [Transposition and inversion <span style="float:right;">`transpose`, `inverse`…</span>](#transposition-and-inversion)
|
||||
/// * [Interpolation <span style="float:right;">`slerp`…</span>](#interpolation)
|
||||
///
|
||||
/// # Conversion to a matrix
|
||||
/// * [Conversion to a matrix <span style="float:right;">`matrix`, `to_homogeneous`…</span>](#conversion-to-a-matrix)
|
||||
///
|
||||
#[repr(C)]
|
||||
#[derive(Debug)]
|
||||
pub struct Rotation<N: Scalar, D: DimName>
|
||||
|
@ -111,6 +141,44 @@ where
|
|||
}
|
||||
}
|
||||
|
||||
impl<N: Scalar, D: DimName> Rotation<N, D>
|
||||
where
|
||||
DefaultAllocator: Allocator<N, D, D>,
|
||||
{
|
||||
/// 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<N, D>) -> Self {
|
||||
assert!(
|
||||
matrix.is_square(),
|
||||
"Unable to create a rotation from a non-square matrix."
|
||||
);
|
||||
|
||||
Self { matrix }
|
||||
}
|
||||
}
|
||||
|
||||
/// # Conversion to a matrix
|
||||
impl<N: Scalar, D: DimName> Rotation<N, D>
|
||||
where
|
||||
DefaultAllocator: Allocator<N, D, D>,
|
||||
|
@ -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<N, D>) -> Self {
|
||||
assert!(
|
||||
matrix.is_square(),
|
||||
"Unable to create a rotation from a non-square matrix."
|
||||
);
|
||||
|
||||
Self { matrix }
|
||||
}
|
||||
|
||||
/// # Transposition and inversion
|
||||
impl<N: Scalar, D: DimName> Rotation<N, D>
|
||||
where
|
||||
DefaultAllocator: Allocator<N, D, D>,
|
||||
{
|
||||
/// 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<N: SimdRealField, D: DimName> Rotation<N, D>
|
||||
where
|
||||
N::Element: SimdRealField,
|
||||
|
|
|
@ -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<N> = Rotation<N, U2>;
|
||||
|
||||
/// 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<N> = Rotation<N, U3>;
|
||||
|
|
|
@ -8,6 +8,7 @@ use crate::base::{DefaultAllocator, MatrixN, Scalar};
|
|||
|
||||
use crate::geometry::Rotation;
|
||||
|
||||
/// # Identity
|
||||
impl<N, D: DimName> Rotation<N, D>
|
||||
where
|
||||
N: Scalar + Zero + One,
|
||||
|
|
|
@ -0,0 +1,78 @@
|
|||
use crate::{RealField, Rotation2, Rotation3, SimdRealField, UnitComplex, UnitQuaternion};
|
||||
|
||||
/// # Interpolation
|
||||
impl<N: SimdRealField> Rotation2<N> {
|
||||
/// 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<N: SimdRealField> Rotation3<N> {
|
||||
/// 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<Self>
|
||||
where
|
||||
N: RealField,
|
||||
{
|
||||
let q1 = Rotation3::from(*self);
|
||||
let q2 = Rotation3::from(*other);
|
||||
q1.try_slerp(&q2, t, epsilon).map(|q| q.into())
|
||||
}
|
||||
}
|
|
@ -21,6 +21,7 @@ use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion};
|
|||
* 2D Rotation matrix.
|
||||
*
|
||||
*/
|
||||
/// # Construction from a 2D rotation angle
|
||||
impl<N: SimdRealField> Rotation2<N> {
|
||||
/// Builds a 2 dimensional rotation matrix from an angle in radian.
|
||||
///
|
||||
|
@ -48,7 +49,10 @@ impl<N: SimdRealField> Rotation2<N> {
|
|||
pub fn from_scaled_axis<SB: Storage<N, U1>>(axisangle: Vector<N, U1, SB>) -> Self {
|
||||
Self::new(axisangle[0])
|
||||
}
|
||||
}
|
||||
|
||||
/// # Construction from an existing 2D matrix or rotations
|
||||
impl<N: SimdRealField> Rotation2<N> {
|
||||
/// 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<N: SimdRealField> Rotation2<N> {
|
|||
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<N: SimdRealField> Rotation2<N> {
|
|||
pub fn powf(&self, n: N) -> Self {
|
||||
Self::new(self.angle() * n)
|
||||
}
|
||||
}
|
||||
|
||||
/// # 2D angle extraction
|
||||
impl<N: SimdRealField> Rotation2<N> {
|
||||
/// 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<N: SimdRealField> Rotation2<N> {
|
|||
pub fn scaled_axis(&self) -> VectorN<N, U1> {
|
||||
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<N: SimdRealField> Distribution<Rotation2<N>> for Standard
|
||||
|
@ -292,6 +274,7 @@ where
|
|||
* 3D Rotation matrix.
|
||||
*
|
||||
*/
|
||||
/// # Construction from a 3D axis and/or angles
|
||||
impl<N: SimdRealField> Rotation3<N>
|
||||
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<N>) -> 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<N>, 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<N: SimdRealField> Rotation3<N>
|
||||
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<N: SimdRealField> Rotation3<N>
|
||||
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::<N, U3>::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<N>) -> 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<N>, 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<N: SimdRealField> Rotation3<N> {
|
||||
/// 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::<N, U3>::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<Self>
|
||||
where
|
||||
N: RealField,
|
||||
{
|
||||
let q1 = Rotation3::from(*self);
|
||||
let q2 = Rotation3::from(*other);
|
||||
q1.try_slerp(&q2, t, epsilon).map(|q| q.into())
|
||||
}
|
||||
}
|
||||
|
||||
impl<N: SimdRealField> Distribution<Rotation3<N>> for Standard
|
||||
|
|
Loading…
Reference in New Issue