From 3b92d2b9f3889f834ab2df0d771335780b8782e0 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 15:40:08 +0100
Subject: [PATCH] Math: creating Quaternion from axis/angle.

---
 src/Math/Quaternion.h                | 39 ++++++++++++++++++++++++++++
 src/Math/Test/MathQuaternionTest.cpp | 19 ++++++++++++++
 2 files changed, 58 insertions(+)

diff --git a/src/Math/Quaternion.h b/src/Math/Quaternion.h
index f41c93c73..5cdcb9dbe 100644
--- a/src/Math/Quaternion.h
+++ b/src/Math/Quaternion.h
@@ -23,6 +23,7 @@
 #include <Utility/Assert.h>
 #include <Utility/Debug.h>
 
+#include "Math/Math.h"
 #include "Math/MathTypeTraits.h"
 #include "Math/Vector3.h"
 
@@ -31,6 +32,20 @@ namespace Magnum { namespace Math {
 /** @brief %Quaternion */
 template<class T> class Quaternion {
     public:
+        /**
+         * @brief Create quaternion from rotation
+         * @param angle             Rotation angle (counterclockwise, in radians)
+         * @param normalizedAxis    Normalized rotation axis
+         *
+         * Assumes that the rotation axis is normalized.
+         */
+        inline static Quaternion<T> fromRotation(T angle, const Vector3<T>& normalizedAxis) {
+            CORRADE_ASSERT(MathTypeTraits<T>::equals(normalizedAxis.dot(), T(1)),
+                           "Math::Quaternion::fromRotation(): axis must be normalized", {});
+
+            return {normalizedAxis*std::sin(angle/2), std::cos(angle/2)};
+        }
+
         /** @brief Default constructor */
         inline constexpr Quaternion(): _scalar(T(1)) {}
 
@@ -53,6 +68,30 @@ template<class T> class Quaternion {
         /** @brief %Scalar part */
         inline constexpr T scalar() const { return _scalar; }
 
+        /**
+         * @brief Rotation angle of unit quaternion
+         *
+         * Assumes that the quaternion is normalized.
+         */
+        inline T rotationAngle() const {
+            CORRADE_ASSERT(MathTypeTraits<T>::equals(lengthSquared(), T(1)),
+                           "Math::Quaternion::rotationAngle(): quaternion must be normalized",
+                           std::numeric_limits<T>::quiet_NaN());
+            return T(2)*std::acos(_scalar);
+        }
+
+        /**
+         * @brief Rotation axis of unit quaternion
+         *
+         * Assumes that the quaternion is normalized.
+         */
+        inline Vector3<T> rotationAxis() const {
+            CORRADE_ASSERT(MathTypeTraits<T>::equals(lengthSquared(), T(1)),
+                           "Math::Quaternion::rotationAxis(): quaternion must be normalized",
+                           {});
+            return _vector/std::sqrt(1-pow<2>(_scalar));
+        }
+
         /**
          * @brief Multiply with scalar and assign
          *
diff --git a/src/Math/Test/MathQuaternionTest.cpp b/src/Math/Test/MathQuaternionTest.cpp
index 0e3de5aa0..1b8a351eb 100644
--- a/src/Math/Test/MathQuaternionTest.cpp
+++ b/src/Math/Test/MathQuaternionTest.cpp
@@ -16,6 +16,7 @@
 #include <sstream>
 #include <TestSuite/Tester.h>
 
+#include "Math/Constants.h"
 #include "Math/Quaternion.h"
 
 namespace Magnum { namespace Math { namespace Test {
@@ -32,6 +33,7 @@ class QuaternionTest: public Corrade::TestSuite::Tester {
         void conjugated();
         void inverted();
         void invertedNormalized();
+        void rotation();
 
         void debug();
 };
@@ -48,6 +50,7 @@ QuaternionTest::QuaternionTest() {
              &QuaternionTest::conjugated,
              &QuaternionTest::inverted,
              &QuaternionTest::invertedNormalized,
+             &QuaternionTest::rotation,
              &QuaternionTest::debug);
 }
 
@@ -113,6 +116,22 @@ void QuaternionTest::invertedNormalized() {
     CORRADE_COMPARE(inverted, Quaternion({-1.0f, -3.0f, 2.0f}, -4.0f)/std::sqrt(30.0f));
 }
 
+void QuaternionTest::rotation() {
+    float angle = deg(120.0f);
+    Vector3 axis(1.0f/Constants<float>::sqrt3());
+    Quaternion q = Quaternion::fromRotation(angle, axis);
+    CORRADE_COMPARE(q, Quaternion(Vector3(0.5f, 0.5f, 0.5f), 0.5f));
+    CORRADE_COMPARE(q.rotationAngle(), angle);
+    CORRADE_COMPARE(q.rotationAxis(), axis);
+    CORRADE_COMPARE(q.rotationAxis().length(), 1.0f);
+
+    /* Verify negative angle */
+    Quaternion q2 = Quaternion::fromRotation(deg(-120.0f), axis);
+    CORRADE_COMPARE(q2, Quaternion(Vector3(-0.5f, -0.5f, -0.5f), 0.5f));
+    CORRADE_COMPARE(q2.rotationAngle(), deg(120.0f));
+    CORRADE_COMPARE(q2.rotationAxis(), -axis);
+}
+
 void QuaternionTest::debug() {
     std::ostringstream o;