AP_Math: added rotmat <-> euler functions

these will make the dcm matrix manipulation easier to understand

libraries/AP_Math/AP_Math.cpp

@@ -29,3 +29,41 @@ float safe_sqrt(float v)
 	}
 	return sqrt(v);
 }
+
+
+// create a rotation matrix given some euler angles
+// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
+void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw)
+{
+	float cp = cos(pitch);
+	float sp = sin(pitch);
+	float sr = sin(roll);
+	float cr = cos(roll);
+	float sy = sin(yaw);
+	float cy = cos(yaw);
+
+	m.a.x = cp * cy;
+	m.a.y = (sr * sp * cy) - (cr * sy);
+	m.a.z = (cr * sp * cy) + (sr * sy);
+	m.b.x = cp * sy;
+	m.b.y = (sr * sp * sy) + (cr * cy);
+	m.b.z = (cr * sp * sy) - (sr * cy);
+	m.c.x = -sp;
+	m.c.y = sr * cp;
+	m.c.z = cr * cp;
+}
+
+// calculate euler angles from a rotation matrix
+// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
+void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw)
+{
+	if (pitch != NULL) {
+		*pitch	= -safe_asin(m.c.x);
+	}
+	if (roll != NULL) {
+		*roll 	= atan2(m.c.y, m.c.z);
+	}
+	if (yaw != NULL) {
+		*yaw 	= atan2(m.b.x, m.a.x);
+	}
+}

libraries/AP_Math/AP_Math.h

@@ -18,3 +18,9 @@ float safe_asin(float v);
 
 // a varient of sqrt() that always gives a valid answer.
 float safe_sqrt(float v);
+
+// create a rotation matrix given some euler angles
+void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw);
+
+// calculate euler angles from a rotation matrix
+void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw);