Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: AP_GeodesicGrid: add first implementation

Browse Source 
The search for the geodesic section is still naive in this version.
Additionally, add unit tests.
This commit is contained in:
Gustavo Jose de Sousa 2016-04-11 20:19:45 -03:00 committed by Lucas De Marchi
parent 3f2ad764ca
commit b299261ad4
4 changed files with 610 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

278
libraries/AP_Math/AP_GeodesicGrid.cpp Normal file
View File

@ -0,0 +1,278 @@
/*
* 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/>.
*/
/*
* This comment section explains the basic idea behind the implementation.
*
* Vectors difference notation
* ===========================
* Let v and w be vectors. For readability purposes, unless explicitly
* otherwise noted, the notation vw will be used to represent w - v.
*
* Relationship between a vector and a triangle in 3d space
* ========================================================
* Vector in the area of a triangle
* --------------------------------
* Let T = (a, b, c) be a triangle, where a, b and c are also vectors and
* linearly independent. A vector inside that triangle can be written as one of
* its vertices plus the sum of the positively scaled vectors from that vertex
* to the other ones. Taking a as the first vertex, a vector p in the area
* formed by T can be written as:
*
* p = a + w_ab * ab + w_ac * ac
*
* It's fairly easy to see that if p is in the area formed by T, then w_ab >= 0
* and w_ac >= 0. That vector p can also be written as:
*
* p = b + w_ba * ba + w_bc * bc
*
* It's easy to check that the triangle formed by (a + w_ab * ab, b + w_ba *
* ba, p) is similar to T and, with the correct algebraic manipulations, we can
* come to the conclusion that:
*
* w_ba = 1 - w_ab - w_ac
*
* Since we know that w_ba >= 0, then w_ab + w_ac <= 1. Thus:
*
* ----------------------------------------------------------
* | p = a + w_ab * ab + w_ac * ac is in the area of T iff: |
* | w_ab >= 0 and w_ac >= 0 and w_ab + w_ac <= 1 |
* ----------------------------------------------------------
*
* Proving backwards shouldn't be difficult.
*
* Vector p can also be written as:
*
* p = (1 - w_ab - w_ba) * a + w_ab * b + w_ba * c
*
*
* Vector that crosses a triangle
* ------------------------------
* Let T be the same triangle discussed above and let v be a vector such that:
*
* v = x * a + y * b + z * c
* where x >= 0, y >= 0, z >= 0, and x + y + z > 0.
*
* It's geometrically easy to see that v crosses the triangle T. But that can
* also be verified analytically.
*
* The vector v crosses the triangle T iff there's a positive alpha such that
* alpha * v is in the area formed by T, so we need to prove that such value
* exists. To find alpha, we solve the equation alpha * v = p, which will lead
* us to the system, for the variables alpha, w_ab and w_ac:
*
* alpha * x = 1 - w_ab - w_ac
* alpha * y = w_ab
* alpha * z = w_ac,
* where w_ab >= 0 and w_ac >= 0 and w_ab + w_ac <= 1
*
* That will lead to alpha = 1 / (x + y + z), w_ab = y / (x + y + b) and
* w_ac = z / (x + y + z) and the following holds:
* - alpha does exist because x + y + z > 0.
* - w_ab >= 0 and w_ac >= 0 because y >= 0 and z >= 0 and x + y + z > 0.
* - 0 <= 1 - w_ab - w_ac <= 1 because 0 <= (y + z) / (x + y + z) <= 1.
*
* Thus:
*
* ----------------------------------------------------------
* | v = x * a + y * b + z * c crosses T = (a, b, c), where |
* | a, b and c are linearly independent, iff: |
* | x >= 0, y >= 0, z >= 0 and x + y + z > 0 |
* ----------------------------------------------------------
*
* Moreover:
* - if one of the coefficients is zero, then v crosses the edge formed by the
* vertices multiplied by the non-zero coefficients.
* - if two of the coefficients are zero, then v crosses the vertex multiplied
* by the non-zero coefficient.
*/
#include "AP_GeodesicGrid.h"
AP_GeodesicGrid::AP_GeodesicGrid()
: _triangles{
{{-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}},
{{-1, 0, M_GOLDEN}, { 0,-M_GOLDEN, 1}, { 1, 0, M_GOLDEN}},
{{ 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}},
{{ 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}, {-M_GOLDEN, 1, 0}, {-1, 0, M_GOLDEN}}
}
{
_init_mid_triangles();
_init_all_inverses();
}
int AP_GeodesicGrid::section(const Vector3f& v,
const bool inclusive) const
{
int i = _triangle_index(v, inclusive);
if (i < 0) {
return -1;
}
int j = _subtriangle_index(i, v, inclusive);
if (j < 0) {
return -1;
}
return 4 * i + j;
}
bool AP_GeodesicGrid::section_triangle(unsigned int section_index,
Vector3f& a,
Vector3f& b,
Vector3f& c) const
{
if (section_index >= 20 * NUM_SUBTRIANGLES) {
return false;
}
unsigned int i = section_index / NUM_SUBTRIANGLES;
unsigned int j = section_index % NUM_SUBTRIANGLES;
auto& t = _triangles[i];
auto& mt = _mid_triangles[i];
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;
}
void AP_GeodesicGrid::_init_mid_triangles()
{
for (int i = 0; i < 20; i++) {
_mid_triangles[i][0] = (_triangles[i][0] + _triangles[i][1]) / 2;
_mid_triangles[i][1] = (_triangles[i][1] + _triangles[i][2]) / 2;
_mid_triangles[i][2] = (_triangles[i][2] + _triangles[i][0]) / 2;
}
}
void AP_GeodesicGrid::_init_all_inverses()
{
for (int i = 0; i < 20; i++) {
auto& t = _triangles[i];
_inverses[i]({t[0].x, t[1].x, t[2].x},
{t[0].y, t[1].y, t[2].y},
{t[0].z, t[1].z, t[2].z});
_inverses[i].invert();
auto& mt = _mid_triangles[i];
_mid_inverses[i]({mt[0].x, mt[1].x, mt[2].x},
{mt[0].y, mt[1].y, mt[2].y},
{mt[0].z, mt[1].z, mt[2].z});
_mid_inverses[i].invert();
}
}
int AP_GeodesicGrid::_triangle_index(const Vector3f& v,
const bool inclusive) const
{
for (int i = 0; i < 20; i++) {
/* w holds the coordinates of v with respect to the basis comprised by
* the vectors of _triangles[i] */
auto w = _inverses[i] * v;
if (!is_zero(w.x) && w.x < 0) {
continue;
}
if (!is_zero(w.y) && w.y < 0) {
continue;
}
if (!is_zero(w.z) && w.z < 0) {
continue;
}
if ((is_zero(w.x) || is_zero(w.y) || is_zero(w.z)) && !inclusive) {
return -1;
}
return i;
}
return -1;
}
int AP_GeodesicGrid::_subtriangle_index(const unsigned int triangle_index,
const Vector3f& v,
const bool inclusive) const
{
/* w holds the coordinates of v with respect to the basis comprised by the
* vectors of _mid_triangles[triangle_index] */
auto w = _mid_inverses[triangle_index] * v;
if ((is_zero(w.x) || is_zero(w.y) || is_zero(w.z)) && !inclusive) {
return -1;
}
/* At this point, we know that v crosses the icosahedron triangle pointed
* by triangle_index. Thus, we can geometrically see that if v doesn't
* cross its middle triangle, then one of the coefficients will be negative
* and the other ones positive. Let a and b be the non-negative
* coefficients and c the negative one. In that case, v will cross the
* triangle with vertices (a, b, -c). Since we know that v crosses the
* icosahedron triangle and the only sub-triangle that contains the set of
* points (seen as vectors) that cross the triangle (a, b, -c) is the
* middle triangle's neighbor with respect to a and b, then that
* sub-triangle is the one crossed by v. */
if (!is_zero(w.x) && w.x < 0) {
return 3;
}
if (!is_zero(w.y) && w.y < 0) {
return 1;
}
if (!is_zero(w.z) && w.z < 0) {
return 2;
}
/* If x >= 0 and y >= 0 and z >= 0, then v crosses the middle triangle. */
return 0;
}

216
libraries/AP_Math/AP_GeodesicGrid.h Normal file
View File

@ -0,0 +1,216 @@
/*
* 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 "AP_Math.h"
/**
* AP_GeodesicGrid is a class for working on geodesic sections.
*
* For quick information regarding geodesic grids, see:
* https://en.wikipedia.org/wiki/Geodesic_grid
*
* The grid is formed by a tessellation of an icosahedron by a factor of 2,
* i.e., each triangular face of the icosahedron is divided into 4 by splitting
* each edge into 2 line segments and projecting the vertices to the
* icosahedron's circumscribed sphere. That will give a total of 80 triangular
* faces, which are called sections in this context.
*
* A section index is given by the icosahedron's triangle it belongs to and by
* its index in that triangle. Let i in [0,20) be the icosahedron's triangle
* index and j in [0,4) be the sub-triangle's (which is the section) index
* inside the greater triangle. Then the section index is given by
* s = 4 * i + j .
*
* The icosahedron's triangles are defined by the tuple (T_0, T_1, ..., T_19),
* where T_i is the i-th triangle. Each triangle is represented with a tuple of
* the form (a, b, c), where a, b and c are the triangle vertices in the space.
*
* Given the definitions above and the golden ration as g, the triangles must
* be defined in the following order:
*
* (
* ((-g, 1, 0), (-1, 0,-g), (-g,-1, 0)),
* ((-1, 0,-g), (-g,-1, 0), ( 0,-g,-1)),
* ((-g,-1, 0), ( 0,-g,-1), ( 0,-g, 1)),
* ((-1, 0,-g), ( 0,-g,-1), ( 1, 0,-g)),
* (( 0,-g,-1), ( 0,-g, 1), ( g,-1, 0)),
* (( 0,-g,-1), ( 1, 0,-g), ( g,-1, 0)),
* (( g,-1, 0), ( 1, 0,-g), ( g, 1, 0)),
* (( 1, 0,-g), ( g, 1, 0), ( 0, g,-1)),
* (( 1, 0,-g), ( 0, g,-1), (-1, 0,-g)),
* (( 0, g,-1), (-g, 1, 0), (-1, 0,-g)),
* ((-g, 1, 0), (-1, 0, g), (-g,-1, 0)),
* ((-1, 0, g), (-g,-1, 0), ( 0,-g, 1)),
* ((-1, 0, g), ( 0,-g, 1), ( 1, 0, g)),
* (( 0,-g, 1), ( 1, 0, g), ( g,-1, 0)),
* (( g,-1, 0), ( 1, 0, g), ( g, 1, 0)),
* (( 1, 0, g), ( g, 1, 0), ( 0, g, 1)),
* (( g, 1, 0), ( 0, g, 1), ( 0, g,-1)),
* (( 1, 0, g), ( 0, g, 1), (-1, 0, g)),
* (( 0, g, 1), ( 0, g,-1), (-g, 1, 0)),
* (( 0, g, 1), (-g, 1, 0), (-1, 0, g))
* )
*
* Let an icosahedron triangle T be defined as T = (a, b, c). The "middle
* triangle" M is defined as the triangle formed by the points that bisect the
* edges of T. M is defined by:
*
* M = (m_a, m_b, m_c) = ((a + b) / 2, (b + c) / 2, (c + a) / 2)
*
* Let elements of the tuple (W_0, W_1, W_2, W_3) comprise the sub-triangles of
* T, so that W_j is the j-th sub-triangle of T. The sub-triangles are defined
* as the following:
*
* W_0 = M
* W_1 = (a, m_a, m_c)
* W_2 = (m_a, b, m_b)
* W_3 = (m_c, m_b, c)
*/
class AP_GeodesicGrid {
public:
/*
* The following concepts are used by the description of this class'
* members.
*
* Vector crossing objects
* -----------------------
* We say that a vector v crosses an object in space (point, line, line
* segment, plane etc) iff the line, being Q the set of points of that
* object, the vector v crosses it iff there exists a positive scalar alpha
* such that alpha * v is in Q.
*/
/**
* Number of sub-triangles for an icosahedron triangle.
*/
static const int NUM_SUBTRIANGLES = 4;
AP_GeodesicGrid();
/**
* Find which section is crossed by \p v.
*
* @param v[in] The vector to be verified.
*
* @param inclusive[in] If true, then if \p v crosses one of the edges of
* one of the sections, then that section is returned. If \p inclusive is
* false, then \p v is considered to cross no section. Note that, if \p
* inclusive is true, then \p v can belong to more than one section and
* only the first one found is returned. The order in which the triangles
* are checked is unspecified. The default value for \p inclusive is
* false.
*
* @return The index of the section. The value -1 is returned if the
* section isn't found, which might happen when \p inclusive is false.
*/
int section(const Vector3f& v, const bool inclusive = false) const;
/**
* Get the triangle that defines the section at index \p section_index.
*
* @param section_index[in] The section index.
*
* @param a[out] The triangle's first vertex.
* @param b[out] The triangle's second vertex.
* @param c[out] The triangle's third vertex.
*
* @return If \p section_index is valid, true is returned and the triangle
* vertices are assigned to \p a, \p b and \p c, in that order. Otherwise,
* false is returned and the values of the vertices parameters are left
* unmodified.
*/
bool section_triangle(unsigned int section_index,
Vector3f& a,
Vector3f& b,
Vector3f& c) const;
private:
/**
* The icosahedron's triangles. The item `_triangles[i]` represents T_i.
*/
const Vector3f _triangles[20][3];
/**
* The inverses of the change-of-basis matrices for the icosahedron
* triangles.
*
* The i-th matrix is the inverse of the change-of-basis matrix from
* natural basis to the basis formed by T_i's vectors.
*/
Matrix3f _inverses[20];
/**
* The middle triangles. The item `_mid_triangles[i]` represents the middle
* triangle of T_i.
*/
Vector3f _mid_triangles[20][3];
/**
* The inverses of the change-of-basis matrices for the middle triangles.
*
* The i-th matrix is the inverse of the change-of-basis matrix from
* natural basis to the basis formed by T_i's middle triangle's vectors.
*/
Matrix3f _mid_inverses[20];
/**
* Initialize the vertices of the middle triangles as specified by
* #_mid_triangles.
*/
void _init_mid_triangles();
/**
* Initialize the matrices in #_inverses and #_mid_inverses.
*/
void _init_all_inverses();
/**
* Find which icosahedron's triangle is crossed by \p v.
*
* @param v[in] The vector to be verified.
*
* @param inclusive[in] This parameter follow the same rules defined in
* #section() const.
*
* @return The index of the triangle. The value -1 is returned if the
* triangle isn't found, which might happen when \p inclusive is false.
*/
int _triangle_index(const Vector3f& v, const bool inclusive) const;
/**
* Find which sub-triangle of the icosahedron's triangle pointed by \p
* triangle_index is crossed by \p v.
*
* The vector \p v must belong to the super-section formed by the triangle
* pointed by \p triangle_index, otherwise the result is undefined.
*
* @param triangle_index[in] The icosahedron's triangle index, it must be in
* the interval [0,20). Passing invalid values is undefined behavior.
*
* @param v[in] The vector to be verified.
*
* @param inclusive[in] This parameter follow the same rules defined in
* #section() const.
*
* @return The index of the sub-triangle. The value -1 is returned if the
* triangle isn't found, which might happen when \p inclusive is false.
*/
int _subtriangle_index(const unsigned int triangle_index,
const Vector3f& v,
const bool inclusive) const;
};

10
libraries/AP_Math/tests/math_test.h
View File

@ -22,8 +22,10 @@
#include <AP_Math/AP_Math.h>
/*
* Overload operator << to allow nice printing for failed tests.
* Overload operator << for AP_Math types to allow nice printing for failed
* tests.
*/
template <typename T>
::std::ostream& operator<<(::std::ostream& os, const Matrix3<T>& m)
{
@ -31,3 +33,9 @@ template <typename T>
"{" << m.b.x << ", " << m.b.y << ", " << m.b.z << "}, " <<
"{" << m.c.x << ", " << m.c.y << ", " << m.c.z << "}}";
}
template <typename T>
::std::ostream& operator<<(::std::ostream& os, const Vector3<T>& v)
{
return os << "{" << v.x << ", " << v.y << ", " << v.z << "}";
}

107
libraries/AP_Math/tests/test_geodesic_grid.cpp Normal file
View File

@ -0,0 +1,107 @@
/*
* 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 <vector>
#include "math_test.h"
#include <AP_Math/AP_GeodesicGrid.h>
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;
};
class GeodesicGridTest : public ::testing::TestWithParam<TestParam> {
protected:
static AP_GeodesicGrid grid;
};
AP_GeodesicGrid GeodesicGridTest::grid;
AP_GTEST_PRINTATBLE_PARAM_MEMBER(TestParam, v);
TEST_P(GeodesicGridTest, Sections)
{
auto p = GetParam();
EXPECT_EQ(p.section, grid.section(p.v));
}
static TestParam icosahedron_vertices[] = {
{{ M_GOLDEN, 1.0f, 0.0f}, -1},
{{ M_GOLDEN, -1.0f, 0.0f}, -1},
{{-M_GOLDEN, 1.0f, 0.0f}, -1},
{{-M_GOLDEN, -1.0f, 0.0f}, -1},
{{ 1.0f, 0.0f, M_GOLDEN}, -1},
{{-1.0f, 0.0f, M_GOLDEN}, -1},
{{ 1.0f, 0.0f, -M_GOLDEN}, -1},
{{-1.0f, 0.0f, -M_GOLDEN}, -1},
{{0.0f, M_GOLDEN, 1.0f}, -1},
{{0.0f, M_GOLDEN, -1.0f}, -1},
{{0.0f, -M_GOLDEN, 1.0f}, -1},
{{0.0f, -M_GOLDEN, -1.0f}, -1},
};
INSTANTIATE_TEST_CASE_P(IcosahedronVertices,
GeodesicGridTest,
::testing::ValuesIn(icosahedron_vertices));
/* Generate vectors that pass through the centroid of each section's triangle. */
static std::vector<TestParam> centroid_vectors = []()
{
std::vector<TestParam> params;
AP_GeodesicGrid grid;
for (int i = 0; i < 20 * AP_GeodesicGrid::NUM_SUBTRIANGLES; i++) {
Vector3f a, b, c;
TestParam p;
grid.section_triangle(i, a, b, c);
p.section = i;
p.v = (a + b + c) / 3;
params.push_back(p);
}
return params;
}();
INSTANTIATE_TEST_CASE_P(MidVectors,
GeodesicGridTest,
::testing::ValuesIn(centroid_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 */
{{.25f * M_GOLDEN, -.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},
/* 2 * m_c + (1 / 3) * m_b + .1 * c for T_17 */
{{-.2667f, .1667f * M_GOLDEN, 2.2667f * M_GOLDEN + .1667f}, 71},
/* .25 * m_a + 5 * b + 2 * m_b for T_8 */
{{-.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()