From ad424c3610364192c9704965295e75c42dc3ae07 Mon Sep 17 00:00:00 2001
From: Ramasamy <ram.kannan@samsung.com>
Date: Wed, 18 Mar 2015 13:32:51 +0530
Subject: [PATCH] Improved rotation matrix to quaternion conversion

- This has better azimuth computation response over previous
implementation.

Change-Id: Ie631f1f3e3815c4553d4bb51efdabf23738f680a
---
 src/sensor_fusion/rotation_matrix.cpp | 54 +++++++++++------------------------
 1 file changed, 16 insertions(+), 38 deletions(-)

diff --git a/src/sensor_fusion/rotation_matrix.cpp b/src/sensor_fusion/rotation_matrix.cpp
index 83ff5d3..fd3e6b5 100644
--- a/src/sensor_fusion/rotation_matrix.cpp
+++ b/src/sensor_fusion/rotation_matrix.cpp
@@ -95,44 +95,22 @@ template <typename T>
 quaternion<T> rot_mat2quat(rotation_matrix<T> rm)
 {
 	T q0, q1, q2, q3;
-
-	T diag_sum = rm.m_rot_mat.m_mat[0][0] + rm.m_rot_mat.m_mat[1][1] + rm.m_rot_mat.m_mat[2][2];
-
-	if ( diag_sum > 0 )
-	{
-		T val = (T) 0.5 / sqrt(diag_sum + (T) 1.0);
-		q0 = (T) 0.25 / val;
-		q1 = ( rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) * val;
-		q2 = ( rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) * val;
-		q3 = ( rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) * val;
-	}
-	else
-	{
-		if ( rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[1][1] && rm.m_rot_mat.m_mat[0][0] > rm.m_rot_mat.m_mat[2][2] )
-		{
-			T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[2][2]);
-			q0 = (rm.m_rot_mat.m_mat[2][1] - rm.m_rot_mat.m_mat[1][2] ) / val;
-			q1 = (T) 0.25 * val;
-			q2 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val;
-			q3 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val;
-		}
-		else if (rm.m_rot_mat.m_mat[1][1] > rm.m_rot_mat.m_mat[2][2])
-		{
-			T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[1][1] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[2][2]);
-			q0 = (rm.m_rot_mat.m_mat[0][2] - rm.m_rot_mat.m_mat[2][0] ) / val;
-			q1 = (rm.m_rot_mat.m_mat[0][1] + rm.m_rot_mat.m_mat[1][0] ) / val;
-			q2 = (T) 0.25 * val;
-			q3 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val;
-		}
-		else
-		{
-			T val = (T) 2.0 * sqrt( 1.0 + rm.m_rot_mat.m_mat[2][2] - rm.m_rot_mat.m_mat[0][0] - rm.m_rot_mat.m_mat[1][1] );
-			q0 = (rm.m_rot_mat.m_mat[1][0] - rm.m_rot_mat.m_mat[0][1] ) / val;
-			q1 = (rm.m_rot_mat.m_mat[0][2] + rm.m_rot_mat.m_mat[2][0] ) / val;
-			q2 = (rm.m_rot_mat.m_mat[1][2] + rm.m_rot_mat.m_mat[2][1] ) / val;
-			q3 = (T) 0.25 * val;
-		}
-	}
+	T phi, theta, psi;
+
+	phi = atan2(rm.m_rot_mat.m_mat[2][1], rm.m_rot_mat.m_mat[2][2]);
+	theta = atan2(-rm.m_rot_mat.m_mat[2][0],
+			sqrt((rm.m_rot_mat.m_mat[2][1] * rm.m_rot_mat.m_mat[2][1]) +
+					(rm.m_rot_mat.m_mat[2][2] * rm.m_rot_mat.m_mat[2][2])));
+	psi = atan2(rm.m_rot_mat.m_mat[1][0], rm.m_rot_mat.m_mat[0][0]);
+
+	q0 = (cos(phi/2) * cos(theta/2) * cos(psi/2)) +
+			(sin(phi/2) * sin(theta/2) * sin(psi/2));
+	q1 = (-cos(phi/2) * sin(theta/2) * sin(psi/2)) +
+			(sin(phi/2) * cos(theta/2) * cos(psi/2));
+	q2 = (cos(phi/2) * sin(theta/2) * cos(psi/2)) +
+			(sin(phi/2) * cos(theta/2) * sin(psi/2));
+	q3 = (cos(phi/2) * cos(theta/2) * sin(psi/2)) -
+			(sin(phi/2) * sin(theta/2) * cos(psi/2));
 
 	quaternion<T> q(q0, q1, q2, q3);
 
-- 
2.7.4