From 06eea2838f02ee042bd267eda8643a9d933fde89 Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Thu, 7 Apr 2016 19:57:57 -0300 Subject: [PATCH] AP_Math: Matrix3: add det() function Function to calculate determinant. Additionally, add unit tests. --- libraries/AP_Math/matrix3.cpp | 10 +++ libraries/AP_Math/matrix3.h | 7 +++ libraries/AP_Math/tests/math_test.h | 33 ++++++++++ libraries/AP_Math/tests/test_matrix3.cpp | 80 ++++++++++++++++++++++++ 4 files changed, 130 insertions(+) create mode 100644 libraries/AP_Math/tests/math_test.h create mode 100644 libraries/AP_Math/tests/test_matrix3.cpp diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 3a178bdc88..66e1edbab4 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -219,6 +219,14 @@ Matrix3 Matrix3::transposed(void) const Vector3(a.z, b.z, c.z)); } +template +T Matrix3::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 void Matrix3::zero(void) { @@ -267,6 +275,7 @@ template Vector3 Matrix3::operator *(const Vector3 &v) cons template Vector3 Matrix3::mul_transpose(const Vector3 &v) const; template Matrix3 Matrix3::operator *(const Matrix3 &m) const; template Matrix3 Matrix3::transposed(void) const; +template float Matrix3::det() const; template Vector2 Matrix3::mulXY(const Vector3 &v) const; template void Matrix3::zero(void); @@ -279,4 +288,5 @@ template Vector3 Matrix3::operator *(const Vector3 &v) c template Vector3 Matrix3::mul_transpose(const Vector3 &v) const; template Matrix3 Matrix3::operator *(const Matrix3 &m) const; template Matrix3 Matrix3::transposed(void) const; +template double Matrix3::det() const; template Vector2 Matrix3::mulXY(const Vector3 &v) const; diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index 17078e3c8c..e076d233fb 100644 --- a/libraries/AP_Math/matrix3.h +++ b/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); diff --git a/libraries/AP_Math/tests/math_test.h b/libraries/AP_Math/tests/math_test.h new file mode 100644 index 0000000000..b92e726e86 --- /dev/null +++ b/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 . + */ +#pragma once + +#include + +#include +#include + +/* + * Overload operator << to allow nice printing for failed tests. + */ +template +::std::ostream& operator<<(::std::ostream& os, const Matrix3& 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 << "}}"; +} diff --git a/libraries/AP_Math/tests/test_matrix3.cpp b/libraries/AP_Math/tests/test_matrix3.cpp new file mode 100644 index 0000000000..a3ad10f5a7 --- /dev/null +++ b/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 . + */ + +#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 {}; + +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()