From 3d82c4335e1802290ce2b845dbc2ce3147f22aaf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Sun, 25 Oct 2020 11:23:34 +0100 Subject: [PATCH] Add inverse_transform_unit_vector to rotations and isometries. --- src/geometry/abstract_rotation.rs | 17 ++++++++++++++++- src/geometry/isometry.rs | 25 ++++++++++++++++++++++++- src/geometry/quaternion.rs | 20 ++++++++++++++++++++ src/geometry/rotation.rs | 21 ++++++++++++++++++++- src/geometry/unit_complex.rs | 16 ++++++++++++++++ 5 files changed, 96 insertions(+), 3 deletions(-) diff --git a/src/geometry/abstract_rotation.rs b/src/geometry/abstract_rotation.rs index ff057a09..52471851 100644 --- a/src/geometry/abstract_rotation.rs +++ b/src/geometry/abstract_rotation.rs @@ -1,6 +1,6 @@ use crate::allocator::Allocator; use crate::geometry::{Rotation, UnitComplex, UnitQuaternion}; -use crate::{DefaultAllocator, DimName, Point, Scalar, SimdRealField, VectorN, U2, U3}; +use crate::{DefaultAllocator, DimName, Point, Scalar, SimdRealField, Unit, VectorN, U2, U3}; use simba::scalar::ClosedMul; @@ -24,6 +24,13 @@ pub trait AbstractRotation: PartialEq + ClosedMul + Clone fn inverse_transform_vector(&self, v: &VectorN) -> VectorN where DefaultAllocator: Allocator; + /// Apply the inverse rotation to the given unit vector. + fn inverse_transform_unit_vector(&self, v: &Unit>) -> Unit> + where + DefaultAllocator: Allocator, + { + Unit::new_unchecked(self.inverse_transform_vector(&**v)) + } /// Apply the inverse rotation to the given point. fn inverse_transform_point(&self, p: &Point) -> Point where @@ -74,6 +81,14 @@ where self.inverse_transform_vector(v) } + #[inline] + fn inverse_transform_unit_vector(&self, v: &Unit>) -> Unit> + where + DefaultAllocator: Allocator, + { + self.inverse_transform_unit_vector(v) + } + #[inline] fn inverse_transform_point(&self, p: &Point) -> Point where diff --git a/src/geometry/isometry.rs b/src/geometry/isometry.rs index a4e16abe..b59c8b60 100755 --- a/src/geometry/isometry.rs +++ b/src/geometry/isometry.rs @@ -16,7 +16,7 @@ use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1}; use crate::base::storage::Owned; -use crate::base::{DefaultAllocator, MatrixN, Scalar, VectorN}; +use crate::base::{DefaultAllocator, MatrixN, Scalar, Unit, VectorN}; use crate::geometry::{AbstractRotation, Point, Translation}; /// 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. @@ -350,6 +350,29 @@ where pub fn inverse_transform_vector(&self, v: &VectorN) -> VectorN { self.rotation.inverse_transform_vector(v) } + + /// Transform the given unit vector by the inverse of this isometry, ignoring the + /// translation component of the isometry. This may be + /// less expensive than computing the entire isometry inverse and then + /// transforming the point. + /// + /// # Example + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use std::f32; + /// # use nalgebra::{Isometry3, Translation3, UnitQuaternion, Vector3}; + /// let tra = Translation3::new(0.0, 0.0, 3.0); + /// let rot = UnitQuaternion::from_scaled_axis(Vector3::z() * f32::consts::FRAC_PI_2); + /// let iso = Isometry3::from_parts(tra, rot); + /// + /// let transformed_point = iso.inverse_transform_unit_vector(&Vector3::x_axis()); + /// assert_relative_eq!(transformed_point, -Vector3::y_axis(), epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn inverse_transform_unit_vector(&self, v: &Unit>) -> Unit> { + self.rotation.inverse_transform_unit_vector(v) + } } // NOTE: we don't require `R: Rotation<...>` here because this is not useful for the implementation diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs index 0c996ede..22ee5309 100755 --- a/src/geometry/quaternion.rs +++ b/src/geometry/quaternion.rs @@ -1542,6 +1542,26 @@ where pub fn inverse_transform_vector(&self, v: &Vector3) -> Vector3 { self.inverse() * v } + + /// Rotate a vector by the inverse of this unit quaternion. This may be + /// cheaper than inverting the unit quaternion and transforming the + /// vector. + /// + /// # Example + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use std::f32; + /// # use nalgebra::{UnitQuaternion, Vector3}; + /// let rot = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), f32::consts::FRAC_PI_2); + /// let transformed_vector = rot.inverse_transform_unit_vector(&Vector3::x_axis()); + /// + /// assert_relative_eq!(transformed_vector, -Vector3::y_axis(), epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn inverse_transform_unit_vector(&self, v: &Unit>) -> Unit> { + self.inverse() * v + } } impl Default for UnitQuaternion { diff --git a/src/geometry/rotation.rs b/src/geometry/rotation.rs index bb3fc235..c30a3a97 100755 --- a/src/geometry/rotation.rs +++ b/src/geometry/rotation.rs @@ -19,7 +19,7 @@ use simba::simd::SimdRealField; use crate::base::allocator::Allocator; use crate::base::dimension::{DimName, DimNameAdd, DimNameSum, U1}; -use crate::base::{DefaultAllocator, MatrixN, Scalar, VectorN}; +use crate::base::{DefaultAllocator, MatrixN, Scalar, Unit, VectorN}; use crate::geometry::Point; /// A rotation matrix. @@ -441,6 +441,25 @@ where pub fn inverse_transform_vector(&self, v: &VectorN) -> VectorN { self.matrix().tr_mul(v) } + + /// Rotate the given vector by the inverse of this rotation. This may be + /// cheaper than inverting the rotation and then transforming the given + /// vector. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use std::f32; + /// # use nalgebra::{Rotation2, Rotation3, UnitQuaternion, Vector3}; + /// let rot = Rotation3::new(Vector3::z() * f32::consts::FRAC_PI_2); + /// let transformed_vector = rot.inverse_transform_unit_vector(&Vector3::x_axis()); + /// + /// assert_relative_eq!(transformed_vector, -Vector3::y_axis(), epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn inverse_transform_unit_vector(&self, v: &Unit>) -> Unit> { + Unit::new_unchecked(self.inverse_transform_vector(&**v)) + } } impl Eq for Rotation where DefaultAllocator: Allocator {} diff --git a/src/geometry/unit_complex.rs b/src/geometry/unit_complex.rs index a8ac5bcd..723ac6c0 100755 --- a/src/geometry/unit_complex.rs +++ b/src/geometry/unit_complex.rs @@ -360,6 +360,22 @@ where pub fn inverse_transform_vector(&self, v: &Vector2) -> Vector2 { self.inverse() * v } + + /// Rotate the given vector by the inverse of this unit complex number. + /// + /// # Example + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{UnitComplex, Vector2}; + /// # use std::f32; + /// let rot = UnitComplex::new(f32::consts::FRAC_PI_2); + /// let transformed_vector = rot.inverse_transform_unit_vector(&Vector2::x_axis()); + /// assert_relative_eq!(transformed_vector, -Vector2::y_axis(), epsilon = 1.0e-6); + /// ``` + #[inline] + pub fn inverse_transform_unit_vector(&self, v: &Unit>) -> Unit> { + self.inverse() * v + } } impl fmt::Display for UnitComplex {