diff --git a/src/lib/matrix/matrix/Scalar.hpp b/src/lib/matrix/matrix/Scalar.hpp index 26538f976c..f34551fbd4 100644 --- a/src/lib/matrix/matrix/Scalar.hpp +++ b/src/lib/matrix/matrix/Scalar.hpp @@ -47,6 +47,11 @@ public: return m; } + bool operator==(const float other) const + { + return isEqualF(_value, other); + } + private: const Type _value; diff --git a/src/lib/matrix/test/CMakeLists.txt b/src/lib/matrix/test/CMakeLists.txt index 9767284b9a..3e3789c4c3 100644 --- a/src/lib/matrix/test/CMakeLists.txt +++ b/src/lib/matrix/test/CMakeLists.txt @@ -11,7 +11,6 @@ set(tests slice matrixMult vectorAssignment - matrixAssignment matrixScalarMult transpose vector @@ -43,5 +42,6 @@ foreach(test_name ${tests}) add_dependencies(test_results test-matrix_${test_name}) endforeach() +px4_add_unit_gtest(SRC MatrixAssignmentTest.cpp) px4_add_unit_gtest(SRC MatrixSparseVectorTest.cpp) px4_add_unit_gtest(SRC MatrixUnwrapTest.cpp) diff --git a/src/lib/matrix/test/matrixAssignment.cpp b/src/lib/matrix/test/MatrixAssignmentTest.cpp similarity index 52% rename from src/lib/matrix/test/matrixAssignment.cpp rename to src/lib/matrix/test/MatrixAssignmentTest.cpp index ff8e43304e..2738f9325d 100644 --- a/src/lib/matrix/test/matrixAssignment.cpp +++ b/src/lib/matrix/test/MatrixAssignmentTest.cpp @@ -1,11 +1,42 @@ -#include "test_macros.hpp" +/**************************************************************************** + * + * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include #include using namespace matrix; -template class matrix::Matrix; - -int main() +TEST(MatrixAssignmentTest, Assignment) { Matrix3f m; m.setZero(); @@ -25,7 +56,7 @@ int main() for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { - TEST(fabs(data[i * 3 + j] - m2(i, j)) < FLT_EPSILON); + EXPECT_FLOAT_EQ(data[i * 3 + j], m2(i, j)); } } @@ -34,11 +65,11 @@ int main() for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { - TEST(isnan(m_nan(i, j))); + EXPECT_TRUE(isnan(m_nan(i, j))); } } - TEST(m_nan.isAllNan()); + EXPECT_TRUE(m_nan.isAllNan()); float data2d[3][3] = { {1, 2, 3}, @@ -49,32 +80,32 @@ int main() for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { - TEST(fabs(data[i * 3 + j] - m2(i, j)) < FLT_EPSILON); + EXPECT_FLOAT_EQ(data[i * 3 + j], m2(i, j)); } } - TEST(!m2.isAllNan()); + EXPECT_FALSE(m2.isAllNan()); float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; Matrix3f m3(data_times_2); - TEST(isEqual(m, m2)); - TEST(!(isEqual(m, m3))); + EXPECT_EQ(m, m2); + EXPECT_NE(m, m3); m2 *= 2; - TEST(isEqual(m2, m3)); + EXPECT_EQ(m2, m3); m2 /= 2; m2 -= 1; float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8}; - TEST(isEqual(Matrix3f(data_minus_1), m2)); + EXPECT_EQ(Matrix3f(data_minus_1), m2); m2 += 1; - TEST(isEqual(Matrix3f(data), m2)); + EXPECT_EQ(Matrix3f(data), m2); m3 -= m2; - TEST(isEqual(m3, m2)); + EXPECT_EQ(m3, m2); // set rows and columns to value Matrix3f m2e(data2d); @@ -94,9 +125,9 @@ int main() Matrix3f m2e_check2(data2e_check2); m2e.setCol(1, 11); - TEST(isEqual(m2e, m2e_check1)); + EXPECT_EQ(m2e, m2e_check1); m2e.setRow(2, 0); - TEST(isEqual(m2e, m2e_check2)); + EXPECT_EQ(m2e, m2e_check2); float data_row_02_swap[9] = { 7, 8, 9, @@ -112,53 +143,53 @@ int main() Matrix3f m4(data); - TEST(isEqual(-m4, m4 * (-1))); + EXPECT_EQ(-m4, m4 * (-1)); // col swap m4.swapCols(0, 2); - TEST(isEqual(m4, Matrix3f(data_col_02_swap))); + EXPECT_EQ(m4, Matrix3f(data_col_02_swap)); m4.swapCols(0, 2); // row swap m4.swapRows(0, 2); - TEST(isEqual(m4, Matrix3f(data_row_02_swap))); + EXPECT_EQ(m4, Matrix3f(data_row_02_swap)); m4.swapRows(0, 2); // swapping with same row should do nothing m4.swapRows(0, 0); m4.swapRows(1, 1); m4.swapRows(2, 2); - TEST(isEqual(m4, Matrix3f(data))); + EXPECT_EQ(m4, Matrix3f(data)); // swapping with same col should do nothing m4.swapCols(0, 0); m4.swapCols(1, 1); m4.swapCols(2, 2); - TEST(isEqual(m4, Matrix3f(data))); + EXPECT_EQ(m4, Matrix3f(data)); - TEST(fabs(m4.min() - 1) < FLT_EPSILON); - TEST(fabs((-m4).min() + 9) < FLT_EPSILON); + EXPECT_EQ(m4.min(), 1); + EXPECT_EQ((-m4).min(), -9); Scalar s = 1; const Vector &s_vect = s; - TEST(fabs(s - 1) < FLT_EPSILON); - TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON); + EXPECT_EQ(s, 1.0f); + EXPECT_EQ(s_vect(0), 1.0f); Matrix m5 = s; - TEST(fabs(m5(0, 0) - s) < FLT_EPSILON); + EXPECT_EQ(s, m5(0, 0)); Matrix m6; m6.setRow(0, Vector2f(1, 2)); float m7_array[] = {1, 2, 0, 0}; Matrix m7(m7_array); - TEST(isEqual(m6, m7)); + EXPECT_EQ(m6, m7); m6.setCol(0, Vector2f(3, 4)); float m8_array[] = {3, 2, 4, 0}; Matrix m8(m8_array); - TEST(isEqual(m6, m8)); + EXPECT_EQ(m6, m8); m7.setNaN(); - TEST(m7 != m8); + EXPECT_NE(m7, m8); // min, max, constrain matrix values with scalar float data_m9[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; @@ -171,12 +202,12 @@ int main() Matrix3f m9_lower_bounded(data_m9_lower_bounded); Matrix3f m9_upper_bounded(data_m9_upper_bounded); Matrix3f m9_lower_upper_constrained(data_m9_lower_constrained); - TEST(isEqual(max(m9, lower_bound), m9_lower_bounded)); - TEST(isEqual(max(lower_bound, m9), m9_lower_bounded)); - TEST(isEqual(min(m9, upper_bound), m9_upper_bounded)); - TEST(isEqual(min(upper_bound, m9), m9_upper_bounded)); - TEST(isEqual(constrain(m9, lower_bound, upper_bound), m9_lower_upper_constrained)); - TEST(isEqual(constrain(m9, 8.0f, 7.0f), m_nan)); + EXPECT_EQ(max(m9, lower_bound), m9_lower_bounded); + EXPECT_EQ(max(lower_bound, m9), m9_lower_bounded); + EXPECT_EQ(min(m9, upper_bound), m9_upper_bounded); + EXPECT_EQ(min(upper_bound, m9), m9_upper_bounded); + EXPECT_EQ(constrain(m9, lower_bound, upper_bound), m9_lower_upper_constrained); + EXPECT_EQ(constrain(m9, 8.0f, 7.0f), m_nan); // min, max, constrain matrix values with matrix of same size float data_m10[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; @@ -191,28 +222,28 @@ int main() Matrix3f m10_upper_bound(data_m10_upper_bound); Matrix3f m10_upper_bounded_ref(data_m10_upper_bounded_ref); Matrix3f m10_constrained_ref(data_m10_constrained_ref); - TEST(isEqual(max(m10, m10_lower_bound), m10_lower_bounded_ref)); - TEST(isEqual(max(m10_lower_bound, m10), m10_lower_bounded_ref)); - TEST(isEqual(min(m10, m10_upper_bound), m10_upper_bounded_ref)); - TEST(isEqual(min(m10_upper_bound, m9), m10_upper_bounded_ref)); - TEST(isEqual(constrain(m10, m10_lower_bound, m10_upper_bound), m10_constrained_ref)); + EXPECT_EQ(max(m10, m10_lower_bound), m10_lower_bounded_ref); + EXPECT_EQ(max(m10_lower_bound, m10), m10_lower_bounded_ref); + EXPECT_EQ(min(m10, m10_upper_bound), m10_upper_bounded_ref); + EXPECT_EQ(min(m10_upper_bound, m9), m10_upper_bounded_ref); + EXPECT_EQ(constrain(m10, m10_lower_bound, m10_upper_bound), m10_constrained_ref); // min, max, constrain with NAN - TEST(isEqualF(matrix::typeFunction::min(5.f, NAN), 5.f)); - TEST(isEqualF(matrix::typeFunction::min(NAN, 5.f), 5.f)); - TEST(isEqualF(matrix::typeFunction::min(NAN, NAN), NAN)); - TEST(isEqualF(matrix::typeFunction::max(5.f, NAN), 5.f)); - TEST(isEqualF(matrix::typeFunction::max(NAN, 5.f), 5.f)); - TEST(isEqualF(matrix::typeFunction::max(NAN, NAN), NAN)); - TEST(isEqualF(matrix::typeFunction::constrain(NAN, 5.f, 6.f), NAN)); - TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, 4.f), NAN)); - TEST(isEqualF(matrix::typeFunction::constrain(6.f, NAN, 5.f), 5.f)); - TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, NAN), 5.f)); + EXPECT_TRUE(isEqualF(matrix::typeFunction::min(5.f, NAN), 5.f)); + EXPECT_TRUE(isEqualF(matrix::typeFunction::min(NAN, 5.f), 5.f)); + EXPECT_TRUE(isEqualF(matrix::typeFunction::min(NAN, NAN), NAN)); + EXPECT_TRUE(isEqualF(matrix::typeFunction::max(5.f, NAN), 5.f)); + EXPECT_TRUE(isEqualF(matrix::typeFunction::max(NAN, 5.f), 5.f)); + EXPECT_TRUE(isEqualF(matrix::typeFunction::max(NAN, NAN), NAN)); + EXPECT_TRUE(isEqualF(matrix::typeFunction::constrain(NAN, 5.f, 6.f), NAN)); + EXPECT_TRUE(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, 4.f), NAN)); + EXPECT_TRUE(isEqualF(matrix::typeFunction::constrain(6.f, NAN, 5.f), 5.f)); + EXPECT_TRUE(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, NAN), 5.f)); Vector2f v1{NAN, 5.0f}; Vector2f v1_min = min(v1, 1.f); Matrix3f m11 = min(m10_constrained_ref, NAN); - TEST(isEqualF(fmin(NAN, 1.f), float(v1_min(0)))); - TEST(isEqual(m11, m10_constrained_ref)); + EXPECT_FLOAT_EQ(fmin(NAN, 1.f), float(v1_min(0))); + EXPECT_EQ(m11, m10_constrained_ref); // check write_string() float comma[6] = { @@ -234,7 +265,7 @@ int main() printf("%d: \"%c\" != \"%c\"", int(i), buffer[i], output[i]); // LCOV_EXCL_LINE only print on failure } - TEST(buffer[i] == output[i]); + EXPECT_EQ(buffer[i], output[i]); if (buffer[i] == '\0') { break; @@ -243,14 +274,14 @@ int main() // check print() // Redirect stdout - TEST(freopen("testoutput.txt", "w", stdout) != NULL); + EXPECT_TRUE(freopen("testoutput.txt", "w", stdout) != NULL); // write Comma.print(); fclose(stdout); // read FILE *fp = fopen("testoutput.txt", "r"); - TEST(fp != nullptr); - TEST(!fseek(fp, 0, SEEK_SET)); + EXPECT_NE(fp, nullptr); + EXPECT_FALSE(fseek(fp, 0, SEEK_SET)); for (size_t i = 0; i < len; i++) { char c = static_cast(fgetc(fp)); @@ -260,11 +291,8 @@ int main() } printf("%d %d %d\n", static_cast(i), output[i], c); - TEST(c == output[i]); + EXPECT_EQ(c, output[i]); } - TEST(!fclose(fp)); - - return 0; + EXPECT_FALSE(fclose(fp)); } -