AP_Math: made rotation matrices more C++

thanks to Adam for the suggestion

libraries/AP_Math/AP_Math.cpp

@@ -30,42 +30,3 @@ float safe_sqrt(float v)
 	}
 	return ret;
 }
-
-
-// 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(const 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

@@ -24,10 +24,4 @@ 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(const Matrix3f &m, float *roll, float *pitch, float *yaw);
-
 #endif

libraries/AP_Math/matrix3.cpp

@@ -94,5 +94,46 @@ void Matrix3<T>::rotation(enum Rotation r)
     }
 }
 
+// create a rotation matrix given some euler angles
+// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
+template <typename T>
+void Matrix3<T>::from_euler(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);
+
+	a.x = cp * cy;
+	a.y = (sr * sp * cy) - (cr * sy);
+	a.z = (cr * sp * cy) + (sr * sy);
+	b.x = cp * sy;
+	b.y = (sr * sp * sy) + (cr * cy);
+	b.z = (cr * sp * sy) - (sr * cy);
+	c.x = -sp;
+	c.y = sr * cp;
+	c.z = cr * cp;
+}
+
+// calculate euler angles from a rotation matrix
+// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
+template <typename T>
+void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw)
+{
+	if (pitch != NULL) {
+		*pitch = -safe_asin(c.x);
+	}
+	if (roll != NULL) {
+		*roll = atan2(c.y, c.z);
+	}
+	if (yaw != NULL) {
+		*yaw = atan2(b.x, a.x);
+	}
+}
+
 // only define for float
 template void Matrix3<float>::rotation(enum Rotation);
+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

@@ -131,6 +131,11 @@ public:
 	// fill in the matrix with a standard rotation
 	void rotation(enum Rotation rotation);
 
+    // create a rotation matrix from Euler angles
+	void from_euler(float roll, float pitch, float yaw);
+
+    // create eulers from a rotation matrix
+	void to_euler(float *roll, float *pitch, float *yaw);
 };
 
 typedef Matrix3<int16_t>		Matrix3i;