diff --git a/CHANGELOG.md b/CHANGELOG.md index 1c158452..853529f0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,8 @@ This project adheres to [Semantic Versioning](https://semver.org/). others set to zero. * The `Isometry.lerp_slerp` and `Isometry.try_lerp_slerp` methods to interpolate between two isometries using linear interpolation for the translational part, and spherical interpolation for the rotational part. + * The `Rotation2.slerp`, `Rotation3.slerp`, and `UnitQuaternion.slerp` method for + spherical interpolation. ## [0.22.0] In this release, we are using the new version 0.2 of simba. One major change of that version is that the diff --git a/src/geometry/abstract_rotation.rs b/src/geometry/abstract_rotation.rs index e1cf7c10..52471851 100644 --- a/src/geometry/abstract_rotation.rs +++ b/src/geometry/abstract_rotation.rs @@ -35,14 +35,6 @@ pub trait AbstractRotation: PartialEq + ClosedMul + Clone fn inverse_transform_point(&self, p: &Point) -> Point where DefaultAllocator: Allocator; - /// Perfom a spherical interpolation between two rolations. - fn slerp(&self, other: &Self, t: N) -> Self - where - DefaultAllocator: Allocator; - /// Attempts to perfom a spherical interpolation between two rolations. - fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option - where - DefaultAllocator: Allocator; } impl AbstractRotation for Rotation @@ -104,22 +96,6 @@ where { self.inverse_transform_point(p) } - - #[inline] - fn slerp(&self, other: &Self, t: N) -> Self - where - DefaultAllocator: Allocator, - { - self.slerp(other, t) - } - - #[inline] - fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option - where - DefaultAllocator: Allocator, - { - self.try_slerp(other, t, epsilon) - } } impl AbstractRotation for UnitQuaternion @@ -160,16 +136,6 @@ where fn inverse_transform_point(&self, p: &Point) -> Point { self.inverse_transform_point(p) } - - #[inline] - fn slerp(&self, other: &Self, t: N) -> Self { - self.slerp(other, t) - } - - #[inline] - fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option { - self.try_slerp(other, t, epsilon) - } } impl AbstractRotation for UnitComplex @@ -210,14 +176,4 @@ where fn inverse_transform_point(&self, p: &Point) -> Point { self.inverse_transform_point(p) } - - #[inline] - fn slerp(&self, other: &Self, t: N) -> Self { - self.slerp(other, t) - } - - #[inline] - fn try_slerp(&self, other: &Self, t: N, epsilon: N) -> Option { - self.try_slerp(other, t, epsilon) - } } diff --git a/src/geometry/isometry.rs b/src/geometry/isometry.rs index 5b7e5dc7..c8eb34fb 100755 --- a/src/geometry/isometry.rs +++ b/src/geometry/isometry.rs @@ -14,10 +14,12 @@ use simba::scalar::{RealField, SubsetOf}; use simba::simd::SimdRealField; use crate::base::allocator::Allocator; -use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1}; +use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1, U2, U3}; use crate::base::storage::Owned; use crate::base::{DefaultAllocator, MatrixN, Scalar, Unit, VectorN}; -use crate::geometry::{AbstractRotation, Point, Translation}; +use crate::geometry::{ + AbstractRotation, Point, Rotation2, Rotation3, Translation, UnitComplex, UnitQuaternion, +}; /// A direct isometry, i.e., a rotation followed by a translation, aka. a rigid-body motion, aka. an element of a Special Euclidean (SE) group. #[repr(C)] @@ -373,7 +375,9 @@ where pub fn inverse_transform_unit_vector(&self, v: &Unit>) -> Unit> { self.rotation.inverse_transform_unit_vector(v) } +} +impl Isometry> { /// Interpolates between two isometries using a linear interpolation for the translation part, /// and a spherical interpolation for the rotation part. /// @@ -383,7 +387,7 @@ where /// # Examples: /// /// ``` - /// # use nalgebra::geometry::{Translation3, UnitQuaternion}; + /// # use nalgebra::geometry::{Vector3, Translation3, UnitQuaternion}; /// /// let t1 = Translation3::new(1.0, 2.0, 3.0); /// let t2 = Translation3::new(3.0, 6.0, 9.0); @@ -416,7 +420,7 @@ where /// # Examples: /// /// ``` - /// # use nalgebra::geometry::{Translation3, UnitQuaternion}; + /// # use nalgebra::geometry::{Vector3, Translation3, UnitQuaternion}; /// /// let t1 = Translation3::new(1.0, 2.0, 3.0); /// let t2 = Translation3::new(3.0, 6.0, 9.0); @@ -441,6 +445,144 @@ where } } +impl Isometry> { + /// Interpolates between two isometries using a linear interpolation for the translation part, + /// and a spherical interpolation for the rotation part. + /// + /// Panics if the angle between both rotations is 180 degrees (in which case the interpolation + /// is not well-defined). Use `.try_lerp_slerp` instead to avoid the panic. + /// + /// # Examples: + /// + /// ``` + /// # use nalgebra::geometry::{Vector3, Translation3, Rotation3}; + /// + /// let t1 = Translation3::new(1.0, 2.0, 3.0); + /// let t2 = Translation3::new(3.0, 6.0, 9.0); + /// 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 iso1 = Isometry3::from_parts(t1, q1); + /// let iso2 = Isometry3::from_parts(t2, q2); + /// + /// let iso3 = iso1.lerp_slerp(&iso2, 1.0 / 3.0); + /// + /// assert_eq!(iso3.translation_vector, Vector3::new(2.0, 4.0, 6.0)); + /// assert_eq!(iso3.rotation.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0)); + /// ``` + #[inline] + pub fn lerp_slerp(&self, other: &Self, t: N) -> Self + where + N: RealField, + { + let tr = self.translation.vector.lerp(&other.translation.vector, t); + let rot = self.rotation.slerp(&other.rotation, t); + Self::from_parts(tr.into(), rot) + } + + /// Attempts to interpolate between two isometries using a linear interpolation for the translation part, + /// and a spherical interpolation for the rotation part. + /// + /// Retuns `None` if the angle between both rotations is 180 degrees (in which case the interpolation + /// is not well-defined). + /// + /// # Examples: + /// + /// ``` + /// # use nalgebra::geometry::{Vector3, Translation3, Rotation3}; + /// + /// let t1 = Translation3::new(1.0, 2.0, 3.0); + /// let t2 = Translation3::new(3.0, 6.0, 9.0); + /// 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 iso1 = Isometry3::from_parts(t1, q1); + /// let iso2 = Isometry3::from_parts(t2, q2); + /// + /// let iso3 = iso1.lerp_slerp(&iso2, 1.0 / 3.0); + /// + /// assert_eq!(iso3.translation_vector, Vector3::new(2.0, 4.0, 6.0)); + /// assert_eq!(iso3.rotation.euler_angles(), (std::f32::consts::FRAC_PI_2, 0.0, 0.0)); + /// ``` + #[inline] + pub fn try_lerp_slerp(&self, other: &Self, t: N, epsilon: N) -> Option + where + N: RealField, + { + let tr = self.translation.vector.lerp(&other.translation.vector, t); + let rot = self.rotation.try_slerp(&other.rotation, t, epsilon)?; + Some(Self::from_parts(tr.into(), rot)) + } +} + +impl Isometry> { + /// Interpolates between two isometries using a linear interpolation for the translation part, + /// and a spherical interpolation for the rotation part. + /// + /// Panics if the angle between both rotations is 180 degrees (in which case the interpolation + /// is not well-defined). Use `.try_lerp_slerp` instead to avoid the panic. + /// + /// # Examples: + /// + /// ``` + /// # use nalgebra::geometry::{Vector2, Translation2, UnitComplex, Isometry2}; + /// + /// let t1 = Translation2::new(1.0, 2.0); + /// let t2 = Translation2::new(3.0, 6.0); + /// let q1 = UnitComplex::new(std::f32::consts::FRAC_PI_4); + /// let q2 = UnitComplex::new(-std::f32::consts::PI); + /// let iso1 = Isometry2::from_parts(t1, q1); + /// let iso2 = Isometry2::from_parts(t2, q2); + /// + /// let iso3 = iso1.lerp_slerp(&iso2, 1.0 / 3.0); + /// + /// assert_eq!(iso3.translation_vector, Vector2::new(2.0, 4.0)); + /// assert_eq!(iso3.rotation.angle(), std::f32::consts::FRAC_PI_2); + /// ``` + #[inline] + pub fn slerp(&self, other: &Self, t: N) -> Self + where + N: RealField, + { + let tr = self.translation.vector.lerp(&other.translation.vector, t); + let rot = self.rotation.slerp(&other.rotation, t); + Self::from_parts(tr.into(), rot) + } +} + +impl Isometry> { + /// Interpolates between two isometries using a linear interpolation for the translation part, + /// and a spherical interpolation for the rotation part. + /// + /// Panics if the angle between both rotations is 180 degrees (in which case the interpolation + /// is not well-defined). Use `.try_lerp_slerp` instead to avoid the panic. + /// + /// # Examples: + /// + /// ``` + /// # use nalgebra::geometry::{Vector2, Translation2, Rotation2, Isometry2}; + /// + /// let t1 = Translation2::new(1.0, 2.0); + /// let t2 = Translation2::new(3.0, 6.0); + /// let q1 = Rotation2::new(std::f32::consts::FRAC_PI_4); + /// let q2 = Rotation2::new(-std::f32::consts::PI); + /// let iso1 = Isometry2::from_parts(t1, q1); + /// let iso2 = Isometry2::from_parts(t2, q2); + /// + /// let iso3 = iso1.lerp_slerp(&iso2, 1.0 / 3.0); + /// + /// assert_eq!(iso3.translation_vector, Vector2::new(2.0, 4.0)); + /// assert_eq!(iso3.rotation.angle(), std::f32::consts::FRAC_PI_2); + /// ``` + #[inline] + pub fn slerp(&self, other: &Self, t: N) -> Self + where + N: RealField, + { + let tr = self.translation.vector.lerp(&other.translation.vector, t); + let rot = self.rotation.slerp(&other.rotation, t); + Self::from_parts(tr.into(), rot) + } +} + // NOTE: we don't require `R: Rotation<...>` here because this is not useful for the implementation // and makes it hard to use it, e.g., for Transform × Isometry implementation. // This is OK since all constructors of the isometry enforce the Rotation bound already (and diff --git a/src/geometry/rotation_specialization.rs b/src/geometry/rotation_specialization.rs index 1ee4a9df..7f140c8b 100644 --- a/src/geometry/rotation_specialization.rs +++ b/src/geometry/rotation_specialization.rs @@ -236,6 +236,30 @@ impl Rotation2 { pub fn scaled_axis(&self) -> VectorN { Vector1::new(self.angle()) } + + /// Spherical linear interpolation between two rotation matrices. + /// + /// # Examples: + /// + /// ``` + /// # 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_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 @@ -862,6 +886,53 @@ where Self::identity() } } + + /// 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 diff --git a/src/geometry/unit_complex.rs b/src/geometry/unit_complex.rs index 723ac6c0..f6a4f619 100755 --- a/src/geometry/unit_complex.rs +++ b/src/geometry/unit_complex.rs @@ -376,6 +376,26 @@ where pub fn inverse_transform_unit_vector(&self, v: &Unit>) -> Unit> { self.inverse() * v } + + /// Spherical linear interpolation between two rotations represented as unit complex numbers. + /// + /// # Examples: + /// + /// ``` + /// # use nalgebra::geometry::UnitComplex; + /// + /// let rot1 = UnitComplex::new(std::f32::consts::FRAC_PI_4); + /// let rot2 = UnitComplex::new(-std::f32::consts::PI); + /// + /// let rot = rot1.slerp(&rot2, 1.0 / 3.0); + /// + /// assert_eq!(rot.angle(), std::f32::consts::FRAC_PI_2); + /// ``` + + #[inline] + pub fn slerp(&self, other: &Self, t: N) -> Self { + Self::new(self.angle() * (N::one() - t) + other.angle() * t) + } } impl fmt::Display for UnitComplex {