Quat::from_rotation_arc (#129)

* Add Vec3::any_orthogonal and Vec3::any_orthonormal

* Add Quat::from_rotation_arc

* Document that Quat::from_axis_angle requires a normalized axis

benches/vec3.rs

@@ -97,6 +97,8 @@ op => vec3_into_tuple,
 from => random_vec3
 );
 
+// ---
+
 #[inline]
 fn vec3_normalize(v: Vec3) -> Vec3 {
     v.normalize()
@@ -121,6 +123,46 @@ bench_func!(
     from => random_vec3
 );
 
+// ---
+
+#[inline(always)]
+fn vec3_any_orthogonal_vector(v: Vec3) -> Vec3 {
+    v.any_orthogonal_vector()
+}
+
+bench_func!(
+    vec3_any_orthogonal_vector_bench,
+    "vec3 any_orthogonal_vector",
+    op => vec3_any_orthogonal_vector,
+    from => random_vec3
+);
+
+#[inline(always)]
+fn vec3_any_orthonormal_vector(v: Vec3) -> Vec3 {
+    v.any_orthonormal_vector()
+}
+
+bench_func!(
+    vec3_any_orthonormal_vector_bench,
+    "vec3 any_orthonormal_vector",
+    op => vec3_any_orthonormal_vector,
+    from => random_vec3
+);
+
+#[inline(always)]
+fn vec3_any_orthonormal_pair(v: Vec3) -> (Vec3, Vec3) {
+    v.any_orthonormal_pair()
+}
+
+bench_func!(
+    vec3_any_orthonormal_pair_bench,
+    "vec3 any_orthonormal_pair",
+    op => vec3_any_orthonormal_pair,
+    from => random_vec3
+);
+
+// ---
+
 euler!(vec3_euler, "vec3 euler", ty => Vec3, storage => Vec3, zero => Vec3::ZERO, rand => random_vec3);
 
 bench_binop!(
@@ -149,6 +191,9 @@ criterion_group!(
     vec3_angle_between,
     vec3_normalize_bench,
     vec3_normalize_or_zero_bench,
+    vec3_any_orthogonal_vector_bench,
+    vec3_any_orthonormal_vector_bench,
+    vec3_any_orthonormal_pair_bench,
     vec3_euler,
     vec3_select,
     vec3_to_array_fields,

src/quat.rs

@@ -29,7 +29,7 @@ use core::{
 use std::iter::{Product, Sum};
 
 macro_rules! impl_quat_methods {
-    ($t:ty, $quat:ident, $vec3:ident, $mat3:ident, $mat4:ident, $inner:ident) => {
+    ($t:ident, $quat:ident, $vec3:ident, $mat3:ident, $mat4:ident, $inner:ident) => {
         /// The identity quaternion. Corresponds to no rotation.
         pub const IDENTITY: Self = Self($inner::W);
 
@@ -78,6 +78,7 @@ macro_rules! impl_quat_methods {
         }
 
         /// Create a quaterion for a normalized rotation axis and angle (in radians).
+        /// The axis must be normalized (unit-length).
         #[inline(always)]
         pub fn from_axis_angle(axis: $vec3, angle: $t) -> Self {
             Self($inner::from_axis_angle(axis.0, angle))
@@ -128,6 +129,52 @@ macro_rules! impl_quat_methods {
             ))
         }
 
+        /// Gets the minimal rotation for transforming `from` to `to`.
+        /// The rotation is in the plane spanned by the two vectors.
+        /// Will rotate at most 180 degrees.
+        ///
+        /// The input vectors must be normalized (unit-length).
+        ///
+        /// `from_rotation_arc(from, to) * from ≈ to`.
+        ///
+        /// For near-singular cases (from≈to and from≈-to) the current implementation
+        /// is only accurate to about 0.001 (for `f32`).
+        pub fn from_rotation_arc(from: $vec3, to: $vec3) -> Self {
+            glam_assert!(from.is_normalized());
+            glam_assert!(to.is_normalized());
+
+            let one_minus_eps = 1.0 - 2.0 * core::$t::EPSILON;
+            let dot = from.dot(to);
+            if dot > one_minus_eps {
+                // 0° singulary: from ≈ to
+                Self::IDENTITY
+            } else if dot < -one_minus_eps {
+                // 180° singulary: from ≈ -to
+                let pi = core::$t::consts::PI; // half a turn = 𝛕/2 = 180°
+                Self::from_axis_angle(from.any_orthonormal_vector(), pi)
+            } else {
+                let c = from.cross(to);
+                Self::from_xyzw(c.x, c.y, c.z, 1.0 + dot).normalize()
+            }
+        }
+
+        /// Gets the minimal rotation for transforming `from` to either `to` or `-to`.
+        /// This means that the resulting quaternion will rotate `from` so that it is colinear with `to`.
+        ///
+        /// The rotation is in the plane spanned by the two vectors.
+        /// Will rotate at most 90 degrees.
+        ///
+        /// The input vectors must be normalized (unit-length).
+        ///
+        /// `to.dot(from_rotation_arc_colinear(from, to) * from).abs() ≈ 1`.
+        pub fn from_rotation_arc_colinear(from: $vec3, to: $vec3) -> Self {
+            if from.dot(to) < 0.0 {
+                Self::from_rotation_arc(from, -to)
+            } else {
+                Self::from_rotation_arc(from, to)
+            }
+        }
+
         /// Returns the rotation axis and angle of `self`.
         #[inline(always)]
         pub fn to_axis_angle(self) -> ($vec3, $t) {

src/vec3.rs

@@ -193,6 +193,49 @@ macro_rules! impl_vec3_float_methods {
         pub fn angle_between(self, other: Self) -> $t {
             self.0.angle_between(other.0)
         }
+
+        /// Returns somes vector that is orthogonal to the given one.
+        ///
+        /// The input vector must be finite and non-zero.
+        ///
+        /// The output vector is not necessarily unit-length.
+        /// For that use [`Self::any_orthonormal_vector`] instead.
+        #[inline]
+        pub fn any_orthogonal_vector(&self) -> Self {
+            // This can probably be optimized
+            if self.x.abs() > self.y.abs() {
+                Self::new(-self.z, 0.0, self.x) // self.cross(Self::Y)
+            } else {
+                Self::new(0.0, self.z, -self.y) // self.cross(Self::X)
+            }
+        }
+
+        /// Returns any unit-length vector that is orthogonal to the given one.
+        /// The input vector must be finite and non-zero.
+        #[inline]
+        pub fn any_orthonormal_vector(&self) -> Self {
+            glam_assert!(self.is_normalized());
+            // From https://graphics.pixar.com/library/OrthonormalB/paper.pdf
+            let sign = (1.0 as $t).copysign(self.z);
+            let a = -1.0 / (sign + self.z);
+            let b = self.x * self.y * a;
+            Self::new(b, sign + self.y * self.y * a, -self.y)
+        }
+
+        /// Given a unit-length vector return two other vectors that together form an orthonormal basis.
+        /// That is, all three vectors are orthogonal to each other and are normalized.
+        #[inline]
+        pub fn any_orthonormal_pair(&self) -> (Self, Self) {
+            glam_assert!(self.is_normalized());
+            // From https://graphics.pixar.com/library/OrthonormalB/paper.pdf
+            let sign = (1.0 as $t).copysign(self.z);
+            let a = -1.0 / (sign + self.z);
+            let b = self.x * self.y * a;
+            (
+                Self::new(1.0 + sign * self.x * self.x * a, sign * b, -sign * self.x),
+                Self::new(b, sign + self.y * self.y * a, -self.y),
+            )
+        }
     };
 }
 

tests/quat.rs

@@ -317,6 +317,53 @@ macro_rules! impl_quat_tests {
             assert!(!$quat::from_xyzw(0.0, 0.0, NEG_INFINITY, 0.0).is_finite());
             assert!(!$quat::from_xyzw(0.0, 0.0, 0.0, NAN).is_finite());
         }
+
+        #[test]
+        fn test_rotation_arc() {
+            let eps = 2.0 * core::$t::EPSILON.sqrt();
+
+            for &from in &vec3_float_test_vectors!($vec3) {
+                let from = from.normalize();
+
+                {
+                    let q = $quat::from_rotation_arc(from, from);
+                    assert!(q.is_near_identity(), "from: {}, q: {}", from, q);
+                }
+
+                {
+                    let q = $quat::from_rotation_arc_colinear(from, from);
+                    assert!(q.is_near_identity(), "from: {}, q: {}", from, q);
+                }
+
+                {
+                    let to = -from;
+                    let q = $quat::from_rotation_arc(from, to);
+                    assert!(q.is_normalized());
+                    assert!((q * from - to).length() < eps);
+                }
+
+                {
+                    let to = -from;
+                    let q = $quat::from_rotation_arc_colinear(from, to);
+                    assert!(q.is_near_identity(), "from: {}, q: {}", from, q);
+                }
+
+                for &to in &vec3_float_test_vectors!($vec3) {
+                    let to = to.normalize();
+
+                    let q = $quat::from_rotation_arc(from, to);
+                    assert!(q.is_normalized());
+                    assert!((q * from - to).length() < eps);
+
+                    let q = $quat::from_rotation_arc_colinear(from, to);
+                    assert!(q.is_normalized());
+                    let transformed = q * from;
+                    assert!(
+                        (transformed - to).length() < eps || (-transformed - to).length() < eps
+                    );
+                }
+            }
+        }
     };
 }
 

tests/support/macros.rs

@@ -110,3 +110,33 @@ macro_rules! impl_vec_float_normalize_tests {
         }
     };
 }
+
+/// Useful test vectors
+#[macro_export]
+macro_rules! vec3_float_test_vectors {
+    ($vec3:ident) => {
+        [
+            $vec3::X,
+            $vec3::Y,
+            $vec3::Z,
+            -$vec3::X,
+            -$vec3::Y,
+            -$vec3::Z,
+            $vec3::new(1.0, 1e-3, 0.0),
+            $vec3::new(1.0, 1e-4, 0.0),
+            $vec3::new(1.0, 1e-5, 0.0),
+            $vec3::new(1.0, 1e-6, 0.0),
+            $vec3::new(1.0, 1e-7, 0.0),
+            $vec3::new(1.0, 1e-14, 0.0),
+            $vec3::new(1.0, 1e-15, 0.0),
+            $vec3::new(1.0, 1e-16, 0.0),
+            $vec3::new(0.1, 0.2, 0.3),
+            $vec3::new(0.2, 0.3, 0.4),
+            $vec3::new(4.0, -5.0, 6.0),
+            $vec3::new(-2.0, 0.5, -1.0),
+            // Pathalogical cases from <https://graphics.pixar.com/library/OrthonormalB/paper.pdf>:
+            $vec3::new(0.00038527316, 0.00038460016, -0.99999988079),
+            $vec3::new(-0.00019813581, -0.00008946839, -0.99999988079),
+        ]
+    };
+}

tests/vec3.rs

@@ -669,6 +669,27 @@ macro_rules! impl_vec3_float_tests {
                 $vec3::new(6.0, 8.0, 0.0) // lengthened to length 10.0
             );
         }
+
+        #[test]
+        fn test_any_ortho() {
+            let eps = 2.0 * core::$t::EPSILON;
+
+            for &v in &vec3_float_test_vectors!($vec3) {
+                let orthogonal = v.any_orthogonal_vector();
+                assert!(orthogonal != $vec3::ZERO && orthogonal.is_finite());
+                assert!(v.dot(orthogonal).abs() < eps);
+
+                let n = v.normalize();
+
+                let orthonormal = n.any_orthonormal_vector();
+                assert!(orthonormal.is_normalized());
+                assert!(n.dot(orthonormal).abs() < eps);
+
+                let (a, b) = n.any_orthonormal_pair();
+                assert!(a.is_normalized() && n.dot(a).abs() < eps);
+                assert!(b.is_normalized() && n.dot(b).abs() < eps);
+            }
+        }
     };
 }