diff --git a/src/quaternion.rs b/src/quaternion.rs index 5ee7ffe..3414eb6 100644 --- a/src/quaternion.rs +++ b/src/quaternion.rs @@ -60,6 +60,34 @@ impl Quaternion { 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, dst: Vector3, fallback: Option>) + -> Quaternion { + 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 { diff --git a/tests/quaternion.rs b/tests/quaternion.rs index 78ac4f5..bfa9cee 100644 --- a/tests/quaternion.rs +++ b/tests/quaternion.rs @@ -113,6 +113,42 @@ mod from { } } +mod arc { + use cgmath::*; + + #[inline] + fn test(src: Vector3, dst: Vector3) { + 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 = 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::*;