From 689f230d4074c281e51ee5200eb09a976c375cea Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 15 Feb 2014 12:20:31 +1100
Subject: [PATCH] AP_Math: added quaternion.from_rotation_matrix()

---
 libraries/AP_Math/quaternion.cpp | 48 ++++++++++++++++++++++++++++++++
 libraries/AP_Math/quaternion.h   |  2 ++
 2 files changed, 50 insertions(+)

diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp
index 163d716bce..7d9a06e33f 100644
--- a/libraries/AP_Math/quaternion.cpp
+++ b/libraries/AP_Math/quaternion.cpp
@@ -43,6 +43,54 @@ void Quaternion::rotation_matrix(Matrix3f &m) const
     m.c.z = 1-2*(q2q2 + q3q3);
 }
 
+// return the rotation matrix equivalent for this quaternion
+// Thanks to Martin John Baker
+// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
+void Quaternion::from_rotation_matrix(const Matrix3f &m)
+{
+    const float &m00 = m.a.x;
+    const float &m11 = m.b.y;
+    const float &m22 = m.c.z;
+    const float &m10 = m.b.x;
+    const float &m01 = m.a.y;
+    const float &m20 = m.c.x;
+    const float &m02 = m.a.z;
+    const float &m21 = m.c.y;
+    const float &m12 = m.b.z;
+    float &qw = q1;
+    float &qx = q2;
+    float &qy = q3;
+    float &qz = q4;
+
+    float tr = m00 + m11 + m22;
+
+    if (tr > 0) {
+        float S = sqrtf(tr+1) * 2;
+        qw = 0.25f * S;
+        qx = (m21 - m12) / S;
+        qy = (m02 - m20) / S; 
+        qz = (m10 - m01) / S; 
+    } else if ((m00 > m11) && (m00 > m22)) { 
+        float S = sqrtf(1.0 + m00 - m11 - m22) * 2;
+        qw = (m21 - m12) / S;
+        qx = 0.25f * S;
+        qy = (m01 + m10) / S; 
+        qz = (m02 + m20) / S; 
+    } else if (m11 > m22) { 
+        float S = sqrtf(1.0 + m11 - m00 - m22) * 2;
+        qw = (m02 - m20) / S;
+        qx = (m01 + m10) / S; 
+        qy = 0.25f * S;
+        qz = (m12 + m21) / S; 
+    } else { 
+        float S = sqrtf(1.0 + m22 - m00 - m11) * 2;
+        qw = (m10 - m01) / S;
+        qx = (m02 + m20) / S;
+        qy = (m12 + m21) / S;
+        qz = 0.25f * S;
+    }
+}
+
 // convert a vector from earth to body frame
 void Quaternion::earth_to_body(Vector3f &v) const
 {
diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h
index 5387ad3b0b..33aff95c8b 100644
--- a/libraries/AP_Math/quaternion.h
+++ b/libraries/AP_Math/quaternion.h
@@ -55,6 +55,8 @@ public:
     // return the rotation matrix equivalent for this quaternion
     void        rotation_matrix(Matrix3f &m) const;
 
+    void		from_rotation_matrix(const Matrix3f &m);
+
     // convert a vector from earth to body frame
     void        earth_to_body(Vector3f &v) const;