From f555074a447782d8ae9fd12543a77d60e04cddc0 Mon Sep 17 00:00:00 2001
From: Dzmitry Malyshau <kvarkus@gmail.com>
Date: Sat, 2 Jul 2016 23:56:01 -0400
Subject: [PATCH] Quaternion closest arc

---
 src/quaternion.rs   | 28 ++++++++++++++++++++++++++++
 tests/quaternion.rs | 25 +++++++++++++++++++++++++
 2 files changed, 53 insertions(+)

diff --git a/src/quaternion.rs b/src/quaternion.rs
index 5ee7ffe..d25f7af 100644
--- a/src/quaternion.rs
+++ b/src/quaternion.rs
@@ -60,6 +60,34 @@ impl<S: BaseFloat> Quaternion<S> {
         Quaternion { s: s, v: v }
     }
 
+    /// Construct a new quaternion as a closest arc between two vectors
+    ///
+    /// Return the closest rotation that turns `src` vector into `dst`.
+    ///
+    /// - [Related StackOverflow question]
+    ///   (http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another)
+    /// - [Ogre implementation for normalized vectors]
+    ///   (https://bitbucket.org/sinbad/ogre/src/9db75e3ba05c/OgreMain/include/OgreVector3.h?fileviewer=file-view-default#cl-651)
+    pub fn from_arc(src: Vector3<S>, dst: Vector3<S>, fallback: Option<Vector3<S>>)
+                    -> Quaternion<S> {
+        let mag_avg = (src.magnitude2() * dst.magnitude2()).sqrt();
+        let dot = src.dot(dst);
+        if dot.approx_eq(&mag_avg) {
+            One::one()
+        }else if dot.approx_eq(&-mag_avg) {
+            let axis = fallback.unwrap_or_else(|| {
+                let mut v = Vector3::unit_x().cross(src);
+                if v.approx_eq(&Zero::zero()) {
+                    v = Vector3::unit_y().cross(src);
+                }
+                v.normalize()
+            });
+            Rotation3::from_axis_angle(axis, Angle::turn_div_2())
+        }else {
+            Quaternion::from_sv(mag_avg + dot, src.cross(dst)).normalize()
+        }
+    }
+
     /// The conjugate of the quaternion.
     #[inline]
     pub fn conjugate(self) -> Quaternion<S> {
diff --git a/tests/quaternion.rs b/tests/quaternion.rs
index 78ac4f5..9f81e72 100644
--- a/tests/quaternion.rs
+++ b/tests/quaternion.rs
@@ -113,6 +113,31 @@ mod from {
     }
 }
 
+mod arc {
+    use cgmath::*;
+
+    fn test(src: Vector3<f32>, dst: Vector3<f32>) {
+        let q = Quaternion::from_arc(src, dst, None);
+        let v = q.rotate_vector(src);
+        assert_approx_eq!(v, dst);
+    }
+
+    #[test]
+    fn test_arc() {
+        let v = Vector3::unit_x();
+        {
+            let q = Quaternion::from_arc(v, v, None);
+            assert_eq!(q, Quaternion::new(1.0, 0.0, 0.0, 0.0));
+        }
+        test(v, -v);
+        {
+            let q: Quaternion<f32> = Quaternion::from_arc(v, Vector3::unit_y(), None);
+            let q2 = Quaternion::from_axis_angle(Vector3::unit_z(), Angle::turn_div_4());
+            assert_approx_eq!(q, q2);
+        }
+    }
+}
+
 mod rotate_from_euler {
     use cgmath::*;