From 52598de44c33b8716c1438d2ee6371263d9f510d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= <sebastien@crozet.re>
Date: Fri, 2 Feb 2018 12:26:25 +0100
Subject: [PATCH] Add rotation_between_axis/scaled_rotation_between_axis.

---
 CHANGELOG.md                            |  3 ++
 src/geometry/quaternion_construction.rs | 66 +++++++++++++++++++------
 2 files changed, 55 insertions(+), 14 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 2fe62236..664ceeaa 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,9 @@ This project adheres to [Semantic Versioning](http://semver.org/).
     `::from_element(...)`.
   * The `.iamin()` methods that returns the index of the vector entry with
     smallest absolute value.
+  * `UnitQuaternion::scaled_rotation_between_axis` and
+    `UnitQuaternion::rotation_between_axis` that take Unit vectors instead of
+    Vector as arguments.
 
 
 
diff --git a/src/geometry/quaternion_construction.rs b/src/geometry/quaternion_construction.rs
index 2951e8b1..de91e7fa 100644
--- a/src/geometry/quaternion_construction.rs
+++ b/src/geometry/quaternion_construction.rs
@@ -207,24 +207,62 @@ impl<N: Real> UnitQuaternion<N> {
         where SB: Storage<N, U3>,
               SC: Storage<N, U3> {
         // FIXME: code duplication with Rotation.
-        if let (Some(na), Some(nb)) = (a.try_normalize(N::zero()), b.try_normalize(N::zero())) {
-            let c = na.cross(&nb);
+        if let (Some(na), Some(nb)) = (Unit::try_new(a.clone_owned(), N::zero()),
+                                       Unit::try_new(b.clone_owned(), N::zero())) {
+            Self::scaled_rotation_between_axis(&na, &nb, s)
+        }
+        else {
+            Some(Self::identity())
+        }
+    }
 
-            if let Some(axis) = Unit::try_new(c, N::default_epsilon()) {
-                return Some(Self::from_axis_angle(&axis, na.dot(&nb).acos() * s))
+    /// The unit quaternion needed to make `a` and `b` be collinear and point toward the same
+    /// direction.
+    #[inline]
+    pub fn rotation_between_axis<SB, SC>(a: &Unit<Vector<N, U3, SB>>, b: &Unit<Vector<N, U3, SC>>) -> Option<Self>
+        where SB: Storage<N, U3>,
+              SC: Storage<N, U3> {
+        Self::scaled_rotation_between_axis(a, b, N::one())
+    }
+
+    /// The smallest rotation needed to make `a` and `b` collinear and point toward the same
+    /// direction, raised to the power `s`.
+    #[inline]
+    pub fn scaled_rotation_between_axis<SB, SC>(na: &Unit<Vector<N, U3, SB>>,
+                                                nb: &Unit<Vector<N, U3, SC>>,
+                                                s:  N)
+                                                -> Option<Self>
+        where SB: Storage<N, U3>,
+              SC: Storage<N, U3> {
+
+        // FIXME: code duplication with Rotation.
+        let c = na.cross(&nb);
+
+        if let Some(axis) = Unit::try_new(c, N::default_epsilon()) {
+            let cos = na.dot(&nb);
+
+            // The cosinus may be out of [-1, 1] because of innacuracies.
+            if cos <= -N::one() {
+                return None
             }
-
-            // Zero or PI.
-            if na.dot(&nb) < N::zero() {
-                // PI
-                //
-                // The rotation axis is undefined but the angle not zero. This is not a
-                // simple rotation.
-                return None;
+            else if cos >= N::one() {
+                return Some(Self::identity())
+            }
+            else {
+                return Some(Self::from_axis_angle(&axis, cos.acos() * s))
             }
         }
-
-        Some(Self::identity())
+        else if na.dot(&nb) < N::zero() {
+            // PI
+            //
+            // The rotation axis is undefined but the angle not zero. This is not a
+            // simple rotation.
+            return None;
+        }
+        else {
+            // Zero
+            Some(Self::identity())
+        }
     }