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

@@ -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);

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;