Merge pull request #364 from kvark/arc

Quaternion closest arc

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) {
+            Quaternion::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()
+            });
+            Quaternion::from_axis_angle(axis, Rad::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> {

tests/quaternion.rs

@@ -113,6 +113,42 @@ mod from {
     }
 }
 
+mod arc {
+    use cgmath::*;
+
+    #[inline]
+    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.normalize(), dst.normalize());
+    }
+
+    #[test]
+    fn test_same() {
+        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]
+    fn test_opposite() {
+        let v = Vector3::unit_x();
+        test(v, -v);
+    }
+
+    #[test]
+    fn test_random() {
+        test(vec3(1.0, 2.0, 3.0), vec3(-4.0, 5.0, -6.0));
+    }
+
+    #[test]
+    fn test_ortho() {
+        let q: Quaternion<f32> = Quaternion::from_arc(Vector3::unit_x(), Vector3::unit_y(), None);
+        let q2 = Quaternion::from_axis_angle(Vector3::unit_z(), Rad::turn_div_4());
+        assert_approx_eq!(q, q2);
+    }
+}
+
 mod rotate_from_euler {
     use cgmath::*;