mirror of https://github.com/ArduPilot/ardupilot
AP_Math: AP_GeodesicGrid: optimize _from_neighbor_umbrella()
This is the second optimization. With that we don't have to iterate over the umbrella's components. The table below summarizes the mean CPU time in ns from the brenchmark results on an Intel(R) Core(TM) i7-3520M CPU @ 2.90GHz processor: | Naive implementation | First Optimization | Second Optimization ------------------------------------------------------------------------ Min. | 26.0 | 28.00 | 26.0 1st Qu.| 78.0 | 48.75 | 39.0 Median | 132.0 | 57.00 | 41.0 Mean | 130.1 | 61.20 | 41.6 3rd Qu.| 182.2 | 76.00 | 47.0 Max. | 234.0 | 98.00 | 54.0
This commit is contained in:
parent
eb90ef23a1
commit
c7eb46fae2
|
@ -119,12 +119,12 @@ AP_GeodesicGrid::AP_GeodesicGrid()
|
||||||
{{ 0, M_GOLDEN,-1}, {-M_GOLDEN, 1, 0}, {-1, 0,-M_GOLDEN}},
|
{{ 0, M_GOLDEN,-1}, {-M_GOLDEN, 1, 0}, {-1, 0,-M_GOLDEN}},
|
||||||
}
|
}
|
||||||
, _neighbor_umbrellas{
|
, _neighbor_umbrellas{
|
||||||
{{ 9, 8, 7, 12, 14}},
|
{{ 9, 8, 7, 12, 14}, 1, 2, 0, 0, 2},
|
||||||
{{ 1, 2, 4, 5, 3}},
|
{{ 1, 2, 4, 5, 3}, 0, 0, 2, 2, 0},
|
||||||
{{16, 15, 13, 18, 17}},
|
{{16, 15, 13, 18, 17}, 2, 2, 0, 2, 1},
|
||||||
{{19, 18, 17, 2, 4}},
|
{{19, 18, 17, 2, 4}, 1, 2, 0, 0, 2},
|
||||||
{{11, 12, 14, 15, 13}},
|
{{11, 12, 14, 15, 13}, 0, 0, 2, 2, 0},
|
||||||
{{ 6, 5, 3, 8, 7}},
|
{{ 6, 5, 3, 8, 7}, 2, 2, 0, 2, 1},
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
_init_opposite_triangles();
|
_init_opposite_triangles();
|
||||||
|
@ -229,27 +229,85 @@ int AP_GeodesicGrid::_from_neighbor_umbrella(int idx,
|
||||||
bool inclusive) const
|
bool inclusive) const
|
||||||
{
|
{
|
||||||
const struct neighbor_umbrella& umbrella = _neighbor_umbrellas[idx];
|
const struct neighbor_umbrella& umbrella = _neighbor_umbrellas[idx];
|
||||||
for (int i = 0; i < 5; i++) {
|
|
||||||
auto w = _inverses[umbrella.components[i]] * v;
|
|
||||||
|
|
||||||
if (!is_zero(w.x) && w.x < 0) {
|
/* The following comparisons between the umbrella's first and second
|
||||||
continue;
|
* vertices' coefficients work for this algorithm because all vertices'
|
||||||
}
|
* vectors are of the same length. */
|
||||||
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) {
|
if (is_equal(u.x, u.y)) {
|
||||||
|
/* 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]] * v;
|
||||||
|
float x0 = w[umbrella.v0_c0];
|
||||||
|
if (is_zero(x0)) {
|
||||||
|
if (!inclusive) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
return umbrella.components[0];
|
||||||
return umbrella.components[i];
|
} else if (x0 < 0) {
|
||||||
|
if (!inclusive) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
return umbrella.components[u.x < u.y ? 3 : 2];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
return umbrella.components[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
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]] * v;
|
||||||
|
float x1 = w[umbrella.v1_c1];
|
||||||
|
float x2 = w[umbrella.v2_c1];
|
||||||
|
|
||||||
|
if (is_zero(x1)) {
|
||||||
|
if (!inclusive) {
|
||||||
return -1;
|
return -1;
|
||||||
|
}
|
||||||
|
return umbrella.components[x1 < 0 ? 2 : 1];
|
||||||
|
} else if (x1 < 0) {
|
||||||
|
return umbrella.components[2];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (is_zero(x2)) {
|
||||||
|
if (!inclusive) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
return umbrella.components[x2 > 0 ? 1 : 0];
|
||||||
|
} else if (x2 < 0) {
|
||||||
|
return umbrella.components[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
return umbrella.components[1];
|
||||||
|
} 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]] * v;
|
||||||
|
float x4 = w[umbrella.v4_c4];
|
||||||
|
float x0 = w[umbrella.v0_c4];
|
||||||
|
|
||||||
|
if (is_zero(x4)) {
|
||||||
|
if (!inclusive) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
return umbrella.components[x4 < 0 ? 0 : 4];
|
||||||
|
} else if (x4 < 0) {
|
||||||
|
return umbrella.components[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (is_zero(x0)) {
|
||||||
|
if (!inclusive) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
return umbrella.components[x0 > 0 ? 4 : 3];
|
||||||
|
} else if (x0 < 0) {
|
||||||
|
return umbrella.components[3];
|
||||||
|
}
|
||||||
|
|
||||||
|
return umbrella.components[4];
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_GeodesicGrid::_triangle_index(const Vector3f& v,
|
int AP_GeodesicGrid::_triangle_index(const Vector3f& v,
|
||||||
|
|
|
@ -233,6 +233,17 @@ private:
|
||||||
* icosahedron triangle index of the i-th component.
|
* icosahedron triangle index of the i-th component.
|
||||||
*/
|
*/
|
||||||
int components[5];
|
int components[5];
|
||||||
|
/**
|
||||||
|
* The fields with name in the format vi_cj are interpreted as the
|
||||||
|
* 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.
|
||||||
|
*/
|
||||||
|
int v0_c0;
|
||||||
|
int v1_c1;
|
||||||
|
int v2_c1;
|
||||||
|
int v4_c4;
|
||||||
|
int v0_c4;
|
||||||
} _neighbor_umbrellas[6];
|
} _neighbor_umbrellas[6];
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
Loading…
Reference in New Issue