From c16b353ea75961b61f7729ccec55743c54d34afd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 19 Mar 2012 17:22:11 +1100
Subject: [PATCH] Math: added rotate() method to Matrix3f

this is the core method used to update the DCM matrix with a gyro
vector. Moving it to AP_Math allows us to have a test for it
---
 libraries/AP_Math/matrix3.cpp | 20 ++++++++++++++++++++
 libraries/AP_Math/matrix3.h   |  4 ++++
 2 files changed, 24 insertions(+)

diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp
index 133c7bdd6e..d819a37263 100644
--- a/libraries/AP_Math/matrix3.cpp
+++ b/libraries/AP_Math/matrix3.cpp
@@ -134,7 +134,27 @@ void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw)
 	}
 }
 
+// apply an additional rotation from a body frame gyro vector
+// to a rotation matrix.
+template <typename T>
+void Matrix3<T>::rotate(const Vector3<T> &g)
+{
+	Matrix3f temp_matrix;
+	temp_matrix.a.x = a.y * g.z - a.z * g.y;
+	temp_matrix.a.y = a.z * g.x - a.x * g.z;
+	temp_matrix.a.z = a.x * g.y - a.y * g.x;
+	temp_matrix.b.x = b.y * g.z - b.z * g.y;
+	temp_matrix.b.y = b.z * g.x - b.x * g.z;
+	temp_matrix.b.z = b.x * g.y - b.y * g.x;
+	temp_matrix.c.x = c.y * g.z - c.z * g.y;
+	temp_matrix.c.y = c.z * g.x - c.x * g.z;
+	temp_matrix.c.z = c.x * g.y - c.y * g.x;
+
+	(*this) += temp_matrix;
+}
+
 // only define for float
 template void Matrix3<float>::rotation(enum Rotation);
+template void Matrix3<float>::rotate(const Vector3<float> &g);
 template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
 template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);
diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h
index 1f16d23717..db5cbe088e 100644
--- a/libraries/AP_Math/matrix3.h
+++ b/libraries/AP_Math/matrix3.h
@@ -151,6 +151,10 @@ public:
 
     // create eulers from a rotation matrix
 	void to_euler(float *roll, float *pitch, float *yaw);
+
+    // apply an additional rotation from a body frame gyro vector
+    // to a rotation matrix.
+	void rotate(const Vector3<T> &g);
 };
 
 typedef Matrix3<int16_t>		Matrix3i;