diff --git a/src/base/cg.rs b/src/base/cg.rs index 650e4c1b3..8e0f028cc 100644 --- a/src/base/cg.rs +++ b/src/base/cg.rs @@ -69,6 +69,64 @@ where } } +/// # Rotation in any dimension +impl OMatrix +where + T: RealField, + D: DimName, + DefaultAllocator: Allocator + Allocator + Allocator, +{ + /// The n-dimensional rotation matrix described by an oriented minor arc. + /// + /// See [`Rotation::from_arc`](`crate::Rotation::from_arc`) for details. + #[must_use] + pub fn from_arc( + from: &Unit>, + to: &Unit>, + ) -> Option + where + SB: Storage + Clone, + SC: Storage + Clone, + { + let (a, b, ab) = (from.as_ref(), to.as_ref(), from.dot(to)); + let d = T::one() + ab.clone(); + (d > T::default_epsilon().sqrt()).then(|| { + let [at, bt] = &[a.transpose(), b.transpose()]; + let [b_at, a_bt, a_at, b_bt] = &[b * at, a * bt, a * at, b * bt]; + let [k1, k2] = [b_at - a_bt, (b_at + a_bt) * ab - (a_at + b_bt)]; + // Codesido's Rotation Formula + // + Self::identity() + k1 + k2 / d + }) + } + /// The n-dimensional rotation matrix described by an oriented minor arc and a signed angle. + /// + /// See [`Rotation::from_arc_angle`](`crate::Rotation::from_arc_angle`) for details. + #[must_use] + pub fn from_arc_angle( + from: &Unit>, + to: &Unit>, + angle: T, + ) -> Option + where + SB: Storage + Clone, + SC: Storage + Clone, + { + let (a, b, ab) = (from.as_ref(), to.as_ref(), from.dot(to)); + // Tries making `b` orthonormal to `a` via Gram-Schmidt process. + (b - a * ab) + .try_normalize(T::default_epsilon().sqrt()) + .map(|ref b| { + let (sin, cos) = angle.sin_cos(); + let [at, bt] = &[a.transpose(), b.transpose()]; + let [k1, k2] = [b * at - a * bt, -(a * at + b * bt)]; + // Simple rotations / Rotation in a two–plane + // + Self::identity() + k1 * sin + k2 * (T::one() - cos) + }) + } +} + /// # 2D transformations as a Matrix3 impl Matrix3 { /// Builds a 2 dimensional homogeneous rotation matrix from an angle in radian. diff --git a/src/base/matrix.rs b/src/base/matrix.rs index 945038cdd..91e48d766 100644 --- a/src/base/matrix.rs +++ b/src/base/matrix.rs @@ -81,6 +81,7 @@ pub type MatrixCross = /// - [2D transformations as a Matrix3 `new_rotation`…](#2d-transformations-as-a-matrix3) /// - [3D transformations as a Matrix4 `new_rotation`, `new_perspective`, `look_at_rh`…](#3d-transformations-as-a-matrix4) /// - [Translation and scaling in any dimension `new_scaling`, `new_translation`…](#translation-and-scaling-in-any-dimension) +/// - [Rotation in any dimension `from_arc`, `from_arc_angle`](#rotation-in-any-dimension) /// - [Append/prepend translation and scaling `append_scaling`, `prepend_translation_mut`…](#appendprepend-translation-and-scaling) /// - [Transformation of vectors and points `transform_vector`, `transform_point`…](#transformation-of-vectors-and-points) /// diff --git a/src/geometry/rotation.rs b/src/geometry/rotation.rs index 4e90ff7a0..4e0d6fec5 100644 --- a/src/geometry/rotation.rs +++ b/src/geometry/rotation.rs @@ -22,14 +22,14 @@ use rkyv::bytecheck; /// 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. +/// This is also known as an element of a Special Orthogonal (SO) group. The [`Rotation`] type can +/// represent an n-dimensional rotation 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. +/// For fixed dimensions, you should use its aliases (e.g., [`Rotation2`](crate::Rotation2) or +/// [`Rotation3`](crate::Rotation3)) instead of the dimension-generic [`Rotation`](crate::Rotation) +/// type. Though keep in mind that all the documentation of all the methods of these aliases will +/// also appears on this page. /// /// # Construction /// * [Identity `identity`](#identity) @@ -38,6 +38,7 @@ use rkyv::bytecheck; /// * [From a 3D axis and/or angles `new`, `from_euler_angles`, `from_axis_angle`…](#construction-from-a-3d-axis-andor-angles) /// * [From a 3D eye position and target point `look_at`, `look_at_lh`, `rotation_between`…](#construction-from-a-3d-eye-position-and-target-point) /// * [From an existing 3D matrix or rotations `from_matrix`, `rotation_between`, `powf`…](#construction-from-an-existing-3d-matrix-or-rotations) +/// * [From an arc in any dimension `from_arc`, `from_arc_angle`…](#construction-in-any-dimension) /// /// # Transformation and composition /// Note that transforming vectors and points can be done by multiplication, e.g., `rotation * point`. diff --git a/src/geometry/rotation_alias.rs b/src/geometry/rotation_alias.rs index f44e24bc1..170bf8945 100644 --- a/src/geometry/rotation_alias.rs +++ b/src/geometry/rotation_alias.rs @@ -2,10 +2,25 @@ 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.** +/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type, too.** pub type Rotation2 = Rotation; /// A 3-dimensional rotation matrix. /// -/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type too.** +/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type, too.** pub type Rotation3 = Rotation; + +/// A 4-dimensional rotation matrix. +/// +/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type, too.** +pub type Rotation4 = Rotation; + +/// A 5-dimensional rotation matrix. +/// +/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type, too.** +pub type Rotation5 = Rotation; + +/// A 6-dimensional rotation matrix. +/// +/// **Because this is an alias, not all its methods are listed here. See the [`Rotation`](crate::Rotation) type, too.** +pub type Rotation6 = Rotation; diff --git a/src/geometry/rotation_construction.rs b/src/geometry/rotation_construction.rs index 7204c0d44..20048435a 100644 --- a/src/geometry/rotation_construction.rs +++ b/src/geometry/rotation_construction.rs @@ -1,8 +1,8 @@ use num::{One, Zero}; -use simba::scalar::{ClosedAddAssign, ClosedMulAssign, SupersetOf}; +use simba::scalar::{ClosedAddAssign, ClosedMulAssign, RealField, SupersetOf}; -use crate::base::{SMatrix, Scalar}; +use crate::base::{storage::Storage, Const, SMatrix, Scalar, Unit, Vector}; use crate::geometry::Rotation; @@ -44,6 +44,99 @@ where } } +/// # Construction in any dimension +impl Rotation { + /// The n-dimensional rotation matrix described by an oriented minor arc. + /// + /// This is the rotation `rot` aligning `from` with `to` over their minor angle such that + /// `(rot * from).angle(to) == 0` and `(rot * from).dot(to).is_positive()`. + /// + /// Returns `None` with `from` and `to` being anti-parallel. In contrast to + /// [`Self::from_arc_angle`], this method is robust for approximately parallel vectors + /// continuously approaching identity. + /// + /// See also [`OMatrix::from_arc`](`crate::OMatrix::from_arc`) for owned matrices generic over + /// storage. + /// + /// # Example + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{Rotation, Unit, Vector6}; + /// let from = Unit::new_normalize(Vector6::new(-4.0, -2.4, 0.0, -3.3, -1.0, -9.0)); + /// let to = Unit::new_normalize(Vector6::new(3.0, 1.0, 2.0, 2.0, 9.0, 6.0)); + /// + /// // Aligns `from` with `to`. + /// let rot = Rotation::from_arc(&from, &to).unwrap(); + /// assert_relative_eq!(rot * from, to, epsilon = 1.0e-6); + /// assert_relative_eq!(rot.inverse() * to, from, epsilon = 1.0e-6); + /// + /// // Returns identity with `from` and `to` being parallel. + /// let rot = Rotation::from_arc(&from, &from).unwrap(); + /// assert_relative_eq!(rot, Rotation::identity(), epsilon = 1.0e-6); + /// + /// // Returns `None` with `from` and `to` being anti-parallel. + /// assert!(Rotation::from_arc(&from, &-from).is_none()); + /// ``` + #[must_use] + #[inline] + pub fn from_arc( + from: &Unit, SB>>, + to: &Unit, SC>>, + ) -> Option + where + T: RealField, + SB: Storage> + Clone, + SC: Storage> + Clone, + { + SMatrix::from_arc(from, to).map(Self::from_matrix_unchecked) + } + /// The n-dimensional rotation matrix described by an oriented minor arc and a signed angle. + /// + /// Returns `None` with `from` and `to` being collinear. This method is more robust, the less + /// `from` and `to` are collinear, regardless of `angle`. + /// + /// See also [`Self::from_arc`] aligning `from` with `to` over their minor angle. + /// + /// See also [`OMatrix::from_arc_angle`](`crate::OMatrix::from_arc_angle`) for owned matrices + /// generic over storage. + /// + /// # Example + /// + /// ``` + /// # #[macro_use] extern crate approx; + /// # use nalgebra::{Rotation, Unit, Vector6}; + /// let from = Unit::new_normalize(Vector6::new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0)); + /// let to = Unit::new_normalize(Vector6::new(3.0, 1.0, 2.0, 5.0, 9.0, 4.0)); + /// + /// // Rotates by signed angle where `from` and `to` define its orientation. + /// let angle = 70f64.to_radians(); + /// let rot = Rotation::from_arc_angle(&from, &to, angle).unwrap(); + /// assert_relative_eq!((rot * from).angle(&from), angle, epsilon = 1.0e-6); + /// assert_relative_eq!((rot.inverse() * to).angle(&to), angle, epsilon = 1.0e-6); + /// let inv = Rotation::from_arc_angle(&from, &to, -angle).unwrap(); + /// assert_relative_eq!(rot.inverse(), inv, epsilon = 1.0e-6); + /// + /// // Returns `None` with `from` and `to` being collinear. + /// assert!(Rotation::from_arc_angle(&from, &from, angle).is_none()); + /// assert!(Rotation::from_arc_angle(&from, &-from, angle).is_none()); + /// ``` + #[must_use] + #[inline] + pub fn from_arc_angle( + from: &Unit, SB>>, + to: &Unit, SC>>, + angle: T, + ) -> Option + where + T: RealField, + SB: Storage> + Clone, + SC: Storage> + Clone, + { + SMatrix::from_arc_angle(from, to, angle).map(Self::from_matrix_unchecked) + } +} + impl Rotation { /// Cast the components of `self` to another type. ///