diff --git a/CHANGELOG.md b/CHANGELOG.md index 2fe62236..664ceeaa 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,9 @@ This project adheres to [Semantic Versioning](http://semver.org/). `::from_element(...)`. * The `.iamin()` methods that returns the index of the vector entry with smallest absolute value. + * `UnitQuaternion::scaled_rotation_between_axis` and + `UnitQuaternion::rotation_between_axis` that take Unit vectors instead of + Vector as arguments. diff --git a/src/geometry/quaternion_construction.rs b/src/geometry/quaternion_construction.rs index 2951e8b1..de91e7fa 100644 --- a/src/geometry/quaternion_construction.rs +++ b/src/geometry/quaternion_construction.rs @@ -207,24 +207,62 @@ impl UnitQuaternion { where SB: Storage, SC: Storage { // FIXME: code duplication with Rotation. - if let (Some(na), Some(nb)) = (a.try_normalize(N::zero()), b.try_normalize(N::zero())) { - let c = na.cross(&nb); + if let (Some(na), Some(nb)) = (Unit::try_new(a.clone_owned(), N::zero()), + Unit::try_new(b.clone_owned(), N::zero())) { + Self::scaled_rotation_between_axis(&na, &nb, s) + } + else { + Some(Self::identity()) + } + } - if let Some(axis) = Unit::try_new(c, N::default_epsilon()) { - return Some(Self::from_axis_angle(&axis, na.dot(&nb).acos() * s)) + /// The unit quaternion needed to make `a` and `b` be collinear and point toward the same + /// direction. + #[inline] + pub fn rotation_between_axis(a: &Unit>, b: &Unit>) -> Option + where SB: Storage, + SC: Storage { + Self::scaled_rotation_between_axis(a, b, N::one()) + } + + /// The smallest rotation needed to make `a` and `b` collinear and point toward the same + /// direction, raised to the power `s`. + #[inline] + pub fn scaled_rotation_between_axis(na: &Unit>, + nb: &Unit>, + s: N) + -> Option + where SB: Storage, + SC: Storage { + + // FIXME: code duplication with Rotation. + let c = na.cross(&nb); + + if let Some(axis) = Unit::try_new(c, N::default_epsilon()) { + let cos = na.dot(&nb); + + // The cosinus may be out of [-1, 1] because of innacuracies. + if cos <= -N::one() { + return None } - - // Zero or PI. - if na.dot(&nb) < N::zero() { - // PI - // - // The rotation axis is undefined but the angle not zero. This is not a - // simple rotation. - return None; + else if cos >= N::one() { + return Some(Self::identity()) + } + else { + return Some(Self::from_axis_angle(&axis, cos.acos() * s)) } } - - Some(Self::identity()) + else if na.dot(&nb) < N::zero() { + // PI + // + // The rotation axis is undefined but the angle not zero. This is not a + // simple rotation. + return None; + } + else { + // Zero + Some(Self::identity()) + } }