From 7e9f7a462b144bfa718267c31cb4a1da76b5f4e7 Mon Sep 17 00:00:00 2001
From: Kearwood Gilbert <kearwood@kearwood.com>
Date: Mon, 26 Feb 2018 17:49:41 -0800
Subject: [PATCH] Added some quaternion functions

---
 include/quaternion.h |  4 ++++
 src/quaternion.cpp   | 46 ++++++++++++++++++++++++++++++++++++++++++--
 2 files changed, 48 insertions(+), 2 deletions(-)

diff --git a/include/quaternion.h b/include/quaternion.h
index 45a5ab7..dc12380 100644
--- a/include/quaternion.h
+++ b/include/quaternion.h
@@ -86,8 +86,12 @@ public:
     
   void conjugate();
   static Quaternion Conjugate(const Quaternion &v1);
+
+  void invert();
+  static Quaternion Invert(const Quaternion &v1);
     
   static Quaternion FromAngleAxis(const Vector3 &axis, float angle);
+  static Quaternion FromRotationMatrix(const Matrix4 &m);
   static Quaternion Lerp(const Quaternion &a, const Quaternion &b, float t);
   static Quaternion Slerp(const Quaternion &a, const Quaternion &b, float t);
   static float Dot(const Quaternion &v1, const Quaternion &v2);
diff --git a/src/quaternion.cpp b/src/quaternion.cpp
index 9e893aa..6deba0a 100644
--- a/src/quaternion.cpp
+++ b/src/quaternion.cpp
@@ -274,7 +274,7 @@ Quaternion Quaternion::operator -() const {
     return Quaternion::Create(-c[0], -c[1], -c[2], -c[3]);
 }
 
-Quaternion Normalize(const Quaternion &v1) {
+Quaternion Quaternion::Normalize(const Quaternion &v1) {
     float inv_magnitude = 1.0f / sqrtf(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2] + v1[3] * v1[3]);
     return Quaternion::Create(
         v1[0] * inv_magnitude,
@@ -292,7 +292,7 @@ void Quaternion::normalize() {
     c[3] *= inv_magnitude;
 }
 
-Quaternion Conjugate(const Quaternion &v1) {
+Quaternion Quaternion::Conjugate(const Quaternion &v1) {
     return Quaternion::Create(v1[0], -v1[1], -v1[2], -v1[3]);
 }
 
@@ -302,6 +302,15 @@ void Quaternion::conjugate() {
     c[3] = -c[3];
 }
 
+void Quaternion::invert() {
+  conjugate();
+  normalize();
+}
+
+Quaternion Quaternion::Invert(const Quaternion &v1) {
+  return Normalize(Conjugate(v1));
+}
+
 Matrix4 Quaternion::rotationMatrix() const {
     Matrix4 matRotate;
     
@@ -339,6 +348,39 @@ Quaternion Quaternion::FromAngleAxis(const Vector3 &axis, float angle)
     return Quaternion::Create(cos(ha), axis.x * sha, axis.y * sha, axis.z * sha);
 }
 
+Quaternion Quaternion::FromRotationMatrix(const Matrix4 &m)
+{
+// see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
+  const float trace = m[0] + m[5] + m[10];
+  float w, x, y, z;
+  if (trace > 0.0) {
+    const float s = 0.5f / sqrt(trace + 1.0f);
+    w = 0.25f / s;
+    x = (m[9] - m[6]) * s;
+    y = (m[2] - m[8]) * s;
+    z = (m[4] - m[1]) * s;
+  } else if (m[0] > m[5] && m[0] > m[10]) {
+    const float s = 2.0f * sqrt(1.0f + m[0] - m[5] - m[10]);
+    w = (m[9] - m[6]) / s;
+    x = 0.25f * s;
+    y = (m[1] + m[4]) / s;
+    z = (m[2] + m[8]) / s;
+  } else if (m[5] > m[10]) {
+    const float s = 2.0 * sqrt(1.0f + m[5] - m[0] - m[10]);
+    w = (m[2] - m[8]) / s;
+    x = (m[1] + m[4]) / s;
+    y = 0.25f * s;
+    z = (m[6] + m[9]) / s;
+  } else {
+    const float s = 2.0 * sqrt(1.0f + m[10] - m[0] - m[5]);
+    w = (m[4] - m[1]) / s;
+    x = (m[2] + m[8]) / s;
+    y = (m[6] + m[9]) / s;
+    z = 0.25f * s;
+  }
+   return Quaternion::Create(w, x, y, z);
+}
+
 float Quaternion::Dot(const Quaternion &v1, const Quaternion &v2)
 {
     return v1.c[0] * v2.c[0] + v1.c[1] * v2.c[1] + v1.c[2] * v2.c[2] + v1.c[3] * v2.c[3];