Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: AP_GeodesicGrid: optimize with neighbor umbrellas 

This is a first optimization of the algorithm. The struct for the neighbor
umbrella has only one member, but new members will be added in the next
optimization.

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:

Cases  | Naive implementation | First Optimization
--------------------------------------------------
Min.   |                 26.0 |              28.00
1st Qu.|                 78.0 |              48.75
Median |                132.0 |              57.00
Mean   |                130.1 |              61.20
3rd Qu.|                182.2 |              76.00
Max.   |                234.0 |              98.00

This optimization reduces the mean time for the worst case (Max. line) by more
than 50%.
This commit is contained in:
Gustavo Jose de Sousa 2016-04-14 19:56:07 -03:00 committed by Lucas De Marchi
parent 0ff05b7af3
commit c28c3265c8
2 changed files with 237 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
libraries/AP_Math/AP_GeodesicGrid.cpp
View File

@ -101,6 +101,8 @@
* by the non-zero coefficient.
*/
#include <assert.h>
#include "AP_GeodesicGrid.h"
AP_GeodesicGrid::AP_GeodesicGrid()
@ -116,6 +118,14 @@ AP_GeodesicGrid::AP_GeodesicGrid()
{{ 1, 0,-M_GOLDEN}, { 0, M_GOLDEN,-1}, {-1, 0,-M_GOLDEN}},
{{ 0, M_GOLDEN,-1}, {-M_GOLDEN, 1, 0}, {-1, 0,-M_GOLDEN}},
}
, _neighbor_umbrellas{
{{ 9, 8, 7, 12, 14}},
{{ 1, 2, 4, 5, 3}},
{{16, 15, 13, 18, 17}},
{{19, 18, 17, 2, 4}},
{{11, 12, 14, 15, 13}},
{{ 6, 5, 3, 8, 7}},
}
{
_init_opposite_triangles();
_init_mid_triangles();
@ -213,13 +223,14 @@ void AP_GeodesicGrid::_init_all_inverses()
}
}
int AP_GeodesicGrid::_triangle_index(const Vector3f& v,
const bool inclusive) const
int AP_GeodesicGrid::_from_neighbor_umbrella(int idx,
const Vector3f& v,
const Vector3f& u,
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;
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) {
continue;
@ -235,12 +246,147 @@ int AP_GeodesicGrid::_triangle_index(const Vector3f& v,
return -1;
}
return i;
return umbrella.components[i];
}
return -1;
}
int AP_GeodesicGrid::_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 _triangles[0] */
auto w = _inverses[0] * v;
int zero_count = 0;
int balance = 0;
int umbrella = -1;
if (is_zero(w.x)) {
zero_count++;
} else if (w.x > 0) {
balance++;
} else {
balance--;
}
if (is_zero(w.y)) {
zero_count++;
} else if (w.y > 0) {
balance++;
} else {
balance--;
}
if (is_zero(w.z)) {
zero_count++;
} else if (w.z > 0) {
balance++;
} else {
balance--;
}
switch (balance) {
case 3:
/* All coefficients are positive, thus return the first triangle. */
return 0;
case -3:
/* All coefficients are negative, which means that the coefficients for
* -w are positive, thus return the first triangle's opposite. */
return 10;
case 2:
/* Two coefficients are positive and one is zero, thus v crosses one of
* the edges of the first triangle. */
return inclusive ? 0 : -1;
case -2:
/* Analogous to the previous case, but for the opposite of the first
* triangle. */
return inclusive ? 10 : -1;
case 1:
/* There are two possible cases when balance is 1:
*
* 1) Two coefficients are zero, which means v crosses one of the
* vertices of the first triangle.
*
* 2) Two coefficients are positive and one is negative. Let a and b be
* vertices with positive coefficients and c the one with the negative
* coefficient. That means that v crosses the triangle formed by a, b
* and -c. The vector -c happens to be the 3-th vertex, with respect to
* (a, b), of the first triangle's neighbor umbrella with respect to a
* and b. Thus, v crosses one of the components of that umbrella. */
if (zero_count == 2) {
return inclusive ? 0 : -1;
}
if (!is_zero(w.x) && w.x < 0) {
umbrella = 1;
} else if (!is_zero(w.y) && w.y < 0) {
umbrella = 2;
} else {
umbrella = 0;
}
break;
case -1:
/* Analogous to the previous case, but for the opposite of the first
* triangle. */
if (zero_count == 2) {
return inclusive ? 10 : -1;
}
if (!is_zero(w.x) && w.x > 0) {
umbrella = 4;
} else if (!is_zero(w.y) && w.y > 0) {
umbrella = 5;
} else {
umbrella = 3;
}
w = -w;
break;
case 0:
/* There are two possible cases when balance is 1:
*
* 1) The vector v is the null vector. Arbitrarily return first
* triangle.
*
* 2) One coefficient is zero, another is positive and yet another is
* negative. Let a, b and c be the respective vertices for those
* coefficients, then the statements in case (2) for when balance is 1
* are also valid here.
*/
if (zero_count == 3) {
return inclusive ? 0 : -1;
}
if (!is_zero(w.x) && w.x < 0) {
umbrella = 1;
} else if (!is_zero(w.y) && w.y < 0) {
umbrella = 2;
} else {
umbrella = 0;
}
break;
}
assert(umbrella >= 0);
switch (umbrella % 3) {
case 0:
w.z = -w.z;
break;
case 1:
w(w.y, w.z, -w.x);
break;
case 2:
w(w.z, w.x, -w.y);
break;
}
return _from_neighbor_umbrella(umbrella, v, w, inclusive);
}
int AP_GeodesicGrid::_subtriangle_index(const unsigned int triangle_index,
const Vector3f& v,
const bool inclusive) const

84
libraries/AP_Math/AP_GeodesicGrid.h
View File

@ -146,6 +146,41 @@ public:
Vector3f& c) const;
private:
/*
* The following are concepts used in the description of the private
* members.
*
* Neighbor triangle with respect to an edge
* -----------------------------------------
* Let T be a triangle. The triangle W is a neighbor of T with respect to
* edge e if T and W share that edge. If e is formed by vectors a and b,
* then W can be said to be a neighbor of T with respect to a and b.
*
* Umbrella of a vector
* --------------------
* Let v be one vertex of the icosahedron. The umbrella of v is the set of
* icosahedron triangles that share that vertex. The vector v is called the
* umbrella's pivot.
*
* Let T have vertices v, a and b. Then, with respect to (a, b):
* - The vector a is the umbrella's 0-th vertex.
* - The vector b is the 1-th vertex.
* - The triangle formed by the v, the i-th and ((i + 1) % 5)-th vertex is
* the umbrella's i-th component.
* - For i in [2,5), the i-th vertex is the vertex that, with the
* (i - 1)-th and v, forms the neighbor of the (i - 2)-th component with
* respect to v and the (i - 1)-th vertex.
*
* Still with respect to (a, b), the umbrella's i-th component is the
* triangle formed by the i-th and ((i + 1) % 5)-th vertices and the pivot.
*
* Neighbor umbrella with respect to an icosahedron triangle's edge
* ----------------------------------------------------------------
* Let T be an icosahedron triangle. Let W be the T's neighbor triangle wrt
* the edge e. Let w be the W's vertex that is opposite to e. Then the
* neighbor umbrella of T with respect to e is the umbrella of w.
*/
/**
* The icosahedron's triangles. The item `_triangles[i]` represents T_i.
*/
@ -174,6 +209,31 @@ private:
*/
Matrix3f _mid_inverses[20];
/**
* The representation of the neighbor umbrellas of T_0 and its opposite
* (i.e. T_10).
*
* 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 edges are represented with pairs because the order of the vertices
* matters to the order the triangles' indexes are defined - the order of
* the umbrellas' vertices and components is convertioned to be with
* respect to those pairs.
*/
struct neighbor_umbrella {
/**
* The umbrella's components. The value of #components[i] is the
* icosahedron triangle index of the i-th component.
*/
int components[5];
} _neighbor_umbrellas[6];
/**
* Initialize the opposite of the first 10 icosahedron triangles.
*/
@ -190,6 +250,30 @@ private:
*/
void _init_all_inverses();
/**
* Find the icosahedron triangle index of the component of
* #_neighbor_umbrellas[umbrella_index] that is crossed by \p v.
*
* @param umbrella_index[in] The umbrella index. Must be in [0,6).
*
* @param v[in] The vector to be tested.
*
* @param u[in] The vector \p u must be \p v expressed with respect to the
* base formed by the umbrella's 0-th, 1-th and 3-th vertices, in that
* order.
*
* @param inclusive[in] This parameter follows the same rules defined in
* #section() const.
*
* @return The index of the icosahedron triangle. The value -1 is returned
* if the triangle isn't found, which might happen when \p inclusive is
* false.
*/
int _from_neighbor_umbrella(int umbrella_index,
const Vector3f& v,
const Vector3f& u,
bool inclusive) const;
/**
* Find which icosahedron's triangle is crossed by \p v.
*