From cd9e87373958494c79c3500a0edbedcd75957f90 Mon Sep 17 00:00:00 2001
From: andystanton <andystanton@gmail.com>
Date: Tue, 11 Apr 2017 23:42:34 +0100
Subject: [PATCH] Fix between_vectors for opposite vectors

---
 src/quaternion.rs   | 25 ++++++++++++++++++---
 tests/quaternion.rs | 54 +++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 76 insertions(+), 3 deletions(-)

diff --git a/src/quaternion.rs b/src/quaternion.rs
index b5db09c..4a0888b 100644
--- a/src/quaternion.rs
+++ b/src/quaternion.rs
@@ -624,9 +624,28 @@ impl<S: BaseFloat> Rotation<Point3<S>> for Quaternion<S> {
 
     #[inline]
     fn between_vectors(a: Vector3<S>, b: Vector3<S>) -> Quaternion<S> {
-        //http://stackoverflow.com/questions/1171849/
-        //finding-quaternion-representing-the-rotation-from-one-vector-to-another
-        Quaternion::from_sv(S::one() + a.dot(b), a.cross(b)).normalize()
+        // http://stackoverflow.com/a/11741520/2074937 see 'Half-Way Quaternion Solution'
+
+        let k_cos_theta = a.dot(b);
+
+        // same direction
+        if ulps_eq!(k_cos_theta, S::one()) {
+            return Quaternion::one();
+        }
+
+        let k = (a.magnitude2() * b.magnitude2()).sqrt();
+
+        // opposite direction
+        if ulps_eq!(k_cos_theta / k, -S::one()) {
+            let mut orthogonal = a.cross(Vector3::unit_x());
+            if ulps_eq!(orthogonal.magnitude2(), S::zero()) {
+                orthogonal = a.cross(Vector3::unit_y());
+            }
+            return Quaternion::from_sv(S::zero(), orthogonal.normalize());
+        }
+
+        // any other direction
+        Quaternion::from_sv(k + k_cos_theta, a.cross(b)).normalize()
     }
 
     #[inline]
diff --git a/tests/quaternion.rs b/tests/quaternion.rs
index 5b00619..d2e9a8c 100644
--- a/tests/quaternion.rs
+++ b/tests/quaternion.rs
@@ -258,3 +258,57 @@ mod rotate_from_axis_angle {
         assert_ulps_eq!(vec3(-2.0f32.sqrt() / 2.0, 0.0, 2.0f32.sqrt() / 2.0), rot * vec);
     }
 }
+
+mod rotate_between_vectors {
+    use cgmath::*;
+
+    #[test]
+    fn test_around_z_0() {
+        let expected = Quaternion::new(1.0, 0.0, 0.0, 0.0);
+
+        let a = vec3(12.0, 0.0, 0.0);
+        let b = vec3(1.0, 0.0, 0.0);
+
+        assert_ulps_eq!(Quaternion::between_vectors(a, b), expected);
+    }
+
+    #[test]
+    fn test_around_z_90_cw() {
+        let expected = Quaternion::new(0.5_f32.sqrt(), 0.0, 0.0, 0.5_f32.sqrt());
+
+        let a = vec3(8.0, 0.0, 0.0);
+        let b = vec3(0.0, 9.0, 0.0);
+
+        assert_ulps_eq!(Quaternion::between_vectors(a, b), expected);
+    }
+
+    #[test]
+    fn test_around_z_90_ccw() {
+        let expected = Quaternion::new(0.5_f32.sqrt(), 0.0, 0.0, -0.5_f32.sqrt());
+
+        let a = vec3(-26.0, 0.0, 0.0);
+        let b = vec3(0.0, 10.0, 0.0);
+
+        assert_ulps_eq!(Quaternion::between_vectors(a, b), expected);
+    }
+
+    #[test]
+    fn test_around_z_180_cw() {
+        let expected = Quaternion::new(0.0, 0.0, 0.0, 1.0);
+
+        let a = vec3(10.0, 0.0, 0.0);
+        let b = vec3(-5.0, 0.0, 0.0);
+
+        assert_ulps_eq!(Quaternion::between_vectors(a, b), expected);
+    }
+
+    #[test]
+    fn test_around_z_180_ccw() {
+        let expected = Quaternion::new(0.0, 0.0, 0.0, -1.0);
+
+        let a = vec3(-3.0, 0.0, 0.0);
+        let b = vec3(40.0, 0.0, 0.0);
+
+        assert_ulps_eq!(Quaternion::between_vectors(a, b), expected);
+    }
+}
\ No newline at end of file