zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Math: AP_GeodesicGrid: reduce number of _neighbor_umbrellas items 

Only the first half is necessary. The values for the other half can be derived.
mission-4.1.18
Gustavo Jose de Sousa 9 years ago committed by Lucas De Marchi
parent
43d9e00ab7
commit
fd2428db34
2 changed files with 69 additions and 39 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 65
      libraries/AP_Math/AP_GeodesicGrid.cpp
  2. 43
      libraries/AP_Math/AP_GeodesicGrid.h

65
libraries/AP_Math/AP_GeodesicGrid.cpp
Unescape View File

@ -110,9 +110,6 @@ AP_GeodesicGrid::AP_GeodesicGrid() @@ -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, @@ -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, @@ -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
Unescape View File

@ -181,16 +181,21 @@ private: @@ -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: @@ -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: @@ -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.

Write Preview
Loading…
Cancel
Save