From 993cd0f8f6d3c799e7a1e531652c0464103939c7 Mon Sep 17 00:00:00 2001
From: Laura Sebesta <kenn@eissq.com>
Date: Mon, 8 Oct 2012 15:17:47 +0200
Subject: [PATCH] Added conversion from rotation matrix to quaternions.

---
 .../src/libs/utils/coordinateconversions.cpp  | 21 +++++++++++++++++++
 .../src/libs/utils/coordinateconversions.h    |  1 +
 2 files changed, 22 insertions(+)

diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
index a7f7117b1..bf5b987e8 100644
--- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
+++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
@@ -257,5 +257,26 @@ void CoordinateConversions::Quaternion2R(const float q[4], float Rbe[3][3])
 	Rbe[2][2] = q0s - q1s - q2s + q3s;
 }
 
+//** Find quaternion vector from a rotation matrix, Rbe, a matrix which rotates a vector from earth frame to body frame **
+void CoordinateConversions::R2Quaternion(float const Rbe[3][3], float q[4])
+{
+    qreal w, x, y, z;
+
+    // w always >= 0
+    w = sqrt(std::max(0.0, 1.0 + Rbe[0][0] + Rbe[1][1] + Rbe[2][2])) / 2.0;
+    x = sqrt(std::max(0.0, 1.0 + Rbe[0][0] - Rbe[1][1] - Rbe[2][2])) / 2.0;
+    y = sqrt(std::max(0.0, 1.0 - Rbe[0][0] + Rbe[1][1] - Rbe[2][2])) / 2.0;
+    z = sqrt(std::max(0.0, 1.0 - Rbe[0][0] - Rbe[1][1] + Rbe[2][2])) / 2.0;
+
+    x = copysign(x, (Rbe[1][2] - Rbe[2][1]));
+    y = copysign(y, (Rbe[2][0] - Rbe[0][2]));
+    z = copysign(z, (Rbe[0][1] - Rbe[1][0]));
+
+    q[0]=w;
+    q[1]=x;
+    q[2]=y;
+    q[3]=z;
+}
+
 
 }
diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h
index ca077a0c4..5f515c147 100644
--- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.h
+++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.h
@@ -50,6 +50,7 @@ public:
     void Quaternion2RPY(const float q[4], float rpy[3]);
     void RPY2Quaternion(const float rpy[3], float q[4]);
     void Quaternion2R(const float q[4], float Rbe[3][3]);
+    void R2Quaternion(float const Rbe[3][3], float q[4]);
 };
 
 }