ardupilot/libraries/AP_Math/AP_GeodesicGrid.h

299 lines
12 KiB
C
Raw Normal View History

/*
* Copyright (C) 2016 Intel Corporation. All rights reserved.
*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "AP_Math.h"
/**
* AP_GeodesicGrid is a class for working on geodesic sections.
*
* For quick information regarding geodesic grids, see:
* https://en.wikipedia.org/wiki/Geodesic_grid
*
* The grid is formed by a tessellation of an icosahedron by a factor of 2,
* i.e., each triangular face of the icosahedron is divided into 4 by splitting
* each edge into 2 line segments and projecting the vertices to the
* icosahedron's circumscribed sphere. That will give a total of 80 triangular
* faces, which are called sections in this context.
*
* A section index is given by the icosahedron's triangle it belongs to and by
* its index in that triangle. Let i in [0,20) be the icosahedron's triangle
* index and j in [0,4) be the sub-triangle's (which is the section) index
* inside the greater triangle. Then the section index is given by
* s = 4 * i + j .
*
* The icosahedron's triangles are defined by the tuple (T_0, T_1, ..., T_19),
* where T_i is the i-th triangle. Each triangle is represented with a tuple of
* the form (a, b, c), where a, b and c are the triangle vertices in the space.
*
* Given the definitions above and the golden ration as g, the triangles must
* be defined in the following order:
*
* (
* ((-g, 1, 0), (-1, 0,-g), (-g,-1, 0)),
* ((-1, 0,-g), (-g,-1, 0), ( 0,-g,-1)),
* ((-g,-1, 0), ( 0,-g,-1), ( 0,-g, 1)),
* ((-1, 0,-g), ( 0,-g,-1), ( 1, 0,-g)),
* (( 0,-g,-1), ( 0,-g, 1), ( g,-1, 0)),
* (( 0,-g,-1), ( 1, 0,-g), ( g,-1, 0)),
* (( g,-1, 0), ( 1, 0,-g), ( g, 1, 0)),
* (( 1, 0,-g), ( g, 1, 0), ( 0, g,-1)),
* (( 1, 0,-g), ( 0, g,-1), (-1, 0,-g)),
* (( 0, g,-1), (-g, 1, 0), (-1, 0,-g)),
* -T_0,
* -T_1,
* -T_2,
* -T_3,
* -T_4,
* -T_5,
* -T_6,
* -T_7,
* -T_8,
* -T_9,
* )
*
* Where for a given T_i = (a, b, c), -T_i = (-a, -b, -c). We call -T_i the
* opposite triangle of T_i in this specification. For any i in [0,20), T_j is
* the opposite of T_i iff j = (i + 10) % 20.
*
* Let an icosahedron triangle T be defined as T = (a, b, c). The "middle
* triangle" M is defined as the triangle formed by the points that bisect the
* edges of T. M is defined by:
*
* M = (m_a, m_b, m_c) = ((a + b) / 2, (b + c) / 2, (c + a) / 2)
*
* Let elements of the tuple (W_0, W_1, W_2, W_3) comprise the sub-triangles of
* T, so that W_j is the j-th sub-triangle of T. The sub-triangles are defined
* as the following:
*
* W_0 = M
* W_1 = (a, m_a, m_c)
* W_2 = (m_a, b, m_b)
* W_3 = (m_c, m_b, c)
*/
class AP_GeodesicGrid {
friend class GeodesicGridTest;
public:
/*
* The following concepts are used by the description of this class'
* members.
*
* Vector crossing objects
* -----------------------
* We say that a vector v crosses an object in space (point, line, line
* segment, plane etc) iff the line, being Q the set of points of that
* object, the vector v crosses it iff there exists a positive scalar alpha
* such that alpha * v is in Q.
*/
/**
* Number of sub-triangles for an icosahedron triangle.
*/
static const int NUM_SUBTRIANGLES = 4;
/**
* Find which section is crossed by \p v.
*
* @param v[in] The vector to be verified.
*
* @param inclusive[in] If true, then if \p v crosses one of the edges of
* one of the sections, then that section is returned. If \p inclusive is
* false, then \p v is considered to cross no section. Note that, if \p
* inclusive is true, then \p v can belong to more than one section and
* only the first one found is returned. The order in which the triangles
* are checked is unspecified. The default value for \p inclusive is
* false.
*
* @return The index of the section. The value -1 is returned if \p v is
* the null vector or the section isn't found, which might happen when \p
* inclusive is false.
*/
static int section(const Vector3f &v, bool inclusive = false);
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 inverses of the change-of-basis matrices for the icosahedron
* triangles.
*
* The i-th matrix is the inverse of the change-of-basis matrix from
* natural basis to the basis formed by T_i's vectors.
*/
static const Matrix3f _inverses[10];
/**
* The inverses of the change-of-basis matrices for the middle triangles.
*
* The i-th matrix is the inverse of the change-of-basis matrix from
* natural basis to the basis formed by T_i's middle triangle's vectors.
*/
static const Matrix3f _mid_inverses[10];
/**
* The representation of the neighbor umbrellas of T_0.
*
* 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
* the umbrellas' vertices and components is convertioned to be with
* respect to those pairs.
*/
static const struct neighbor_umbrella {
/**
* 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];
/**
* 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.
*
* 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[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.
*/
static int _neighbor_umbrella_component(int umbrella_index, int component_idx);
/**
* 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 \p v is the null vector or the triangle isn't found, which might
* happen when \p inclusive is false.
*/
static int _from_neighbor_umbrella(int umbrella_index,
const Vector3f &v,
const Vector3f &u,
bool inclusive);
/**
* Find which icosahedron's triangle is crossed by \p v.
*
* @param v[in] The vector to be verified.
*
* @param inclusive[in] This parameter follow the same rules defined in
* #section() const.
*
* @return The index of the triangle. The value -1 is returned if the
* triangle isn't found, which might happen when \p inclusive is false.
*/
static int _triangle_index(const Vector3f &v, bool inclusive);
/**
* Find which sub-triangle of the icosahedron's triangle pointed by \p
* triangle_index is crossed by \p v.
*
* The vector \p v must belong to the super-section formed by the triangle
* pointed by \p triangle_index, otherwise the result is undefined.
*
* @param triangle_index[in] The icosahedron's triangle index, it must be in
* the interval [0,20). Passing invalid values is undefined behavior.
*
* @param v[in] The vector to be verified.
*
* @param inclusive[in] This parameter follow the same rules defined in
* #section() const.
*
* @return The index of the sub-triangle. The value -1 is returned if the
* triangle isn't found, which might happen when \p inclusive is false.
*/
static int _subtriangle_index(const unsigned int triangle_index,
const Vector3f &v,
bool inclusive);
};