From c7eb46fae21a96fb74f7185cba37a020b6854967 Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Fri, 15 Apr 2016 17:26:32 -0300 Subject: [PATCH] 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 --- libraries/AP_Math/AP_GeodesicGrid.cpp | 102 ++++++++++++++++++++------ libraries/AP_Math/AP_GeodesicGrid.h | 11 +++ 2 files changed, 91 insertions(+), 22 deletions(-) diff --git a/libraries/AP_Math/AP_GeodesicGrid.cpp b/libraries/AP_Math/AP_GeodesicGrid.cpp index 9467cd6baf..9685486d89 100644 --- a/libraries/AP_Math/AP_GeodesicGrid.cpp +++ b/libraries/AP_Math/AP_GeodesicGrid.cpp @@ -119,12 +119,12 @@ AP_GeodesicGrid::AP_GeodesicGrid() {{ 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}}, + {{ 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}, } { _init_opposite_triangles(); @@ -229,27 +229,85 @@ int AP_GeodesicGrid::_from_neighbor_umbrella(int idx, bool inclusive) const { 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; - } - if (!is_zero(w.y) && w.y < 0) { - continue; - } - if (!is_zero(w.z) && w.z < 0) { - continue; + /* 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. */ + + 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 umbrella.components[0]; + } else if (x0 < 0) { + if (!inclusive) { + return -1; + } + return umbrella.components[u.x < u.y ? 3 : 2]; } - if ((is_zero(w.x) || is_zero(w.y) || is_zero(w.z)) && !inclusive) { - return -1; - } - - return umbrella.components[i]; + return umbrella.components[0]; } - return -1; + 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 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, diff --git a/libraries/AP_Math/AP_GeodesicGrid.h b/libraries/AP_Math/AP_GeodesicGrid.h index a6ca538d74..9af70be0ab 100644 --- a/libraries/AP_Math/AP_GeodesicGrid.h +++ b/libraries/AP_Math/AP_GeodesicGrid.h @@ -233,6 +233,17 @@ private: * icosahedron triangle index of the i-th component. */ 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]; /**