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 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) { + 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 { 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, dst: Vector3) { + 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 = 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::*;