/
Sangak
/
ardupilot
Обзор
Документация
Войти
/
Sangak
/
ardupilot
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
Аналитика
Безопасность
master
libraries/AP_Math/tests/test_geodesic_grid.cpp
237 строк
8 KB
muramura
AP_Math: Clarify the floating-point value
05 май 2025, 14:49
05 май 2025, 14:49
4dd2fb2
Код
Авторство
О чём код?
/* * Copyright (C) 2015-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 <cassert> #include <vector> #include "math_test.h" #include <AP_Math/AP_GeodesicGrid.h> const AP_HAL::HAL& hal = AP_HAL::get_HAL(); class TestParam { public: /** * Vector to be tested. */ Vector3f v; /** * Expected section if when AP_GeodesicGrid::section() is called with * inclusive set as false. */ int section; /** * Array terminated with -1. This doesn't have to be touched if #section * isn't negative. If #section is -1, then calling * AP_GeodesicGrid::section() with inclusive set as true expects a return * value as one of the values in #inclusive_sections. */ int inclusive_sections[7]; }; class GeodesicGridTest : public ::testing::TestWithParam<TestParam> { protected: /** * Test the functions for triangles indexes. * * @param p[in] The test parameter. */ void test_triangles_indexes(const TestParam &p) { if (p.section >= 0) { int expected_triangle = p.section / AP_GeodesicGrid::NUM_SUBTRIANGLES; int triangle = AP_GeodesicGrid::_triangle_index(p.v, false); ASSERT_EQ(expected_triangle, triangle); int expected_subtriangle = p.section % AP_GeodesicGrid::NUM_SUBTRIANGLES; int subtriangle = AP_GeodesicGrid::_subtriangle_index(triangle, p.v, false); ASSERT_EQ(expected_subtriangle, subtriangle); } else { int triangle = AP_GeodesicGrid::_triangle_index(p.v, false); if (triangle >= 0) { int subtriangle = AP_GeodesicGrid::_subtriangle_index(triangle, p.v, false); ASSERT_EQ(-1, subtriangle) << "triangle is " << triangle; } } } }; static const Vector3f triangles[20][3] = { {{-M_GOLDEN, 1, 0}, {-1, 0,-M_GOLDEN}, {-M_GOLDEN,-1, 0}}, {{-1, 0,-M_GOLDEN}, {-M_GOLDEN,-1, 0}, { 0,-M_GOLDEN,-1}}, {{-M_GOLDEN,-1, 0}, { 0,-M_GOLDEN,-1}, { 0,-M_GOLDEN, 1}}, {{-1, 0,-M_GOLDEN}, { 0,-M_GOLDEN,-1}, { 1, 0,-M_GOLDEN}}, {{ 0,-M_GOLDEN,-1}, { 0,-M_GOLDEN, 1}, { M_GOLDEN,-1, 0}}, {{ 0,-M_GOLDEN,-1}, { 1, 0,-M_GOLDEN}, { M_GOLDEN,-1, 0}}, {{ M_GOLDEN,-1, 0}, { 1, 0,-M_GOLDEN}, { M_GOLDEN, 1, 0}}, {{ 1, 0,-M_GOLDEN}, { M_GOLDEN, 1, 0}, { 0, M_GOLDEN,-1}}, {{ 1, 0,-M_GOLDEN}, { 0, M_GOLDEN,-1}, {-1, 0,-M_GOLDEN}}, {{ 0, M_GOLDEN,-1}, {-M_GOLDEN, 1, 0}, {-1, 0,-M_GOLDEN}}, {{ M_GOLDEN,-1, 0}, { 1, 0, M_GOLDEN}, { M_GOLDEN, 1, 0}}, {{ 1, 0, M_GOLDEN}, { M_GOLDEN, 1, 0}, { 0, M_GOLDEN, 1}}, {{ M_GOLDEN, 1, 0}, { 0, M_GOLDEN, 1}, { 0, M_GOLDEN,-1}}, {{ 1, 0, M_GOLDEN}, { 0, M_GOLDEN, 1}, {-1, 0, M_GOLDEN}}, {{ 0, M_GOLDEN, 1}, { 0, M_GOLDEN,-1}, {-M_GOLDEN, 1, 0}}, {{ 0, M_GOLDEN, 1}, {-1, 0, M_GOLDEN}, {-M_GOLDEN, 1, 0}}, {{-M_GOLDEN, 1, 0}, {-1, 0, M_GOLDEN}, {-M_GOLDEN,-1, 0}}, {{-1, 0, M_GOLDEN}, {-M_GOLDEN,-1, 0}, { 0,-M_GOLDEN, 1}}, {{-1, 0, M_GOLDEN}, { 0,-M_GOLDEN, 1}, { 1, 0, M_GOLDEN}}, {{ 0,-M_GOLDEN, 1}, { M_GOLDEN,-1, 0}, { 1, 0, M_GOLDEN}}, }; static bool section_triangle(unsigned int section_index, Vector3f &a, Vector3f &b, Vector3f &c) { if (section_index >= 80) { return false; // LCOV_EXCL_LINE } unsigned int i = section_index / 4; unsigned int j = section_index % 4; auto &t = triangles[i]; Vector3f mt[3]{(t[0] + t[1]) / 2, (t[1] + t[2]) / 2, (t[2] + t[0]) / 2}; switch (j) { case 0: a = mt[0]; b = mt[1]; c = mt[2]; break; case 1: a = t[0]; b = mt[0]; c = mt[2]; break; case 2: a = mt[0]; b = t[1]; c = mt[1]; break; case 3: a = mt[2]; b = mt[1]; c = t[2]; break; } return true; } AP_GTEST_PRINTATBLE_PARAM_MEMBER(TestParam, v); TEST_P(GeodesicGridTest, Sections) { auto p = GetParam(); test_triangles_indexes(p); EXPECT_EQ(p.section, AP_GeodesicGrid::section(p.v)); if (p.section < 0) { int s = AP_GeodesicGrid::section(p.v, true); int i; for (i = 0; p.inclusive_sections[i] > 0; i++) { assert(i < 7); if (s == p.inclusive_sections[i]) { break; } } if (p.inclusive_sections[i] < 0) { ADD_FAILURE() << "section " << s << " with inclusive=true not found in inclusive_sections"; // LCOV_EXCL_LINE } } } static TestParam icosahedron_vertices[] = { {{ M_GOLDEN, 1.0f, 0.0f}, -1, {27, 30, 43, 46, 49, -1}}, {{ M_GOLDEN, -1.0f, 0.0f}, -1, {19, 23, 25, 41, 78, -1}}, {{-M_GOLDEN, 1.0f, 0.0f}, -1, { 1, 38, 59, 63, 65, -1}}, {{-M_GOLDEN, -1.0f, 0.0f}, -1, { 3, 6, 9, 67, 70, -1}}, {{ 1.0f, 0.0f, M_GOLDEN}, -1, {42, 45, 53, 75, 79, -1}}, {{-1.0f, 0.0f, M_GOLDEN}, -1, {55, 62, 66, 69, 73, -1}}, {{ 1.0f, 0.0f, -M_GOLDEN}, -1, {15, 22, 26, 29, 33, -1}}, {{-1.0f, 0.0f, -M_GOLDEN}, -1, { 2, 5, 13, 35, 39, -1}}, {{0.0f, M_GOLDEN, 1.0f}, -1, {47, 50, 54, 57, 61, -1}}, {{0.0f, M_GOLDEN, -1.0f}, -1, {31, 34, 37, 51, 58, -1}}, {{0.0f, -M_GOLDEN, 1.0f}, -1, {11, 18, 71, 74, 77, -1}}, {{0.0f, -M_GOLDEN, -1.0f}, -1, { 7, 10, 14, 17, 21, -1}}, }; INSTANTIATE_TEST_CASE_P(IcosahedronVertices, GeodesicGridTest, ::testing::ValuesIn(icosahedron_vertices)); /* Generate vectors for each triangle */ static std::vector<TestParam> general_vectors = []() { std::vector<TestParam> params; for (int i = 0; i < 20 * AP_GeodesicGrid::NUM_SUBTRIANGLES; i++) { Vector3f a, b, c; TestParam p; section_triangle(i, a, b, c); p.section = i; /* Vector that crosses the centroid */ p.v = a + b + c; params.push_back(p); /* Vectors that cross the triangle close to the edges */ p.v = a + b + c * 0.001f; params.push_back(p); p.v = a + b * 0.001f + c; params.push_back(p); p.v = a * 0.001f + b + c; params.push_back(p); /* Vectors that cross the triangle close to the vertices */ p.v = a + b * 0.001 + c * 0.001f; params.push_back(p); p.v = a * 0.001f + b + c * 0.001f; params.push_back(p); p.v = a * 0.001f + b * 0.001f + c; params.push_back(p); } return params; }(); INSTANTIATE_TEST_CASE_P(GeneralVectors, GeodesicGridTest, ::testing::ValuesIn(general_vectors)); /* Other hardcoded vectors, so we don't rely just on the centroid vectors * (which are dependent on how the triangles are *defined by the * implementation*) * * See AP_GeodesicGrid.h for the notation on the comments below. */ static TestParam hardcoded_vectors[] = { /* a + 2 * m_a + .5 * m_c for T_4 */ {{0.25f * M_GOLDEN, -0.25f * (13.0f * M_GOLDEN + 1.0f), - 1.25f}, 17}, /* 3 * m_a + 2 * m_b 0 * m_c for T_4 */ {{M_GOLDEN, -4.0f * M_GOLDEN -1.0f, 1.0f}, -1, {16, 18, -1}}, /* 2 * m_c + (1 / 3) * m_b + .1 * c for T_13 */ {{-0.2667f, 0.1667f * M_GOLDEN, 2.2667f * M_GOLDEN + 0.1667f}, 55}, /* .25 * m_a + 5 * b + 2 * m_b for T_8 */ {{-0.875f, 6.125f * M_GOLDEN, -1.125f * M_GOLDEN - 6.125f}, 34}, }; INSTANTIATE_TEST_CASE_P(HardcodedVectors, GeodesicGridTest, ::testing::ValuesIn(hardcoded_vectors)); AP_GTEST_MAIN()