From 62571b698403e610038cb762f7fb70f2f0d0f714 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 23 Feb 2022 14:47:41 +0100 Subject: [PATCH] Matrix: convert copy to test to gtest --- src/lib/matrix/test/CMakeLists.txt | 2 +- src/lib/matrix/test/MatrixCopyToTest.cpp | 98 ++++++++++++++++++++++++ src/lib/matrix/test/copyto.cpp | 78 ------------------- 3 files changed, 99 insertions(+), 79 deletions(-) create mode 100644 src/lib/matrix/test/MatrixCopyToTest.cpp delete mode 100644 src/lib/matrix/test/copyto.cpp diff --git a/src/lib/matrix/test/CMakeLists.txt b/src/lib/matrix/test/CMakeLists.txt index d51f3dc75d..a34928fb0b 100644 --- a/src/lib/matrix/test/CMakeLists.txt +++ b/src/lib/matrix/test/CMakeLists.txt @@ -21,7 +21,6 @@ set(tests squareMatrix helper hatvee - copyto least_squares upperRightTriangle dual @@ -43,5 +42,6 @@ endforeach() px4_add_unit_gtest(SRC MatrixAssignmentTest.cpp) px4_add_unit_gtest(SRC MatrixAttitudeTest.cpp) +px4_add_unit_gtest(SRC MatrixCopyToTest.cpp) px4_add_unit_gtest(SRC MatrixSparseVectorTest.cpp) px4_add_unit_gtest(SRC MatrixUnwrapTest.cpp) diff --git a/src/lib/matrix/test/MatrixCopyToTest.cpp b/src/lib/matrix/test/MatrixCopyToTest.cpp new file mode 100644 index 0000000000..4c462df0e4 --- /dev/null +++ b/src/lib/matrix/test/MatrixCopyToTest.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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; + +TEST(MatrixCopyToTest, CopyTo) +{ + // Vector3 copyTo + const Vector3f v(1, 2, 3); + float dst3[3] = {}; + v.copyTo(dst3); + + for (size_t i = 0; i < 3; i++) { + EXPECT_FLOAT_EQ(v(i), dst3[i]); + } + + // Quaternion copyTo + Quatf q(1, 2, 3, 4); + float dst4[4] = {}; + q.copyTo(dst4); + + for (size_t i = 0; i < 4; i++) { + EXPECT_FLOAT_EQ(q(i), dst4[i]); + } + + // Matrix copyTo + Matrix A; + A(0, 0) = 1; + A(0, 1) = 2; + A(0, 2) = 3; + A(1, 0) = 4; + A(1, 1) = 5; + A(1, 2) = 6; + float array_A[6] = {}; + A.copyTo(array_A); + float array_row[6] = {1, 2, 3, 4, 5, 6}; + + for (size_t i = 0; i < 6; i++) { + EXPECT_FLOAT_EQ(array_A[i], array_row[i]); + } + + // Matrix copyToColumnMajor + A.copyToColumnMajor(array_A); + float array_column[6] = {1, 4, 2, 5, 3, 6}; + + for (size_t i = 0; i < 6; i++) { + EXPECT_FLOAT_EQ(array_A[i], array_column[i]); + } + + // Slice copyTo + float dst5[2] = {}; + v.slice<2, 1>(0, 0).copyTo(dst5); + + for (size_t i = 0; i < 2; i++) { + EXPECT_FLOAT_EQ(v(i), dst5[i]); + } + + float subarray_A[4] = {}; + A.slice<2, 2>(0, 0).copyToColumnMajor(subarray_A); + float subarray_column[4] = {1, 4, 2, 5}; + + for (size_t i = 0; i < 4; i++) { + EXPECT_FLOAT_EQ(subarray_A[i], subarray_column[i]); + } +} diff --git a/src/lib/matrix/test/copyto.cpp b/src/lib/matrix/test/copyto.cpp deleted file mode 100644 index c8fae3620e..0000000000 --- a/src/lib/matrix/test/copyto.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#include "test_macros.hpp" -#include - -using namespace matrix; - -namespace -{ -void doTheCopy(const Matrix &A, float array_A[6]) -{ - A.copyTo(array_A); -} -} - -int main() -{ - float eps = 1e-6f; - - // Vector3 copyTo - const Vector3f v(1, 2, 3); - float dst3[3] = {}; - v.copyTo(dst3); - - for (size_t i = 0; i < 3; i++) { - TEST(fabs(v(i) - dst3[i]) < eps); - } - - // Quaternion copyTo - Quatf q(1, 2, 3, 4); - float dst4[4] = {}; - q.copyTo(dst4); - - for (size_t i = 0; i < 4; i++) { - TEST(fabs(q(i) - dst4[i]) < eps); - } - - // Matrix copyTo - Matrix A; - A(0, 0) = 1; - A(0, 1) = 2; - A(0, 2) = 3; - A(1, 0) = 4; - A(1, 1) = 5; - A(1, 2) = 6; - float array_A[6] = {}; - doTheCopy(A, array_A); - float array_row[6] = {1, 2, 3, 4, 5, 6}; - - for (size_t i = 0; i < 6; i++) { - TEST(fabs(array_A[i] - array_row[i]) < eps); - } - - // Matrix copyToColumnMajor - A.copyToColumnMajor(array_A); - float array_column[6] = {1, 4, 2, 5, 3, 6}; - - for (size_t i = 0; i < 6; i++) { - TEST(fabs(array_A[i] - array_column[i]) < eps); - } - - // Slice copyTo - float dst5[2] = {}; - v.slice<2, 1>(0, 0).copyTo(dst5); - - for (size_t i = 0; i < 2; i++) { - TEST(fabs(v(i) - dst5[i]) < eps); - } - - float subarray_A[4] = {}; - A.slice<2, 2>(0, 0).copyToColumnMajor(subarray_A); - float subarray_column[4] = {1, 4, 2, 5}; - - for (size_t i = 0; i < 4; i++) { - TEST(fabs(subarray_A[i] - subarray_column[i]) < eps); - } - - return 0; -} -