diff --git a/libraries/AP_Math/AP_GeodesicGrid.cpp b/libraries/AP_Math/AP_GeodesicGrid.cpp index a4c59e1def..d7786812d4 100644 --- a/libraries/AP_Math/AP_GeodesicGrid.cpp +++ b/libraries/AP_Math/AP_GeodesicGrid.cpp @@ -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; } } diff --git a/libraries/AP_Math/AP_GeodesicGrid.h b/libraries/AP_Math/AP_GeodesicGrid.h index f4063220a7..feb8e071b3 100644 --- a/libraries/AP_Math/AP_GeodesicGrid.h +++ b/libraries/AP_Math/AP_GeodesicGrid.h @@ -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.