From 1ac8bbd3d1d1610ae4f05ede3072708d6bd4ca16 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= <sebastien@crozet.re>
Date: Thu, 8 Mar 2018 17:30:59 +0100
Subject: [PATCH] Add .axis_angle to UnitComplex and UnitQuaternion +
 .rotation_between_axis to UnitComplex.

---
 .gitignore                                |  1 +
 CHANGELOG.md                              |  8 ++++
 src/core/blas.rs                          |  6 +--
 src/geometry/quaternion.rs                | 12 ++++++
 src/geometry/unit_complex.rs              | 16 ++++++++
 src/geometry/unit_complex_construction.rs | 45 ++++++++++++++++++++---
 6 files changed, 78 insertions(+), 10 deletions(-)

diff --git a/.gitignore b/.gitignore
index 84b112d0..17a97cc7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -7,3 +7,4 @@ target/
 Cargo.lock
 *.orig
 *.swo
+site/
\ No newline at end of file
diff --git a/CHANGELOG.md b/CHANGELOG.md
index d272d63c..d8eacebb 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -4,6 +4,14 @@ documented here.
 
 This project adheres to [Semantic Versioning](http://semver.org/).
 
+## [0.15.0] - WIP
+### Modified
+### Added
+  * Add methods `.rotation_between_axis(...)` and `.scaled_rotation_between_axis(...)` to `UnitComplex`
+    to compute the rotation matrix between two 2D **unit** vectors.
+  * Add methods `.axis_angle()` to `UnitComplex` and `UnitQuaternion` in order to retrieve both the
+    unit rotation axis and the rotation angle simultaneously. 
+
 ## [0.14.0]
 ### Modified
   * Allow the `Isometry * Unit<Vector>` multiplication.
diff --git a/src/core/blas.rs b/src/core/blas.rs
index 91035199..de8020ec 100644
--- a/src/core/blas.rs
+++ b/src/core/blas.rs
@@ -462,8 +462,7 @@ where
         let (nrows3, ncols3) = b.shape();
 
         assert_eq!(
-            ncols2,
-            nrows3,
+            ncols2, nrows3,
             "gemm: dimensions mismatch for multiplication."
         );
         assert_eq!(
@@ -564,8 +563,7 @@ where
         let (nrows3, ncols3) = b.shape();
 
         assert_eq!(
-            nrows2,
-            nrows3,
+            nrows2, nrows3,
             "gemm: dimensions mismatch for multiplication."
         );
         assert_eq!(
diff --git a/src/geometry/quaternion.rs b/src/geometry/quaternion.rs
index 1a70c541..88ad945d 100644
--- a/src/geometry/quaternion.rs
+++ b/src/geometry/quaternion.rs
@@ -491,6 +491,18 @@ impl<N: Real> UnitQuaternion<N> {
         }
     }
 
+    /// The rotation axis and angle in ]0, pi] of this unit quaternion.
+    ///
+    /// Returns `None` if the angle is zero.
+    #[inline]
+    pub fn axis_angle(&self) -> Option<(Unit<Vector3<N>>, N)> {
+        if let Some(axis) = self.axis() {
+            Some((axis, self.angle()))
+        } else {
+            None
+        }
+    }
+
     /// Compute the exponential of a quaternion.
     ///
     /// Note that this function yields a `Quaternion<N>` because it looses the unit property.
diff --git a/src/geometry/unit_complex.rs b/src/geometry/unit_complex.rs
index 299e5d0b..07c7cdca 100644
--- a/src/geometry/unit_complex.rs
+++ b/src/geometry/unit_complex.rs
@@ -34,6 +34,22 @@ impl<N: Real> UnitComplex<N> {
         Vector1::new(self.angle())
     }
 
+    /// The rotation axis and angle in ]0, pi] of this complex number.
+    ///
+    /// Returns `None` if the angle is zero.
+    #[inline]
+    pub fn axis_angle(&self) -> Option<(Unit<Vector1<N>>, N)> {
+        let ang = self.angle();
+
+        if ang.is_zero() {
+            None
+        } else if ang.is_sign_negative() {
+            Some((Unit::new_unchecked(Vector1::x()), -ang))
+        } else {
+            Some((Unit::new_unchecked(-Vector1::<N>::x()), ang))
+        }
+    }
+
     /// The underlying complex number.
     ///
     /// Same as `self.as_ref()`.
diff --git a/src/geometry/unit_complex_construction.rs b/src/geometry/unit_complex_construction.rs
index 73d0ccdd..4ea268f5 100644
--- a/src/geometry/unit_complex_construction.rs
+++ b/src/geometry/unit_complex_construction.rs
@@ -6,7 +6,7 @@ use num_complex::Complex;
 use rand::{Rand, Rng};
 
 use alga::general::Real;
-use core::{DefaultAllocator, Vector};
+use core::{DefaultAllocator, Unit, Vector};
 use core::dimension::{U1, U2};
 use core::storage::Storage;
 use core::allocator::Allocator;
@@ -99,15 +99,48 @@ impl<N: Real> UnitComplex<N> {
         SB: Storage<N, U2, U1>,
         SC: Storage<N, U2, U1>,
     {
-        if let (Some(na), Some(nb)) = (a.try_normalize(N::zero()), b.try_normalize(N::zero())) {
-            let sang = na.perp(&nb);
-            let cang = na.dot(&nb);
-
-            Self::from_angle(sang.atan2(cang) * s)
+        // FIXME: code duplication with Rotation.
+        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 {
             Self::identity()
         }
     }
+
+    /// The unit complex needed to make `a` and `b` be collinear and point toward the same
+    /// direction.
+    #[inline]
+    pub fn rotation_between_axis<SB, SC>(
+        a: &Unit<Vector<N, U2, SB>>,
+        b: &Unit<Vector<N, U2, SC>>,
+    ) -> Self
+    where
+        SB: Storage<N, U2>,
+        SC: Storage<N, U2>,
+    {
+        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<SB, SC>(
+        na: &Unit<Vector<N, U2, SB>>,
+        nb: &Unit<Vector<N, U2, SC>>,
+        s: N,
+    ) -> Self
+    where
+        SB: Storage<N, U2>,
+        SC: Storage<N, U2>,
+    {
+        let sang = na.perp(&nb);
+        let cang = na.dot(&nb);
+
+        Self::from_angle(sang.atan2(cang) * s)
+    }
 }
 
 impl<N: Real> One for UnitComplex<N> {