From 03eb3f62c5036e0ca4bc3226c34404eacd8ea3ca Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 25 Apr 2018 19:48:58 +1000
Subject: [PATCH] AP_Math: added from_rotation() to Matrix3

backport from master
---
 libraries/AP_Math/matrix3.cpp | 14 ++++++++++++++
 libraries/AP_Math/matrix3.h   |  3 +++
 2 files changed, 17 insertions(+)

diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp
index df3f7442b2..d6dcd0743c 100644
--- a/libraries/AP_Math/matrix3.cpp
+++ b/libraries/AP_Math/matrix3.cpp
@@ -59,6 +59,19 @@ void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw) const
     }
 }
 
+template <typename T>
+void Matrix3<T>::from_rotation(enum Rotation rotation)
+{
+    (*this).a(1,0,0);
+    (*this).b(0,1,0);
+    (*this).c(0,0,1);
+
+    (*this).a.rotate(rotation);
+    (*this).b.rotate(rotation);
+    (*this).c.rotate(rotation);
+    (*this).transpose();
+}
+
 /*
   calculate Euler angles (312 convention) for the matrix.
   See http://www.atacolorado.com/eulersequences.doc
@@ -260,6 +273,7 @@ template void Matrix3<float>::rotate(const Vector3<float> &g);
 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_rotation(enum Rotation rotation);
 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;
diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h
index 024b883dd3..6c58c8c52e 100644
--- a/libraries/AP_Math/matrix3.h
+++ b/libraries/AP_Math/matrix3.h
@@ -235,6 +235,9 @@ public:
     // create eulers from a rotation matrix
     void        to_euler(float *roll, float *pitch, float *yaw) const;
 
+    // create matrix from rotation enum
+    void from_rotation(enum Rotation rotation);
+    
     /*
       calculate Euler angles (312 convention) for the matrix.
       See http://www.atacolorado.com/eulersequences.doc