AP_Math: Matrix3: add det() function

Function to calculate determinant. Additionally, add unit tests.

libraries/AP_Math/matrix3.cpp

@@ -219,6 +219,14 @@ Matrix3<T> Matrix3<T>::transposed(void) const
                       Vector3<T>(a.z, b.z, c.z));
 }
 
+template <typename T>
+T Matrix3<T>::det() const
+{
+    return a.x * (b.y * c.z - b.z * c.y) +
+           a.y * (b.z * c.x - b.x * c.z) +
+           a.z * (b.x * c.y - b.y * c.x);
+}
+
 template <typename T>
 void Matrix3<T>::zero(void)
 {
@@ -267,6 +275,7 @@ template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) cons
 template Vector3<float> Matrix3<float>::mul_transpose(const Vector3<float> &v) const;
 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 Vector2<float> Matrix3<float>::mulXY(const Vector3<float> &v) const;
 
 template void Matrix3<double>::zero(void);
@@ -279,4 +288,5 @@ template Vector3<double> Matrix3<double>::operator *(const Vector3<double> &v) c
 template Vector3<double> Matrix3<double>::mul_transpose(const Vector3<double> &v) const;
 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 Vector2<double> Matrix3<double>::mulXY(const Vector3<double> &v) const;

libraries/AP_Math/matrix3.h

@@ -183,6 +183,13 @@ public:
         *this = transposed();
     }
 
+    /**
+     * Calculate the determinant of this matrix.
+     *
+     * @return The value of the determinant.
+     */
+    T det() const;
+
     // zero the matrix
     void        zero(void);
 

libraries/AP_Math/tests/math_test.h

@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2016  Intel Corporation. All rights reserved.
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <iostream>
+
+#include <AP_gtest.h>
+#include <AP_Math/AP_Math.h>
+
+/*
+ * Overload operator << to allow nice printing for failed tests.
+ */
+template <typename T>
+::std::ostream& operator<<(::std::ostream& os, const Matrix3<T>& m)
+{
+    return os << "{{" << m.a.x << ", " << m.a.y << ", " << m.a.z << "}, " <<
+                  "{" << m.b.x << ", " << m.b.y << ", " << m.b.z << "}, " <<
+                  "{" << m.c.x << ", " << m.c.y << ", " << m.c.z << "}}";
+}

libraries/AP_Math/tests/test_matrix3.cpp

@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2016  Intel Corporation. All rights reserved.
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "math_test.h"
+
+class TestParam {
+public:
+    /**
+     * The matrix for this param.
+     */
+    Matrix3f m;
+    /**
+     * The expected determinant for #m.
+     */
+    float det;
+};
+
+AP_GTEST_PRINTATBLE_PARAM_MEMBER(TestParam, m);
+
+class Matrix3fTest : public ::testing::TestWithParam<TestParam> {};
+
+static TestParam invertible[] = {
+    {
+        .m = {
+            {1.0f,  2.0f,  3.0f},
+            {4.0f,  6.0f,  2.0f},
+            {9.0f, 18.0f, 27.0f}
+        },
+        .det = 0.0f,
+    },
+};
+
+static TestParam non_invertible[] = {
+    {
+        .m = {
+            { 6.0f,  2.0f,  20.0f},
+            { 1.0f, -9.0f,   4.0f},
+            {-4.0f,  7.0f, -27.0f}
+        },
+        .det = 732.0f,
+    },
+    {
+        .m = {
+            {-6.0f, -2.0f, -20.0f},
+            {-1.0f,  9.0f,  -4.0f},
+            { 4.0f, -7.0f,  27.0f}
+        },
+        .det = -732.0f,
+    },
+};
+
+TEST_P(Matrix3fTest, Determinants)
+{
+    auto param = GetParam();
+    EXPECT_FLOAT_EQ(param.det, param.m.det());
+}
+
+INSTANTIATE_TEST_CASE_P(InvertibleMatrices,
+                        Matrix3fTest,
+                        ::testing::ValuesIn(invertible));
+
+INSTANTIATE_TEST_CASE_P(NonInvertibleMatrices,
+                        Matrix3fTest,
+                        ::testing::ValuesIn(non_invertible));
+
+AP_GTEST_MAIN()