Merge pull request #131 from sebcrozet/rotation_to

Add the `RotationTo` trait to compute the delta rotation between two elements.

src/lib.rs

@@ -133,7 +133,7 @@ pub use traits::{
     POrdering,
     PntAsVec,
     Repeat,
-    Rotate, Rotation, RotationMatrix, RotationWithTranslation,
+    Rotate, Rotation, RotationMatrix, RotationWithTranslation, RotationTo,
     Row,
     Shape,
     SquareMat,
@@ -569,6 +569,21 @@ pub fn append_rotation_wrt_center<LV: Neg<Output = LV> + Copy,
     RotationWithTranslation::append_rotation_wrt_center(m, amount)
 }
 
+/*
+ * RotationTo
+ */
+/// Computes the angle of the rotation needed to transfom `a` to `b`.
+#[inline(always)]
+pub fn angle_between<V: RotationTo>(a: &V, b: &V) -> V::AngleType {
+    a.angle_to(b)
+}
+
+/// Computes the rotation needed to transform `a` to `b`.
+#[inline(always)]
+pub fn rotation_between<V: RotationTo>(a: &V, b: &V) -> V::DeltaRotationType {
+    a.rotation_to(b)
+}
+
 /*
  * RotationMatrix<LV, AV, R>
  */

src/structs/quat.rs

@@ -12,7 +12,7 @@ use structs::{Vec3, Pnt3, Rot3, Mat3};
 use traits::operations::{ApproxEq, Inv, POrd, POrdering, Axpy};
 use traits::structure::{Cast, Indexable, Iterable, IterableMut, Dim, Shape, BaseFloat, BaseNum,
                         Bounded, Repeat};
-use traits::geometry::{Norm, Rotation, Rotate, Transform};
+use traits::geometry::{Norm, Rotation, Rotate, RotationTo, Transform};
 
 #[cfg(feature="arbitrary")]
 use quickcheck::{Arbitrary, Gen};
@@ -456,6 +456,24 @@ impl<N: BaseNum + Neg<Output = N>> Rotate<Pnt3<N>> for UnitQuat<N> {
     }
 }
 
+impl<N: BaseFloat + ApproxEq<N>> RotationTo for UnitQuat<N> {
+    type AngleType = N;
+    type DeltaRotationType = UnitQuat<N>;
+
+    #[inline]
+    fn angle_to(&self, other: &Self) -> N {
+        let delta = self.rotation_to(other);
+        let _2    = ::one::<N>() + ::one();
+
+        _2 * delta.q.vector().norm().atan2(delta.q.w)
+    }
+
+    #[inline]
+    fn rotation_to(&self, other: &Self) -> UnitQuat<N> {
+        *other / *self
+    }
+}
+
 impl<N: BaseNum + Neg<Output = N>> Transform<Vec3<N>> for UnitQuat<N> {
     #[inline]
     fn transform(&self, v: &Vec3<N>) -> Vec3<N> {

src/structs/rot.rs

@@ -5,8 +5,8 @@
 use std::ops::{Mul, Neg, Index};
 use rand::{Rand, Rng};
 use num::{Zero, One};
-use traits::geometry::{Rotate, Rotation, AbsoluteRotate, RotationMatrix, Transform, ToHomogeneous,
-                       Norm, Cross};
+use traits::geometry::{Rotate, Rotation, AbsoluteRotate, RotationMatrix, RotationTo, Transform,
+                       ToHomogeneous, Norm, Cross};
 use traits::structure::{Cast, Dim, Row, Col, BaseFloat, BaseNum, Eye, Diag};
 use traits::operations::{Absolute, Inv, Transpose, ApproxEq};
 use structs::vec::{Vec1, Vec2, Vec3, Vec4};
@@ -71,6 +71,21 @@ impl<N: BaseFloat + Clone> Rotation<Vec1<N>> for Rot2<N> {
     }
 }
 
+impl<N: BaseFloat> RotationTo for Rot2<N> {
+    type AngleType = N;
+    type DeltaRotationType = Rot2<N>;
+
+    #[inline]
+    fn angle_to(&self, other: &Self) -> N {
+        self.rotation_to(other).rotation().norm()
+    }
+
+    #[inline]
+    fn rotation_to(&self, other: &Self) -> Rot2<N> {
+        *other * ::inv(self).unwrap()
+    }
+}
+
 impl<N: Rand + BaseFloat> Rand for Rot2<N> {
     #[inline]
     fn rand<R: Rng>(rng: &mut R) -> Rot2<N> {
@@ -283,6 +298,22 @@ Rotation<Vec3<N>> for Rot3<N> {
     }
 }
 
+impl<N: BaseFloat> RotationTo for Rot3<N> {
+    type AngleType = N;
+    type DeltaRotationType = Rot3<N>;
+
+    #[inline]
+    fn angle_to(&self, other: &Self) -> N {
+        // FIXME: refactor to avoid the normalization of the rotation axisangle vector.
+        self.rotation_to(other).rotation().norm()
+    }
+
+    #[inline]
+    fn rotation_to(&self, other: &Self) -> Rot3<N> {
+        *other * ::inv(self).unwrap()
+    }
+}
+
 impl<N: Clone + Rand + BaseFloat> Rand for Rot3<N> {
     #[inline]
     fn rand<R: Rng>(rng: &mut R) -> Rot3<N> {

src/structs/spec/vec.rs

@@ -1,9 +1,50 @@
 use std::ops::{Sub, Mul, Neg};
 use num::{Zero, One};
 use traits::structure::{Cast, Row, Basis, BaseFloat};
-use traits::geometry::{Norm, Cross, CrossMatrix, UniformSphereSample};
+use traits::geometry::{Norm, Cross, CrossMatrix, RotationTo, UniformSphereSample};
 use structs::vec::{Vec1, Vec2, Vec3, Vec4};
 use structs::mat::Mat3;
+use structs::rot::{Rot2, Rot3};
+
+impl<N: BaseFloat> RotationTo for Vec2<N> {
+    type AngleType = N;
+    type DeltaRotationType = Rot2<N>;
+
+    #[inline]
+    fn angle_to(&self, other: &Self) -> N {
+        ::cross(self, other).x.atan2(::dot(self, other))
+    }
+
+    #[inline]
+    fn rotation_to(&self, other: &Self) -> Rot2<N> {
+        Rot2::new(Vec1::new(self.angle_to(other)))
+    }
+}
+
+impl<N: BaseFloat> RotationTo for Vec3<N> {
+    type AngleType = N;
+    type DeltaRotationType = Rot3<N>;
+
+    #[inline]
+    fn angle_to(&self, other: &Self) -> N {
+        ::cross(self, other).norm().atan2(::dot(self, other))
+    }
+
+    #[inline]
+    fn rotation_to(&self, other: &Self) -> Rot3<N> {
+        let mut axis = ::cross(self, other);
+        let norm = axis.normalize_mut();
+
+        if ::is_zero(&norm) {
+            ::one()
+        }
+        else {
+            let axis_angle = axis * norm.atan2(::dot(self, other));
+
+            Rot3::new(axis_angle)
+        }
+    }
+}
 
 impl<N: Copy + Mul<N, Output = N> + Sub<N, Output = N>> Cross for Vec2<N> {
     type CrossProductType = Vec1<N>;

src/structs/vec_macros.rs

@@ -79,7 +79,7 @@ macro_rules! at_fast_impl(
 // However, f32/f64 does not implement Ord…
 macro_rules! ord_impl(
     ($t: ident, $comp0: ident, $($compN: ident),*) => (
-        impl<N: BaseFloat + Copy> POrd for $t<N> {
+        impl<N: BaseFloat> POrd for $t<N> {
             #[inline]
             fn inf(&self, other: &$t<N>) -> $t<N> {
                 $t::new(self.$comp0.min(other.$comp0)
@@ -291,7 +291,7 @@ macro_rules! container_impl(
 
 macro_rules! basis_impl(
     ($t: ident, $dim: expr) => (
-        impl<N: Copy + BaseFloat + ApproxEq<N>> Basis for $t<N> {
+        impl<N: BaseFloat + ApproxEq<N>> Basis for $t<N> {
             #[inline]
             fn canonical_basis<F: FnMut($t<N>) -> bool>(mut f: F) {
                 for i in 0..$dim {
@@ -547,7 +547,7 @@ macro_rules! translation_impl(
 
 macro_rules! norm_impl(
     ($t: ident, $($compN: ident),+) => (
-        impl<N: Copy + BaseFloat> Norm<N> for $t<N> {
+        impl<N: BaseFloat> Norm<N> for $t<N> {
             #[inline]
             fn sqnorm(&self) -> N {
                 Dot::dot(self, self)

src/traits/geometry.rs

@@ -64,6 +64,22 @@ pub trait Rotation<V> {
     fn set_rotation(&mut self, V);
 }
 
+/// Trait of object that can be rotated to be superimposed with another one of the same nature.
+pub trait RotationTo {
+    /// Type of the angle between two elements.
+    type AngleType;
+
+    /// Type of the rotation between two elements.
+    type DeltaRotationType;
+
+    /// Computes an angle nedded to transform the first element to the second one using a
+    /// rotation.
+    fn angle_to(&self, other: &Self) -> Self::AngleType;
+
+    /// Computes the smallest rotation needed to transform the first element to the second one.
+    fn rotation_to(&self, other: &Self) -> Self::DeltaRotationType;
+}
+
 /// Trait of objects able to rotate other objects.
 ///
 /// This is typically implemented by matrices which rotate vectors.

src/traits/mod.rs

@@ -1,8 +1,9 @@
 //! Mathematical traits.
 
 pub use traits::geometry::{AbsoluteRotate, Cross, CrossMatrix, Dot, FromHomogeneous, Norm, Orig,
-                           Rotate, Rotation, RotationMatrix, RotationWithTranslation, ToHomogeneous,
-                           Transform, Transformation, Translate, Translation, UniformSphereSample};
+                           Rotate, Rotation, RotationMatrix, RotationWithTranslation, RotationTo,
+                           ToHomogeneous, Transform, Transformation, Translate, Translation,
+                           UniformSphereSample};
 
 pub use traits::structure::{FloatVec, FloatPnt, Basis, Cast, Col, Dim, Indexable, Iterable,
                             IterableMut, Mat, SquareMat, Row, NumVec, NumPnt, PntAsVec, ColSlice,

tests/mat.rs

@@ -2,8 +2,8 @@ extern crate nalgebra as na;
 extern crate rand;
 
 use rand::random;
-use na::{Vec1, Vec3, Mat1, Mat2, Mat3, Mat4, Mat5, Mat6, Rot3, Persp3, PerspMat3, Ortho3, OrthoMat3,
-         DMat, DVec, Row, Col, BaseFloat};
+use na::{Vec1, Vec3, Mat1, Mat2, Mat3, Mat4, Mat5, Mat6, Rot2, Rot3, Persp3, PerspMat3, Ortho3,
+         OrthoMat3, DMat, DVec, Row, Col, BaseFloat};
 
 macro_rules! test_inv_mat_impl(
   ($t: ty) => (
@@ -153,6 +153,48 @@ fn test_inv_rotation3() {
     }
 }
 
+#[test]
+fn test_rot3_rotation_between() {
+    let r1: Rot3<f64> = random();
+    let r2: Rot3<f64> = random();
+
+    let delta = na::rotation_between(&r1, &r2);
+
+    assert!(na::approx_eq(&(delta * r1), &r2))
+}
+
+#[test]
+fn test_rot3_angle_between() {
+    let r1: Rot3<f64> = random();
+    let r2: Rot3<f64> = random();
+
+    let delta = na::rotation_between(&r1, &r2);
+    let delta_angle = na::angle_between(&r1, &r2);
+
+    assert!(na::approx_eq(&na::norm(&na::rotation(&delta)), &delta_angle))
+}
+
+#[test]
+fn test_rot2_rotation_between() {
+    let r1: Rot2<f64> = random();
+    let r2: Rot2<f64> = random();
+
+    let delta = na::rotation_between(&r1, &r2);
+
+    assert!(na::approx_eq(&(delta * r1), &r2))
+}
+
+#[test]
+fn test_rot2_angle_between() {
+    let r1: Rot2<f64> = random();
+    let r2: Rot2<f64> = random();
+
+    let delta = na::rotation_between(&r1, &r2);
+    let delta_angle = na::angle_between(&r1, &r2);
+
+    assert!(na::approx_eq(&na::norm(&na::rotation(&delta)), &delta_angle))
+}
+
 #[test]
 fn test_mean_dmat() {
     let mat = DMat::from_row_vec(

tests/quat.rs

@@ -69,3 +69,24 @@ fn test_quat_euler_angles() {
         assert!(na::approx_eq(&q.to_rot(), &m))
     }
 }
+
+#[test]
+fn test_quat_rotation_between() {
+    let q1: UnitQuat<f64> = random();
+    let q2: UnitQuat<f64> = random();
+
+    let delta = na::rotation_between(&q1, &q2);
+
+    assert!(na::approx_eq(&(delta * q1), &q2))
+}
+
+#[test]
+fn test_quat_angle_between() {
+    let q1: UnitQuat<f64> = random();
+    let q2: UnitQuat<f64> = random();
+
+    let delta = na::rotation_between(&q1, &q2);
+    let delta_angle = na::angle_between(&q1, &q2);
+
+    assert!(na::approx_eq(&na::norm(&na::rotation(&delta)), &delta_angle))
+}

tests/vec.rs

@@ -2,7 +2,7 @@ extern crate nalgebra as na;
 extern crate rand;
 
 use rand::random;
-use na::{Vec0, Vec1, Vec2, Vec3, Vec4, Vec5, Vec6, Mat3, Iterable, IterableMut};
+use na::{Vec0, Vec1, Vec2, Vec3, Vec4, Vec5, Vec6, Mat3, Rot2, Rot3, Iterable, IterableMut};
 
 macro_rules! test_iterator_impl(
     ($t: ty, $n: ty) => (
@@ -317,3 +317,65 @@ fn test_outer_vec3() {
             8.0, 10.0, 12.0,
             12.0, 15.0, 18.0));
 }
+
+
+#[test]
+fn test_vec3_rotation_between() {
+    for _ in (0usize .. 10000) {
+        let v1: Vec3<f64> = random();
+
+        let mut v2: Vec3<f64> = random();
+        v2 = na::normalize(&v2) * na::norm(&v1);
+
+        let rot = na::rotation_between(&v1, &v2);
+
+        assert!(na::approx_eq(&(rot * v1), &v2))
+    }
+}
+
+#[test]
+fn test_vec3_angle_between() {
+    for _ in (0usize .. 10000) {
+        let vec: Vec3<f64> = random();
+        let other: Vec3<f64> = random();
+
+        // Ensure the axis we are using is orthogonal to `vec`.
+        let axis_ang = na::cross(&vec, &other);
+        let ang      = na::norm(&axis_ang);
+        let rot      = Rot3::new(axis_ang);
+
+        let delta = na::angle_between(&vec, &(rot * vec));
+
+        assert!(na::approx_eq(&ang, &delta))
+    }
+}
+
+
+#[test]
+fn test_vec2_rotation_between() {
+    for _ in (0usize .. 10000) {
+        let v1: Vec2<f64> = random();
+
+        let mut v2: Vec2<f64> = random();
+        v2 = na::normalize(&v2) * na::norm(&v1);
+
+        let rot = na::rotation_between(&v1, &v2);
+
+        assert!(na::approx_eq(&(rot * v1), &v2))
+    }
+}
+
+#[test]
+fn test_vec2_angle_between() {
+    for _ in (0usize .. 10000) {
+        let axis_ang: Vec1<f64> = random();
+        let ang = na::norm(&axis_ang);
+
+        let rot: Rot2<f64> = Rot2::new(axis_ang);
+        let vec: Vec2<f64> = random();
+
+        let delta = na::angle_between(&vec, &(rot * vec));
+
+        assert!(na::approx_eq(&ang, &delta))
+    }
+}