Merge pull request #836 from dimforge/rotation_utils

Add various utilities for constructing/appending rotations

CHANGELOG.md

@@ -4,6 +4,13 @@ documented here.
 
 This project adheres to [Semantic Versioning](https://semver.org/).
 
+## [0.25.0] - WIP
+### Added
+* Add `from_basis_unchecked` to all the rotation types. This builds a rotation from a set of basis vectors (representing the columns of the corresponding rotation matrix).
+* Add `Matrix::cap_magnitude` to cap the magnitude of a vector.
+* Add `UnitQuaternion::append_axisangle_linearized` to approximately append a rotation represented as an axis-angle to a rotation represented as an unit quaternion.
+* Re-export `simba::simd::SimdValue` at the root of the `nalgebra` crate.
+
 ## [0.24.0]
 
 ### Added

Cargo.toml

@@ -8,7 +8,7 @@ documentation = "https://www.nalgebra.org/docs"
 homepage = "https://nalgebra.org"
 repository = "https://github.com/dimforge/nalgebra"
 readme = "README.md"
-categories = [ "science", "mathematics", "wasm", "no standard library" ]
+categories = [ "science", "mathematics", "wasm", "no-std" ]
 keywords = [ "linear", "algebra", "matrix", "vector", "math" ]
 license = "BSD-3-Clause"
 edition = "2018"

src/base/norm.rs

@@ -8,7 +8,7 @@ use crate::allocator::Allocator;
 use crate::base::{DefaultAllocator, Dim, DimName, Matrix, MatrixMN, Normed, VectorN};
 use crate::constraint::{SameNumberOfColumns, SameNumberOfRows, ShapeConstraint};
 use crate::storage::{Storage, StorageMut};
-use crate::{ComplexField, Scalar, SimdComplexField, Unit};
+use crate::{ComplexField, RealField, Scalar, SimdComplexField, Unit};
 use simba::scalar::ClosedNeg;
 use simba::simd::{SimdOption, SimdPartialOrd};
 
@@ -334,11 +334,27 @@ impl<N: Scalar, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
     {
         let n = self.norm();
 
-        if n >= min_magnitude {
+        if n > min_magnitude {
             self.scale_mut(magnitude / n)
         }
     }
 
+    /// Returns a new vector with the same magnitude as `self` clamped between `0.0` and `max`.
+    #[inline]
+    pub fn cap_magnitude(&self, max: N::RealField) -> MatrixMN<N, R, C>
+    where
+        N: RealField,
+        DefaultAllocator: Allocator<N, R, C>,
+    {
+        let n = self.norm();
+
+        if n > max {
+            self.scale(max / n)
+        } else {
+            self.clone_owned()
+        }
+    }
+
     /// Returns a normalized version of this matrix unless its norm as smaller or equal to `eps`.
     ///
     /// The components of this matrix cannot be SIMD types (see `simd_try_normalize`) instead.

src/geometry/quaternion.rs

@@ -1542,6 +1542,17 @@ where
     pub fn inverse_transform_unit_vector(&self, v: &Unit<Vector3<N>>) -> Unit<Vector3<N>> {
         self.inverse() * v
     }
+
+    /// Appends to `self` a rotation given in the axis-angle form, using a linearized formulation.
+    ///
+    /// This is faster, but approximate, way to compute `UnitQuaternion::new(axisangle) * self`.
+    #[inline]
+    pub fn append_axisangle_linearized(&self, axisangle: &Vector3<N>) -> Self {
+        let half: N = crate::convert(0.5);
+        let q1 = self.into_inner();
+        let q2 = Quaternion::from_imag(axisangle * half);
+        Unit::new_normalize(q1 + q2 * q1)
+    }
 }
 
 impl<N: RealField> Default for UnitQuaternion<N> {

src/geometry/quaternion_construction.rs

@@ -266,6 +266,17 @@ where
         Self::new_unchecked(q)
     }
 
+    /// Builds an unit quaternion from a basis assumed to be orthonormal.
+    ///
+    /// In order to get a valid unit-quaternion, the input must be an
+    /// orthonormal basis, i.e., all vectors are normalized, and the are
+    /// all orthogonal to each other. These invariants are not checked
+    /// by this method.
+    pub fn from_basis_unchecked(basis: &[Vector3<N>; 3]) -> Self {
+        let rot = Rotation3::from_basis_unchecked(basis);
+        Self::from_rotation_matrix(&rot)
+    }
+
     /// Builds an unit quaternion from a rotation matrix.
     ///
     /// # Example

src/geometry/rotation_specialization.rs

@@ -12,7 +12,7 @@ use std::ops::Neg;
 
 use crate::base::dimension::{U1, U2, U3};
 use crate::base::storage::Storage;
-use crate::base::{Matrix2, Matrix3, MatrixN, Unit, Vector, Vector1, Vector3, VectorN};
+use crate::base::{Matrix2, Matrix3, MatrixN, Unit, Vector, Vector1, Vector2, Vector3, VectorN};
 
 use crate::geometry::{Rotation2, Rotation3, UnitComplex, UnitQuaternion};
 
@@ -53,6 +53,17 @@ impl<N: SimdRealField> Rotation2<N> {
 
 /// # Construction from an existing 2D matrix or rotations
 impl<N: SimdRealField> Rotation2<N> {
+    /// Builds a rotation from a basis assumed to be orthonormal.
+    ///
+    /// In order to get a valid unit-quaternion, the input must be an
+    /// orthonormal basis, i.e., all vectors are normalized, and the are
+    /// all orthogonal to each other. These invariants are not checked
+    /// by this method.
+    pub fn from_basis_unchecked(basis: &[Vector2<N>; 2]) -> Self {
+        let mat = Matrix2::from_columns(&basis[..]);
+        Self::from_matrix_unchecked(mat)
+    }
+
     /// 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
@@ -655,6 +666,17 @@ where
         }
     }
 
+    /// Builds a rotation from a basis assumed to be orthonormal.
+    ///
+    /// In order to get a valid unit-quaternion, the input must be an
+    /// orthonormal basis, i.e., all vectors are normalized, and the are
+    /// all orthogonal to each other. These invariants are not checked
+    /// by this method.
+    pub fn from_basis_unchecked(basis: &[Vector3<N>; 3]) -> Self {
+        let mat = Matrix3::from_columns(&basis[..]);
+        Self::from_matrix_unchecked(mat)
+    }
+
     /// 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

src/geometry/unit_complex_construction.rs

@@ -8,7 +8,7 @@ use rand::Rng;
 
 use crate::base::dimension::{U1, U2};
 use crate::base::storage::Storage;
-use crate::base::{Matrix2, Unit, Vector};
+use crate::base::{Matrix2, Unit, Vector, Vector2};
 use crate::geometry::{Rotation2, UnitComplex};
 use simba::scalar::RealField;
 use simba::simd::SimdRealField;
@@ -164,6 +164,18 @@ where
         Self::new_unchecked(Complex::new(rotmat[(0, 0)], rotmat[(1, 0)]))
     }
 
+    /// Builds a rotation from a basis assumed to be orthonormal.
+    ///
+    /// In order to get a valid unit-quaternion, the input must be an
+    /// orthonormal basis, i.e., all vectors are normalized, and the are
+    /// all orthogonal to each other. These invariants are not checked
+    /// by this method.
+    pub fn from_basis_unchecked(basis: &[Vector2<N>; 2]) -> Self {
+        let mat = Matrix2::from_columns(&basis[..]);
+        let rot = Rotation2::from_matrix_unchecked(mat);
+        Self::from_rotation_matrix(&rot)
+    }
+
     /// Builds an unit complex by extracting the rotation part of the given transformation `m`.
     ///
     /// This is an iterative method. See `.from_matrix_eps` to provide mover

src/lib.rs

@@ -152,7 +152,7 @@ pub use num_complex::Complex;
 pub use simba::scalar::{
     ClosedAdd, ClosedDiv, ClosedMul, ClosedSub, ComplexField, Field, RealField,
 };
-pub use simba::simd::{SimdBool, SimdComplexField, SimdPartialOrd, SimdRealField};
+pub use simba::simd::{SimdBool, SimdComplexField, SimdPartialOrd, SimdRealField, SimdValue};
 
 /// Gets the multiplicative identity element.
 ///