AP_Math: added from_axis_angle() method on Matrix3f

for arbitrary rotations in simulator

libraries/AP_Math/examples/rotations/rotations.cpp

@@ -25,6 +25,68 @@ static void test_rotation_accuracy(void)
 
     hal.console->println("\nRotation method accuracy:");
 
+    // test roll
+    for( i=0; i<90; i++ ) {
+
+        // reset initial attitude
+        attitude.from_euler(0,0,0);
+
+        // calculate small rotation vector
+        rot_angle = ToRad(i);
+        small_rotation = Vector3f(rot_angle,0,0);
+
+        // apply small rotation
+        attitude.rotate(small_rotation);
+
+        // get resulting attitude's euler angles
+        attitude.to_euler(&roll, &pitch, &yaw);
+
+        // now try via from_axis_angle
+        Matrix3f r2;
+        r2.from_axis_angle(Vector3f(1,0,0), rot_angle);
+        attitude.from_euler(0,0,0);
+        attitude = r2 * attitude;
+
+        float roll2, pitch2, yaw2;
+        attitude.to_euler(&roll2, &pitch2, &yaw2);
+        
+        // display results
+        hal.console->printf("actual angle: %d  angle1:%4.2f  angle2:%4.2f\n",
+                            (int)i,ToDeg(roll), ToDeg(roll2));
+    }
+
+    // test pitch
+    for( i=0; i<90; i++ ) {
+
+        // reset initial attitude
+        attitude.from_euler(0,0,0);
+
+        // calculate small rotation vector
+        rot_angle = ToRad(i);
+        small_rotation = Vector3f(0,rot_angle,0);
+
+        // apply small rotation
+        attitude.rotate(small_rotation);
+
+        // get resulting attitude's euler angles
+        attitude.to_euler(&roll, &pitch, &yaw);
+
+        // now try via from_axis_angle
+        Matrix3f r2;
+        r2.from_axis_angle(Vector3f(0,1,0), rot_angle);
+        attitude.from_euler(0,0,0);
+        attitude = r2 * attitude;
+
+        float roll2, pitch2, yaw2;
+        attitude.to_euler(&roll2, &pitch2, &yaw2);
+        
+        // display results
+        hal.console->printf("actual angle: %d  angle1:%4.2f  angle2:%4.2f\n",
+                            (int)i,ToDeg(pitch), ToDeg(pitch2));
+    }
+    
+
+    // test yaw
     for( i=0; i<90; i++ ) {
 
         // reset initial attitude
@@ -40,10 +102,18 @@ static void test_rotation_accuracy(void)
         // get resulting attitude's euler angles
         attitude.to_euler(&roll, &pitch, &yaw);
 
+        // now try via from_axis_angle
+        Matrix3f r2;
+        r2.from_axis_angle(Vector3f(0,0,1), rot_angle);
+        attitude.from_euler(0,0,0);
+        attitude = r2 * attitude;
+
+        float roll2, pitch2, yaw2;
+        attitude.to_euler(&roll2, &pitch2, &yaw2);
+        
         // display results
-        hal.console->printf(
-                "actual angle: %d\tcalculated angle:%4.2f\n",
-                (int)i,ToDeg(yaw));
+        hal.console->printf("actual angle: %d  angle1:%4.2f  angle2:%4.2f\n",
+                            (int)i,ToDeg(yaw), ToDeg(yaw2));
     }
 }
 

libraries/AP_Math/matrix3.cpp

@@ -227,6 +227,30 @@ void Matrix3<T>::zero(void)
     c.x = c.y = c.z = 0;
 }
 
+// create rotation matrix for rotation about the vector v by angle theta
+// See: http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/
+template <typename T>
+void Matrix3<T>::from_axis_angle(const Vector3<T> &v, float theta)
+{
+    float C = cosf(theta);
+    float S = sinf(theta);
+    float t = 1.0f - C;
+    Vector3f normv = v.normalized();
+    float x = normv.x;
+    float y = normv.y;
+    float z = normv.z;
+    
+    a.x = t*x*x + C;
+    a.y = t*x*y - z*S;
+    a.z = t*x*z + y*S;
+    b.x = t*x*y + z*S;
+    b.y = t*y*y + C;
+    b.z = t*y*z - x*S;
+    c.x = t*x*z - y*S;
+    c.y = t*y*z + x*S;
+    c.z = t*z*z + C;
+}
+
 
 // only define for float
 template void Matrix3<float>::zero(void);
@@ -237,6 +261,7 @@ template void Matrix3<float>::normalize(void);
 template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
 template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw) const;
 template void Matrix3<float>::from_euler312(float roll, float pitch, float yaw);
+template void Matrix3<float>::from_axis_angle(const Vector3<float> &v, float theta);
 template Vector3<float> Matrix3<float>::to_euler312(void) const;
 template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) const;
 template Vector3<float> Matrix3<float>::mul_transpose(const Vector3<float> &v) const;

libraries/AP_Math/matrix3.h

@@ -230,6 +230,11 @@ public:
     // only use X, Y elements from rotation vector
     void        rotateXYinv(const Vector3<T> &g);
 
+    // create rotation matrix for rotation about the vector v by angle theta
+    // See: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
+    // "Rotation matrix from axis and angle"
+    void        from_axis_angle(const Vector3<T> &v, float theta);
+    
     // normalize a rotation matrix
     void        normalize(void);
 };