AP_Math: Matrix3: add inverse() and invert() functions

Additionally, add unit tests.

libraries/AP_Math/matrix3.cpp

@@ -227,6 +227,39 @@ T Matrix3<T>::det() const
            a.z * (b.x * c.y - b.y * c.x);
 }
 
+template <typename T>
+bool Matrix3<T>::inverse(Matrix3<T>& inv) const
+{
+    T d = det();
+
+    if (is_zero(d)) {
+        return false;
+    }
+
+    inv.a.x = (b.y * c.z - c.y * b.z) / d;
+    inv.a.y = (a.z * c.y - a.y * c.z) / d;
+    inv.a.z = (a.y * b.z - a.z * b.y) / d;
+    inv.b.x = (b.z * c.x - b.x * c.z) / d;
+    inv.b.y = (a.x * c.z - a.z * c.x) / d;
+    inv.b.z = (b.x * a.z - a.x * b.z) / d;
+    inv.c.x = (b.x * c.y - c.x * b.y) / d;
+    inv.c.y = (c.x * a.y - a.x * c.y) / d;
+    inv.c.z = (a.x * b.y - b.x * a.y) / d;
+
+    return true;
+}
+
+template <typename T>
+bool Matrix3<T>::invert()
+{
+    Matrix3<T> inv;
+    bool success = inverse(inv);
+    if (success) {
+        *this = inv;
+    }
+    return success;
+}
+
 template <typename T>
 void Matrix3<T>::zero(void)
 {
@@ -276,6 +309,8 @@ template Vector3<float> Matrix3<float>::mul_transpose(const Vector3<float> &v) c
 template Matrix3<float> Matrix3<float>::operator *(const Matrix3<float> &m) const;
 template Matrix3<float> Matrix3<float>::transposed(void) const;
 template float Matrix3<float>::det() const;
+template bool Matrix3<float>::inverse(Matrix3<float>& inv) const;
+template bool Matrix3<float>::invert();
 template Vector2<float> Matrix3<float>::mulXY(const Vector3<float> &v) const;
 
 template void Matrix3<double>::zero(void);
@@ -289,4 +324,6 @@ template Vector3<double> Matrix3<double>::mul_transpose(const Vector3<double> &v
 template Matrix3<double> Matrix3<double>::operator *(const Matrix3<double> &m) const;
 template Matrix3<double> Matrix3<double>::transposed(void) const;
 template double Matrix3<double>::det() const;
+template bool Matrix3<double>::inverse(Matrix3<double>& inv) const;
+template bool Matrix3<double>::invert();
 template Vector2<double> Matrix3<double>::mulXY(const Vector3<double> &v) const;

libraries/AP_Math/matrix3.h

@@ -190,6 +190,24 @@ public:
      */
     T det() const;
 
+    /**
+     * Calculate the inverse of this matrix.
+     *
+     * @param inv[in] Where to store the result.
+     *
+     * @return If this matrix is invertible, then true is returned. Otherwise,
+     * \p inv is unmodified and false is returned.
+     */
+    bool inverse(Matrix3<T>& inv) const;
+
+    /**
+     * Invert this matrix if it is invertible.
+     *
+     * @return Return true if this matrix could be successfully inverted and
+     * false otherwise.
+     */
+    bool invert();
+
     // zero the matrix
     void        zero(void);
 

libraries/AP_Math/tests/test_matrix3.cpp

@@ -17,6 +17,18 @@
 
 #include "math_test.h"
 
+#define AP_EXPECT_IDENTITY_MATRIX(m_) {\
+    EXPECT_NEAR(1.0f, m_.a.x, 1.0e-6); \
+    EXPECT_NEAR(0.0f, m_.a.y, 1.0e-6); \
+    EXPECT_NEAR(0.0f, m_.a.z, 1.0e-6); \
+    EXPECT_NEAR(0.0f, m_.b.x, 1.0e-6); \
+    EXPECT_NEAR(1.0f, m_.b.y, 1.0e-6); \
+    EXPECT_NEAR(0.0f, m_.b.z, 1.0e-6); \
+    EXPECT_NEAR(0.0f, m_.c.x, 1.0e-6); \
+    EXPECT_NEAR(0.0f, m_.c.y, 1.0e-6); \
+    EXPECT_NEAR(1.0f, m_.c.z, 1.0e-6); \
+}
+
 class TestParam {
 public:
     /**
@@ -69,6 +81,21 @@ TEST_P(Matrix3fTest, Determinants)
     EXPECT_FLOAT_EQ(param.det, param.m.det());
 }
 
+TEST_P(Matrix3fTest, Inverses)
+{
+    auto param = GetParam();
+    Matrix3f inv;
+    bool success = param.m.inverse(inv);
+
+    if (param.det == 0.0f) {
+        EXPECT_FALSE(success);
+    } else {
+        ASSERT_TRUE(success);
+        auto identity = inv * param.m;
+        AP_EXPECT_IDENTITY_MATRIX(identity);
+    }
+}
+
 INSTANTIATE_TEST_CASE_P(InvertibleMatrices,
                         Matrix3fTest,
                         ::testing::ValuesIn(invertible));