zr-v4/libraries/AP_Math/AP_GeodesicGrid.cpp

/*
 * 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/>.
 */

/*
 * This comment section explains the basic idea behind the implementation.
 *
 * Vectors difference notation
 * ===========================
 * Let v and w be vectors. For readability purposes, unless explicitly
 * otherwise noted, the notation vw will be used to represent w - v.
 *
 * Relationship between a vector and a triangle in 3d space
 * ========================================================
 * Vector in the area of a triangle
 * --------------------------------
 * Let T = (a, b, c) be a triangle, where a, b and c are also vectors and
 * linearly independent. A vector inside that triangle can be written as one of
 * its vertices plus the sum of the positively scaled vectors from that vertex
 * to the other ones. Taking a as the first vertex, a vector p in the area
 * formed by T can be written as:
 *
 *     p = a + w_ab * ab + w_ac * ac
 *
 * It's fairly easy to see that if p is in the area formed by T, then w_ab >= 0
 * and w_ac >= 0. That vector p can also be written as:
 *
 *     p = b + w_ba * ba + w_bc * bc
 *
 * It's easy to check that the triangle formed by (a + w_ab * ab, b + w_ba *
 * ba, p) is similar to T and, with the correct algebraic manipulations, we can
 * come to the conclusion that:
 *
 *     w_ba = 1 - w_ab - w_ac
 *
 * Since we know that w_ba >= 0, then w_ab + w_ac <= 1. Thus:
 *
 *     ----------------------------------------------------------
 *     | p = a + w_ab * ab + w_ac * ac is in the area of T iff: |
 *     | w_ab >= 0 and w_ac >= 0 and w_ab + w_ac <= 1           |
 *     ----------------------------------------------------------
 *
 * Proving backwards shouldn't be difficult.
 *
 * Vector p can also be written as:
 *
 *     p = (1 - w_ab - w_ba) * a + w_ab * b + w_ba * c
 *
 *
 * Vector that crosses a triangle
 * ------------------------------
 * Let T be the same triangle discussed above and let v be a vector such that:
 *
 *     v = x * a + y * b + z * c
 *     where x >= 0, y >= 0, z >= 0, and x + y + z > 0.
 *
 * It's geometrically easy to see that v crosses the triangle T. But that can
 * also be verified analytically.
 *
 * The vector v crosses the triangle T iff there's a positive alpha such that
 * alpha * v is in the area formed by T, so we need to prove that such value
 * exists. To find alpha, we solve the equation alpha * v = p, which will lead
 * us to the system, for the variables alpha, w_ab and w_ac:
 *
 *    alpha * x = 1 - w_ab - w_ac
 *    alpha * y = w_ab
 *    alpha * z = w_ac,
 *    where w_ab >= 0 and w_ac >= 0 and w_ab + w_ac <= 1
 *
 * That will lead to alpha = 1 / (x + y + z), w_ab = y / (x + y + b) and
 * w_ac = z / (x + y + z) and the following holds:
 *  - alpha does exist because x + y + z > 0.
 *  - w_ab >= 0 and w_ac >= 0 because y >= 0 and z >= 0 and x + y + z > 0.
 *  - 0 <= 1 - w_ab - w_ac <= 1 because 0 <= (y + z) / (x + y + z) <= 1.
 *
 * Thus:
 *
 *     ----------------------------------------------------------
 *     | v = x * a + y * b + z * c crosses T = (a, b, c), where |
 *     | a, b and c are linearly independent, iff:              |
 *     | x >= 0, y >= 0, z >= 0 and x + y + z > 0               |
 *     ----------------------------------------------------------
 *
 * Moreover:
 *  - if one of the coefficients is zero, then v crosses the edge formed by the
 *  vertices multiplied by the non-zero coefficients.
 *  - if two of the coefficients are zero, then v crosses the vertex multiplied
 *  by the non-zero coefficient.
 */

#include "AP_GeodesicGrid.h"

AP_GeodesicGrid::AP_GeodesicGrid()
    : _triangles{
          {{-M_GOLDEN, 1, 0}, {-1, 0,-M_GOLDEN}, {-M_GOLDEN,-1, 0}},
          {{-1, 0,-M_GOLDEN}, {-M_GOLDEN,-1, 0}, { 0,-M_GOLDEN,-1}},
          {{-M_GOLDEN,-1, 0}, { 0,-M_GOLDEN,-1}, { 0,-M_GOLDEN, 1}},
          {{-1, 0,-M_GOLDEN}, { 0,-M_GOLDEN,-1}, { 1, 0,-M_GOLDEN}},
          {{ 0,-M_GOLDEN,-1}, { 0,-M_GOLDEN, 1}, { M_GOLDEN,-1, 0}},
          {{ 0,-M_GOLDEN,-1}, { 1, 0,-M_GOLDEN}, { M_GOLDEN,-1, 0}},
          {{ M_GOLDEN,-1, 0}, { 1, 0,-M_GOLDEN}, { M_GOLDEN, 1, 0}},
          {{ 1, 0,-M_GOLDEN}, { M_GOLDEN, 1, 0}, { 0, M_GOLDEN,-1}},
          {{ 1, 0,-M_GOLDEN}, { 0, M_GOLDEN,-1}, {-1, 0,-M_GOLDEN}},
          {{ 0, M_GOLDEN,-1}, {-M_GOLDEN, 1, 0}, {-1, 0,-M_GOLDEN}},
          {{-M_GOLDEN, 1, 0}, {-1, 0, M_GOLDEN}, {-M_GOLDEN,-1, 0}},
          {{-1, 0, M_GOLDEN}, {-M_GOLDEN,-1, 0}, { 0,-M_GOLDEN, 1}},
          {{-1, 0, M_GOLDEN}, { 0,-M_GOLDEN, 1}, { 1, 0, M_GOLDEN}},
          {{ 0,-M_GOLDEN, 1}, { 1, 0, M_GOLDEN}, { M_GOLDEN,-1, 0}},
          {{ M_GOLDEN,-1, 0}, { 1, 0, M_GOLDEN}, { M_GOLDEN, 1, 0}},
          {{ 1, 0, M_GOLDEN}, { M_GOLDEN, 1, 0}, { 0, M_GOLDEN, 1}},
          {{ M_GOLDEN, 1, 0}, { 0, M_GOLDEN, 1}, { 0, M_GOLDEN,-1}},
          {{ 1, 0, M_GOLDEN}, { 0, M_GOLDEN, 1}, {-1, 0, M_GOLDEN}},
          {{ 0, M_GOLDEN, 1}, { 0, M_GOLDEN,-1}, {-M_GOLDEN, 1, 0}},
          {{ 0, M_GOLDEN, 1}, {-M_GOLDEN, 1, 0}, {-1, 0, M_GOLDEN}}
      }
{
    _init_mid_triangles();
    _init_all_inverses();
}

int AP_GeodesicGrid::section(const Vector3f& v,
                             const bool inclusive) const
{
    int i = _triangle_index(v, inclusive);
    if (i < 0) {
        return -1;
    }

    int j = _subtriangle_index(i, v, inclusive);
    if (j < 0) {
        return -1;
    }

    return 4 * i + j;
}

bool AP_GeodesicGrid::section_triangle(unsigned int section_index,
                                       Vector3f& a,
                                       Vector3f& b,
                                       Vector3f& c) const
{
    if (section_index >= 20 * NUM_SUBTRIANGLES) {
        return false;
    }

    unsigned int i = section_index / NUM_SUBTRIANGLES;
    unsigned int j = section_index % NUM_SUBTRIANGLES;
    auto& t = _triangles[i];
    auto& mt = _mid_triangles[i];

    switch (j) {
        case 0:
            a = mt[0];
            b = mt[1];
            c = mt[2];
            break;
        case 1:
            a = t[0];
            b = mt[0];
            c = mt[2];
            break;
        case 2:
            a = mt[0];
            b = t[1];
            c = mt[1];
            break;
        case 3:
            a = mt[2];
            b = mt[1];
            c = t[2];
            break;
    }

    return true;
}

void AP_GeodesicGrid::_init_mid_triangles()
{
    for (int i = 0; i < 20; i++) {
        _mid_triangles[i][0] = (_triangles[i][0] + _triangles[i][1]) / 2;
        _mid_triangles[i][1] = (_triangles[i][1] + _triangles[i][2]) / 2;
        _mid_triangles[i][2] = (_triangles[i][2] + _triangles[i][0]) / 2;
    }
}

void AP_GeodesicGrid::_init_all_inverses()
{
    for (int i = 0; i < 20; i++) {
        auto& t = _triangles[i];
        _inverses[i]({t[0].x, t[1].x, t[2].x},
                     {t[0].y, t[1].y, t[2].y},
                     {t[0].z, t[1].z, t[2].z});
        _inverses[i].invert();

        auto& mt = _mid_triangles[i];
        _mid_inverses[i]({mt[0].x, mt[1].x, mt[2].x},
                         {mt[0].y, mt[1].y, mt[2].y},
                         {mt[0].z, mt[1].z, mt[2].z});
        _mid_inverses[i].invert();
    }
}

int AP_GeodesicGrid::_triangle_index(const Vector3f& v,
                                     const 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;

        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;
        }

        if ((is_zero(w.x) || is_zero(w.y) || is_zero(w.z)) && !inclusive) {
            return -1;
        }

        return i;
    }

    return -1;
}

int AP_GeodesicGrid::_subtriangle_index(const unsigned int 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 _mid_triangles[triangle_index] */
    auto w = _mid_inverses[triangle_index] * v;

    if ((is_zero(w.x) || is_zero(w.y) || is_zero(w.z)) && !inclusive) {
        return -1;
    }

    /* At this point, we know that v crosses the icosahedron triangle pointed
     * by triangle_index. Thus, we can geometrically see that if v doesn't
     * cross its middle triangle, then one of the coefficients will be negative
     * and the other ones positive. Let a and b be the non-negative
     * coefficients and c the negative one. In that case, v will cross the
     * triangle with vertices (a, b, -c). Since we know that v crosses the
     * icosahedron triangle and the only sub-triangle that contains the set of
     * points (seen as vectors) that cross the triangle (a, b, -c) is the
     * middle triangle's neighbor with respect to a and b, then that
     * sub-triangle is the one crossed by v. */
    if (!is_zero(w.x) && w.x < 0) {
        return 3;
    }
    if (!is_zero(w.y) && w.y < 0) {
        return 1;
    }
    if (!is_zero(w.z) && w.z < 0) {
        return 2;
    }

    /* If x >= 0 and y >= 0 and z >= 0, then v crosses the middle triangle. */
    return 0;
}