From 669a08daa538de9fab2e13f3ec44ffe68237460f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Vladim=C3=ADr=20Vondru=C5=A1?= <mosra@centrum.cz>
Date: Sun, 6 Jan 2013 19:06:25 +0100
Subject: [PATCH] Math: angle between two quaternions.

Also updated related Vector test.
---
 src/Math/Quaternion.h                | 13 +++++++++++++
 src/Math/Test/MathQuaternionTest.cpp | 18 ++++++++++++++++++
 src/Math/Test/VectorTest.cpp         | 13 +++++++++----
 3 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/src/Math/Quaternion.h b/src/Math/Quaternion.h
index d4775c629..43c3b959c 100644
--- a/src/Math/Quaternion.h
+++ b/src/Math/Quaternion.h
@@ -46,6 +46,19 @@ template<class T> class Quaternion {
             return Vector3<T>::dot(a.vector(), b.vector()) + a.scalar()*b.scalar();
         }
 
+        /**
+         * @brief Angle between normalized quaternions (in radians)
+         *
+         * Expects that both quaternions are normalized. @f[
+         * \theta = acos \left(\frac{p \cdot q}{|p| \cdot |q|} \right)
+         * @f]
+         */
+        inline static T angle(const Quaternion<T>& normalizedA, const Quaternion<T>& normalizedB) {
+            CORRADE_ASSERT(MathTypeTraits<T>::equals(normalizedA.dot(), T(1)) && MathTypeTraits<T>::equals(normalizedB.dot(), T(1)),
+                           "Math::Quaternion::angle(): quaternions must be normalized", std::numeric_limits<T>::quiet_NaN());
+            return std::acos(dot(normalizedA, normalizedB));
+        }
+
         /**
          * @brief Linear interpolation of two quaternions
          * @param normalizedA   First quaternion
diff --git a/src/Math/Test/MathQuaternionTest.cpp b/src/Math/Test/MathQuaternionTest.cpp
index e134a2769..4f1422084 100644
--- a/src/Math/Test/MathQuaternionTest.cpp
+++ b/src/Math/Test/MathQuaternionTest.cpp
@@ -39,6 +39,7 @@ class QuaternionTest: public Corrade::TestSuite::Tester {
         void inverted();
         void invertedNormalized();
         void rotation();
+        void angle();
         void matrix();
         void lerp();
 
@@ -62,6 +63,7 @@ QuaternionTest::QuaternionTest() {
              &QuaternionTest::inverted,
              &QuaternionTest::invertedNormalized,
              &QuaternionTest::rotation,
+             &QuaternionTest::angle,
              &QuaternionTest::matrix,
              &QuaternionTest::lerp,
              &QuaternionTest::debug);
@@ -172,6 +174,22 @@ void QuaternionTest::rotation() {
     CORRADE_COMPARE(q2.rotationAxis(), -axis);
 }
 
+void QuaternionTest::angle() {
+    std::ostringstream o;
+    Corrade::Utility::Error::setOutput(&o);
+    CORRADE_COMPARE(Quaternion::angle(Quaternion({1.0f, 2.0f, -3.0f}, -4.0f).normalized(), {{4.0f, -3.0f, 2.0f}, -1.0f}),
+                    std::numeric_limits<Vector3::Type>::quiet_NaN());
+    CORRADE_COMPARE(o.str(), "Math::Quaternion::angle(): quaternions must be normalized\n");
+
+    o.str("");
+    CORRADE_COMPARE(Quaternion::angle({{1.0f, 2.0f, -3.0f}, -4.0f}, Quaternion({4.0f, -3.0f, 2.0f}, -1.0f).normalized()),
+                    std::numeric_limits<Vector3::Type>::quiet_NaN());
+    CORRADE_COMPARE(o.str(), "Math::Quaternion::angle(): quaternions must be normalized\n");
+
+    CORRADE_COMPARE(Quaternion::angle(Quaternion({1.0f, 2.0f, -3.0f}, -4.0f).normalized(), Quaternion({4.0f, -3.0f, 2.0f}, -1.0f).normalized()),
+                    rad(1.704528f));
+}
+
 void QuaternionTest::matrix() {
     float angle = deg(37.0f);
     Vector3 axis(1.0f/Constants<float>::sqrt3());
diff --git a/src/Math/Test/VectorTest.cpp b/src/Math/Test/VectorTest.cpp
index 3b1dc417c..d8d9f303f 100644
--- a/src/Math/Test/VectorTest.cpp
+++ b/src/Math/Test/VectorTest.cpp
@@ -140,12 +140,17 @@ void VectorTest::max() {
 void VectorTest::angle() {
     std::ostringstream o;
     Error::setOutput(&o);
-    /* Both vectors must be normalized, otherwise NaN is returned */
-    CORRADE_COMPARE(Vector3::angle(Vector3(2.0f, 3.0f, 4.0f).normalized(), {1.0f, -2.0f, 3.0f}), std::numeric_limits<Vector3::Type>::quiet_NaN());
+    CORRADE_COMPARE(Vector3::angle(Vector3(2.0f, 3.0f, 4.0f).normalized(), {1.0f, -2.0f, 3.0f}),
+                    std::numeric_limits<Vector3::Type>::quiet_NaN());
     CORRADE_COMPARE(o.str(), "Math::Vector::angle(): vectors must be normalized\n");
-    CORRADE_COMPARE(Vector3::angle({2.0f, 3.0f, 4.0f}, Vector3(1.0f, -2.0f, 3.0f).normalized()), std::numeric_limits<Vector3::Type>::quiet_NaN());
 
-    CORRADE_COMPARE(Vector3::angle(Vector3(2.0f, 3.0f, 4.0f).normalized(), Vector3(1.0f, -2.0f, 3.0f).normalized()), rad(1.162514f));
+    o.str("");
+    CORRADE_COMPARE(Vector3::angle({2.0f, 3.0f, 4.0f}, Vector3(1.0f, -2.0f, 3.0f).normalized()),
+                    std::numeric_limits<Vector3::Type>::quiet_NaN());
+    CORRADE_COMPARE(o.str(), "Math::Vector::angle(): vectors must be normalized\n");
+
+    CORRADE_COMPARE(Vector3::angle(Vector3(2.0f, 3.0f, 4.0f).normalized(), Vector3(1.0f, -2.0f, 3.0f).normalized()),
+                    rad(1.162514f));
 }
 
 void VectorTest::debug() {