forked from M-Labs/nalgebra
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 translation part of type [`Translation3`](crate::Translation3)
|
||||||
/// - A rotation part which can either be a [`UnitQuaternion`](crate::UnitQuaternion) or a [`Rotation3`](crate::Rotation3).
|
/// - 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
|
/// 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),
|
/// of its aliases: [`Isometry2`](crate::Isometry2), [`Isometry3`](crate::Isometry3),
|
||||||
/// [`IsometryMatrix2`](crate::IsometryMatrix2), [`IsometryMatrix3`](crate::IsometryMatrix3). Though
|
/// [`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)
|
/// * [From the translation and rotation parts <span style="float:right;">`from_parts`…</span>](#from-the-translation-and-rotation-parts)
|
||||||
///
|
///
|
||||||
/// # Transformation and composition
|
/// # 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.
|
/// 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)
|
/// * [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)
|
/// * [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)
|
/// * [Interpolation <span style="float:right;">`lerp_slerp`…</span>](#interpolation)
|
||||||
/// * [3D interpolation <span style="float:right;">`lerp_slerp`…</span>](#3d-interpolation)
|
|
||||||
///
|
///
|
||||||
/// # Conversion to a matrix
|
/// # Conversion to a matrix
|
||||||
/// * [Conversion to a matrix <span style="float:right;">`to_matrix`…</span>](#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};
|
use crate::{Isometry2, Isometry3, IsometryMatrix2, IsometryMatrix3, RealField, SimdRealField};
|
||||||
|
|
||||||
/// # 3D interpolation
|
/// # Interpolation
|
||||||
impl<N: SimdRealField> Isometry3<N> {
|
impl<N: SimdRealField> Isometry3<N> {
|
||||||
/// Interpolates between two isometries using a linear interpolation for the translation part,
|
/// Interpolates between two isometries using a linear interpolation for the translation part,
|
||||||
/// and a spherical interpolation for the rotation 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> {
|
impl<N: SimdRealField> Isometry2<N> {
|
||||||
/// Interpolates between two isometries using a linear interpolation for the translation part,
|
/// Interpolates between two isometries using a linear interpolation for the translation part,
|
||||||
/// and a spherical interpolation for the rotation part.
|
/// and a spherical interpolation for the rotation part.
|
||||||
|
@ -21,6 +21,7 @@ mod rotation_alga;
|
|||||||
mod rotation_alias;
|
mod rotation_alias;
|
||||||
mod rotation_construction;
|
mod rotation_construction;
|
||||||
mod rotation_conversion;
|
mod rotation_conversion;
|
||||||
|
mod rotation_interpolation;
|
||||||
mod rotation_ops;
|
mod rotation_ops;
|
||||||
mod rotation_simba; // TODO: implement Rotation methods.
|
mod rotation_simba; // TODO: implement Rotation methods.
|
||||||
mod rotation_specialization;
|
mod rotation_specialization;
|
||||||
@ -58,6 +59,7 @@ mod isometry_alga;
|
|||||||
mod isometry_alias;
|
mod isometry_alias;
|
||||||
mod isometry_construction;
|
mod isometry_construction;
|
||||||
mod isometry_conversion;
|
mod isometry_conversion;
|
||||||
|
mod isometry_interpolation;
|
||||||
mod isometry_ops;
|
mod isometry_ops;
|
||||||
mod isometry_simba;
|
mod isometry_simba;
|
||||||
|
|
||||||
@ -83,7 +85,6 @@ mod transform_simba;
|
|||||||
|
|
||||||
mod reflection;
|
mod reflection;
|
||||||
|
|
||||||
mod isometry_interpolation;
|
|
||||||
mod orthographic;
|
mod orthographic;
|
||||||
mod perspective;
|
mod perspective;
|
||||||
|
|
||||||
|
@ -23,6 +23,36 @@ use crate::base::{DefaultAllocator, MatrixN, Scalar, Unit, VectorN};
|
|||||||
use crate::geometry::Point;
|
use crate::geometry::Point;
|
||||||
|
|
||||||
/// A rotation matrix.
|
/// 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)]
|
#[repr(C)]
|
||||||
#[derive(Debug)]
|
#[derive(Debug)]
|
||||||
pub struct Rotation<N: Scalar, D: DimName>
|
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>
|
impl<N: Scalar, D: DimName> Rotation<N, D>
|
||||||
where
|
where
|
||||||
DefaultAllocator: Allocator<N, D, D>,
|
DefaultAllocator: Allocator<N, D, D>,
|
||||||
@ -225,39 +293,13 @@ where
|
|||||||
|
|
||||||
res
|
res
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// Creates a new rotation from the given square matrix.
|
/// # Transposition and inversion
|
||||||
///
|
impl<N: Scalar, D: DimName> Rotation<N, D>
|
||||||
/// The matrix squareness is checked but not its orthonormality.
|
where
|
||||||
///
|
DefaultAllocator: Allocator<N, D, D>,
|
||||||
/// # 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 }
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Transposes `self`.
|
/// Transposes `self`.
|
||||||
///
|
///
|
||||||
/// Same as `.inverse()` because the inverse of a rotation matrix is its transform.
|
/// 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>
|
impl<N: SimdRealField, D: DimName> Rotation<N, D>
|
||||||
where
|
where
|
||||||
N::Element: SimdRealField,
|
N::Element: SimdRealField,
|
||||||
|
@ -3,7 +3,11 @@ use crate::base::dimension::{U2, U3};
|
|||||||
use crate::geometry::Rotation;
|
use crate::geometry::Rotation;
|
||||||
|
|
||||||
/// A 2-dimensional rotation matrix.
|
/// 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>;
|
pub type Rotation2<N> = Rotation<N, U2>;
|
||||||
|
|
||||||
/// A 3-dimensional rotation matrix.
|
/// 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>;
|
pub type Rotation3<N> = Rotation<N, U3>;
|
||||||
|
@ -8,6 +8,7 @@ use crate::base::{DefaultAllocator, MatrixN, Scalar};
|
|||||||
|
|
||||||
use crate::geometry::Rotation;
|
use crate::geometry::Rotation;
|
||||||
|
|
||||||
|
/// # Identity
|
||||||
impl<N, D: DimName> Rotation<N, D>
|
impl<N, D: DimName> Rotation<N, D>
|
||||||
where
|
where
|
||||||
N: Scalar + Zero + One,
|
N: Scalar + Zero + One,
|
||||||
|
78
src/geometry/rotation_interpolation.rs
Normal file
78
src/geometry/rotation_interpolation.rs
Normal file
@ -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.
|
* 2D Rotation matrix.
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
/// # Construction from a 2D rotation angle
|
||||||
impl<N: SimdRealField> Rotation2<N> {
|
impl<N: SimdRealField> Rotation2<N> {
|
||||||
/// Builds a 2 dimensional rotation matrix from an angle in radian.
|
/// 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 {
|
pub fn from_scaled_axis<SB: Storage<N, U1>>(axisangle: Vector<N, U1, SB>) -> Self {
|
||||||
Self::new(axisangle[0])
|
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`.
|
/// 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
|
/// 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())
|
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 rotation matrix needed to make `self` and `other` coincide.
|
||||||
///
|
///
|
||||||
/// The result is such that: `self.rotation_to(other) * self == other`.
|
/// 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 {
|
pub fn powf(&self, n: N) -> Self {
|
||||||
Self::new(self.angle() * n)
|
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.
|
/// 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> {
|
pub fn scaled_axis(&self) -> VectorN<N, U1> {
|
||||||
Vector1::new(self.angle())
|
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
|
impl<N: SimdRealField> Distribution<Rotation2<N>> for Standard
|
||||||
@ -292,6 +274,7 @@ where
|
|||||||
* 3D Rotation matrix.
|
* 3D Rotation matrix.
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
/// # Construction from a 3D axis and/or angles
|
||||||
impl<N: SimdRealField> Rotation3<N>
|
impl<N: SimdRealField> Rotation3<N>
|
||||||
where
|
where
|
||||||
N::Element: SimdRealField,
|
N::Element: SimdRealField,
|
||||||
@ -325,60 +308,6 @@ where
|
|||||||
Self::from_axis_angle(&axis, angle)
|
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.
|
/// Builds a 3D rotation matrix from an axis scaled by the rotation angle.
|
||||||
///
|
///
|
||||||
/// This is the same as `Self::new(axisangle)`.
|
/// This is the same as `Self::new(axisangle)`.
|
||||||
@ -488,67 +417,13 @@ where
|
|||||||
cp * cr,
|
cp * cr,
|
||||||
))
|
))
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// Creates Euler angles from a rotation.
|
/// # Construction from a 3D eye position and target point
|
||||||
///
|
impl<N: SimdRealField> Rotation3<N>
|
||||||
/// The angles are produced in the form (roll, pitch, yaw).
|
where
|
||||||
#[deprecated(note = "This is renamed to use `.euler_angles()`.")]
|
N::Element: SimdRealField,
|
||||||
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())
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Creates a rotation that corresponds to the local frame of an observer standing at the
|
/// Creates a rotation that corresponds to the local frame of an observer standing at the
|
||||||
/// origin and looking toward `dir`.
|
/// origin and looking toward `dir`.
|
||||||
///
|
///
|
||||||
@ -656,7 +531,13 @@ where
|
|||||||
{
|
{
|
||||||
Self::face_towards(dir, up).inverse()
|
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.
|
/// 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()`.
|
/// 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())
|
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].
|
/// The rotation angle in [0; pi].
|
||||||
///
|
///
|
||||||
/// # Example
|
/// # Example
|
||||||
@ -837,103 +835,59 @@ where
|
|||||||
/// assert_relative_eq!(rot1.angle_to(&rot2), 1.0045657, epsilon = 1.0e-6);
|
/// assert_relative_eq!(rot1.angle_to(&rot2), 1.0045657, epsilon = 1.0e-6);
|
||||||
/// ```
|
/// ```
|
||||||
#[inline]
|
#[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()
|
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`.
|
/// The angles are produced in the form (roll, pitch, yaw).
|
||||||
///
|
#[deprecated(note = "This is renamed to use `.euler_angles()`.")]
|
||||||
/// # Example
|
pub fn to_euler_angles(&self) -> (N, N, N)
|
||||||
/// ```
|
|
||||||
/// # #[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
|
where
|
||||||
N: RealField,
|
N: RealField,
|
||||||
{
|
{
|
||||||
if let Some(axis) = self.axis() {
|
self.euler_angles()
|
||||||
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());
|
/// Euler angles corresponding to this rotation from a rotation.
|
||||||
Self::from_matrix_unchecked(minus_id)
|
///
|
||||||
|
/// 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 {
|
} 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
|
impl<N: SimdRealField> Distribution<Rotation3<N>> for Standard
|
||||||
|
Loading…
Reference in New Issue
Block a user