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

AP_Math: AP_GeodesicGrid: reduce number of _neighbor_umbrellas items

Browse Source 
Only the first half is necessary. The values for the other half can be derived.
This commit is contained in:
Gustavo Jose de Sousa 2016-04-20 18:03:34 -03:00 committed by Lucas De Marchi
parent 43d9e00ab7
commit fd2428db34
2 changed files with 69 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
libraries/AP_Math/AP_GeodesicGrid.cpp
View File

@ -110,9 +110,6 @@ AP_GeodesicGrid::AP_GeodesicGrid()
{{ 9, 8, 7, 12, 14}, 1, 2, 0, 0, 2},
{{ 1, 2, 4, 5, 3}, 0, 0, 2, 2, 0},
{{16, 15, 13, 18, 17}, 2, 2, 0, 2, 1},
{{19, 18, 17, 2, 4}, 1, 2, 0, 0, 2},
{{11, 12, 14, 15, 13}, 0, 0, 2, 2, 0},
{{ 6, 5, 3, 8, 7}, 2, 2, 0, 2, 1},
}
, _inverses{
{{-0.309017f, 0.500000f, 0.190983f},
@ -197,13 +194,20 @@ int AP_GeodesicGrid::section(const Vector3f& v,
return 4 * i + j;
}
int AP_GeodesicGrid::_neighbor_umbrella_component(int idx,
int comp_idx) const
{
if (idx < 3) {
return _neighbor_umbrellas[idx].components[comp_idx];
}
return (_neighbor_umbrellas[idx % 3].components[comp_idx] + 10) % 20;
}
int AP_GeodesicGrid::_from_neighbor_umbrella(int idx,
const Vector3f& v,
const Vector3f& u,
bool inclusive) const
{
const struct neighbor_umbrella& umbrella = _neighbor_umbrellas[idx];
/* The following comparisons between the umbrella's first and second
* vertices' coefficients work for this algorithm because all vertices'
* vectors are of the same length. */
@ -212,84 +216,87 @@ int AP_GeodesicGrid::_from_neighbor_umbrella(int idx,
/* If the coefficients of the first and second vertices are equal, then
* v crosses the first component or the edge formed by the umbrella's
* pivot and forth vertex. */
auto w = _inverses[umbrella.components[0] % 10] * v;
if (umbrella.components[0] > 9) {
int comp = _neighbor_umbrella_component(idx, 0);
auto w = _inverses[comp % 10] * v;
if (comp > 9) {
w = -w;
}
float x0 = w[umbrella.v0_c0];
float x0 = w[_neighbor_umbrellas[idx % 3].v0_c0];
if (is_zero(x0)) {
if (!inclusive) {
return -1;
}
return umbrella.components[0];
return comp;
} else if (x0 < 0) {
if (!inclusive) {
return -1;
}
return umbrella.components[u.x < u.y ? 3 : 2];
return _neighbor_umbrella_component(idx, u.x < u.y ? 3 : 2);
}
return umbrella.components[0];
return comp;
}
if (u.y > u.x) {
/* If the coefficient of the second vertex is greater than the first
* one's, then v crosses the first, second or third component. */
auto w = _inverses[umbrella.components[1] % 10] * v;
if (umbrella.components[1] > 9) {
int comp = _neighbor_umbrella_component(idx, 1);
auto w = _inverses[comp % 10] * v;
if (comp > 9) {
w = -w;
}
float x1 = w[umbrella.v1_c1];
float x2 = w[umbrella.v2_c1];
float x1 = w[_neighbor_umbrellas[idx % 3].v1_c1];
float x2 = w[_neighbor_umbrellas[idx % 3].v2_c1];
if (is_zero(x1)) {
if (!inclusive) {
return -1;
}
return umbrella.components[x1 < 0 ? 2 : 1];
return _neighbor_umbrella_component(idx, x1 < 0 ? 2 : 1);
} else if (x1 < 0) {
return umbrella.components[2];
return _neighbor_umbrella_component(idx, 2);
}
if (is_zero(x2)) {
if (!inclusive) {
return -1;
}
return umbrella.components[x2 > 0 ? 1 : 0];
return _neighbor_umbrella_component(idx, x2 > 0 ? 1 : 0);
} else if (x2 < 0) {
return umbrella.components[0];
return _neighbor_umbrella_component(idx, 0);
}
return umbrella.components[1];
return comp;
} else {
/* If the coefficient of the second vertex is lesser than the first
* one's, then v crosses the first, fourth or fifth component. */
auto w = _inverses[umbrella.components[4] % 10] * v;
if (umbrella.components[4] > 9) {
int comp = _neighbor_umbrella_component(idx, 4);
auto w = _inverses[comp % 10] * v;
if (comp > 9) {
w = -w;
}
float x4 = w[umbrella.v4_c4];
float x0 = w[umbrella.v0_c4];
float x4 = w[_neighbor_umbrellas[idx % 3].v4_c4];
float x0 = w[_neighbor_umbrellas[idx % 3].v0_c4];
if (is_zero(x4)) {
if (!inclusive) {
return -1;
}
return umbrella.components[x4 < 0 ? 0 : 4];
return _neighbor_umbrella_component(idx, x4 < 0 ? 0 : 4);
} else if (x4 < 0) {
return umbrella.components[0];
return _neighbor_umbrella_component(idx, 0);
}
if (is_zero(x0)) {
if (!inclusive) {
return -1;
}
return umbrella.components[x0 > 0 ? 4 : 3];
return _neighbor_umbrella_component(idx, x0 > 0 ? 4 : 3);
} else if (x0 < 0) {
return umbrella.components[3];
return _neighbor_umbrella_component(idx, 3);
}
return umbrella.components[4];
return comp;
}
}

43
libraries/AP_Math/AP_GeodesicGrid.h
View File

@ -181,16 +181,21 @@ private:
Matrix3f _mid_inverses[10];
/**
* The representation of the neighbor umbrellas of T_0 and its opposite
* (i.e. T_10).
* The representation of the neighbor umbrellas of T_0.
*
* Let T_0 = (a, b, c). Then:
* - _neighbor_umbrellas[0] is neighbor of T_0 with respect to (a, b).
* - _neighbor_umbrellas[1] is neighbor of T_0 with respect to (b, c).
* - _neighbor_umbrellas[2] is neighbor of T_0 with respect to (c, a).
* - _neighbor_umbrellas[3] is neighbor of T_10 with respect to (-a, -b).
* - _neighbor_umbrellas[4] is neighbor of T_10 with respect to (-b, -c).
* - _neighbor_umbrellas[5] is neighbor of T_10 with respect to (-c, -a).
* The values for the neighbors of T_10 can be derived from the values for
* T_0. How to find the correct values is explained on each member.
*
* Let T_0 = (a, b, c). Thus, 6 indexes can be used for this data
* structure, so that:
* - index 0 represents the neighbor of T_0 with respect to (a, b).
* - index 1 represents the neighbor of T_0 with respect to (b, c).
* - index 2 represents the neighbor of T_0 with respect to (c, a).
* - index 3 represents the neighbor of T_10 with respect to (-a, -b).
* - index 4 represents the neighbor of T_10 with respect to (-b, -c).
* - index 5 represents the neighbor of T_10 with respect to (-c, -a).
*
* Those indexes are mapped to this array with index % 3.
*
* The edges are represented with pairs because the order of the vertices
* matters to the order the triangles' indexes are defined - the order of
@ -201,6 +206,10 @@ private:
/**
* The umbrella's components. The value of #components[i] is the
* icosahedron triangle index of the i-th component.
*
* In order to find the components for T_10, the following just finding
* the index of the opposite triangle is enough. In other words,
* (#components[i] + 10) % 20.
*/
uint8_t components[5];
/**
@ -208,14 +217,28 @@ private:
* following: vi_cj is the index of the vector, in the icosahedron
* triangle pointed by #components[j], that matches the umbrella's i-th
* vertex.
*
* The values don't change for T_10.
*/
uint8_t v0_c0;
uint8_t v1_c1;
uint8_t v2_c1;
uint8_t v4_c4;
uint8_t v0_c4;
} _neighbor_umbrellas[6];
} _neighbor_umbrellas[3];
/**
* Get the component_index-th component of the umbrella_index-th neighbor
* umbrella.
*
* @param umbrella_index[in] The neighbor umbrella's index.
*
* @param component_index[in] The component's index.
*
* @return The icosahedron triangle's index of the component.
*/
int _neighbor_umbrella_component(int umbrella_index,
int component_index) const;
/**
* Find the icosahedron triangle index of the component of
* #_neighbor_umbrellas[umbrella_index] that is crossed by \p v.