From 8afd29cc8598fbfce8f8ff21a984df15ab64b9d9 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Fri, 24 Jun 2016 16:10:35 +0900
Subject: [PATCH] AP_Math: add quaternion::rotation_matrix_norm

---
 libraries/AP_Math/quaternion.cpp | 26 ++++++++++++++++++++++++++
 libraries/AP_Math/quaternion.h   |  3 +++
 2 files changed, 29 insertions(+)

diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp
index 2ffa7ae54f..52407ab565 100644
--- a/libraries/AP_Math/quaternion.cpp
+++ b/libraries/AP_Math/quaternion.cpp
@@ -45,6 +45,32 @@ void Quaternion::rotation_matrix(Matrix3f &m) const
     m.c.z = 1.0f-2.0f*(q2q2 + q3q3);
 }
 
+// return the rotation matrix equivalent for this quaternion after normalization
+void Quaternion::rotation_matrix_norm(Matrix3f &m) const
+{
+    float q1q1 = q1 * q1;
+    float q1q2 = q1 * q2;
+    float q1q3 = q1 * q3;
+    float q1q4 = q1 * q4;
+    float q2q2 = q2 * q2;
+    float q2q3 = q2 * q3;
+    float q2q4 = q2 * q4;
+    float q3q3 = q3 * q3;
+    float q3q4 = q3 * q4;
+    float q4q4 = q4 * q4;
+    float invs = 1.0f / (q1q1 + q2q2 + q3q3 + q4q4);
+
+    m.a.x = ( q2q2 - q3q3 - q4q4 + q1q1)*invs;
+    m.a.y = 2.0f*(q2q3 - q1q4)*invs;
+    m.a.z = 2.0f*(q2q4 + q1q3)*invs;
+    m.b.x = 2.0f*(q2q3 + q1q4)*invs;
+    m.b.y = (-q2q2 + q3q3 - q4q4 + q1q1)*invs;
+    m.b.z = 2.0f*(q3q4 - q1q2)*invs;
+    m.c.x = 2.0f*(q2q4 - q1q3)*invs;
+    m.c.y = 2.0f*(q3q4 + q1q2)*invs;
+    m.c.z = (-q2q2 - q3q3 + q4q4 + q1q1)*invs;
+}
+
 // return the rotation matrix equivalent for this quaternion
 // Thanks to Martin John Baker
 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h
index 37845c7663..5e0dd92828 100644
--- a/libraries/AP_Math/quaternion.h
+++ b/libraries/AP_Math/quaternion.h
@@ -59,6 +59,9 @@ public:
     // return the rotation matrix equivalent for this quaternion
     void        rotation_matrix(Matrix3f &m) const;
 
+    // return the rotation matrix equivalent for this quaternion after normalization
+    void        rotation_matrix_norm(Matrix3f &m) const;
+
     void		from_rotation_matrix(const Matrix3f &m);
 
     // convert a vector from earth to body frame