kev/Drawer/GVision/SurfaceGrid/lib/ASTriangle.h

/*------------------------------------------------------------------------------
 *	Copyright (c) 2023 by Bai Bing (seread@163.com)
 *	See COPYING file for copying and redistribution conditions.
 *
 *  Alians IT Studio.
 *----------------------------------------------------------------------------*/
#pragma once

#include <array>
#include <cmath>
#include <map>
#include <set>
#include <stdexcept>

#include "_Define.h"
#include "utils/EssentiallyEqual.h"
#include "utils/Misc.h"
#include "ASPoint.h"
#include "ASEdge.h"

namespace ais
{
    template <class PT = Point>
    class AIS_EXPORT Triangle
    {
    public:
        PT points[3];
        size_t hash{0};

        PT min_max[2];
        std::vector<PT> polygon;

        Triangle() = default;
        Triangle(const PT &p0, const PT &p1, const PT &p2)
        {
            points[0] = p0;
            points[1] = p1;
            points[2] = p2;

            // test box
            double xMin = std::min(p0.x, std::min(p1.x, p2.x));
            double yMin = std::min(p0.y, std::min(p1.y, p2.y));
            double xMax = std::max(p0.x, std::max(p1.x, p2.x));
            double yMax = std::max(p0.y, std::max(p1.y, p2.y));

            min_max[0] = PT{xMin, yMin};
            min_max[1] = PT{xMax, yMax};

            // test polygon
            PT p2d0{p0};
            PT p2d1{p1};
            PT p2d2{p2};
            polygon = {p2d0, p2d1, p2d2, p2d0};

            // update hash
            get_hash();
        }

        bool operator==(const Triangle<PT> &rhs) const
        {
            // return points[0] == rhs.points[0] && points[1] == rhs.points[1] && points[2] == rhs.points[2];
            return hash == rhs.hash;
        }

        size_t get_hash()
        {
            if (hash != 0)
                return hash;

            std::array<PT, 3> sa{points[0], points[1], points[2]};
            std::sort(sa.begin(), sa.end());

            size_t h0 = sa[0].hash2D();
            size_t h1 = sa[1].hash2D();
            size_t h2 = sa[2].hash2D();

            hash = h0 ^ (h1 << 1) ^ (h2 << 2);
            return hash;
        }

        //============================================================================
        /// test a point in the triangle or not
        ///
        /// @param point
        /// @return bool
        ///
        bool contains(const Point &point) const
        {
            double x1 = points[0].x, y1 = points[0].y;
            double x2 = points[1].x, y2 = points[1].y;
            double x3 = points[2].x, y3 = points[2].y;
            double x = point.x, y = point.y;

            double alpha = ((y2 - y3) * (x - x3) + (x3 - x2) * (y - y3)) /
                           ((y2 - y3) * (x1 - x3) + (x3 - x2) * (y1 - y3));
            double beta = ((y3 - y1) * (x - x3) + (x1 - x3) * (y - y3)) /
                          ((y2 - y3) * (x1 - x3) + (x3 - x2) * (y1 - y3));
            double gamma = 1 - alpha - beta;

            return alpha >= 0 && beta >= 0 && gamma >= 0;
        }

        //============================================================================
        /// test a point inside the circumcircle of triangle or not
        ///
        /// @param point
        /// @return bool
        ///
        bool inside_circle(const PT &point) const
        {
            double x1 = points[0].x, y1 = points[0].y;
            double x2 = points[1].x, y2 = points[1].y;
            double x3 = points[2].x, y3 = points[2].y;

            auto d = 2 * (x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2));
            auto ux = ((x1 * x1 + y1 * y1) * (y2 - y3) + (x2 * x2 + y2 * y2) * (y3 - y1) + (x3 * x3 + y3 * y3) * (y1 - y2)) / d;
            auto uy = ((x1 * x1 + y1 * y1) * (x3 - x2) + (x2 * x2 + y2 * y2) * (x1 - x3) + (x3 * x3 + y3 * y3) * (x2 - x1)) / d;

            auto circumRadius = points[0].distance(ux, uy);
            auto distance = point.distance(ux, uy);

            return circumRadius > distance;
        }

        //============================================================================
        /// Get center point of the triangle's circumcircle
        ///
        /// @return point
        ///
        Point circumcircle_center() const
        {
            double x1 = points[0].x, y1 = points[0].y;
            double x2 = points[1].x, y2 = points[1].y;
            double x3 = points[2].x, y3 = points[2].y;

            double A = x2 - x1, B = y2 - y1, C = x3 - x1, D = y3 - y1;
            double E = A * (x1 + x2) + B * (y1 + y2);
            double F = C * (x1 + x3) + D * (y1 + y3);
            double G = 2 * (A * (y3 - y2) - B * (x3 - x2));

            if (std::fabs(G) < 1e-10)
                return Point(0.0, 0.0);

            double centerX = (D * E - B * F) / G;
            double centerY = (A * F - C * E) / G;

            return Point(centerX, centerY);
        }
    };

    //============================================================================
    // added unique edge to all edges for delaunay triangulation
    template <class PT = Point>
    inline void add_unique_edge(std::map<size_t, std::pair<Edge<PT>, bool>> &edges, const Edge<PT> &edge, bool contact_super_triangle)
    {
        auto key = edge.hash;
        auto ei = edges.find(key);

        if (ei == edges.end())
        {
            auto value = std::make_pair(edge, contact_super_triangle);
            edges.insert({key, value});
        }
        else
        {
            edges.erase(ei);
        }
    }

    //============================================================================
    // test one triangle is contact to super triangle, means test the triangle included the super triangle points
    template <class PT = Point>
    bool is_contact_super_triangle(const Triangle<PT> &triangle, const Triangle<PT> &super_triangle)
    {
        auto &st = super_triangle;
        auto &t = triangle;
        std::set<size_t> super_triangle_hashs = {st.points[0].hash(), st.points[1].hash(), st.points[2].hash()};
        std::set<size_t> triangle_hashs = {
            t.points[0].hash(),
            t.points[1].hash(),
            t.points[2].hash(),
        };

        for (auto &th : triangle_hashs)
        {
            if (super_triangle_hashs.find(th) != super_triangle_hashs.end())
                return true;
        }

        return false;

        /*return triangle.points[0] == super_triangle.points[0] || triangle.points[0] == super_triangle.points[1] || triangle.points[0] == super_triangle.points[2] ||
               triangle.points[1] == super_triangle.points[0] || triangle.points[1] == super_triangle.points[1] || triangle.points[1] == super_triangle.points[2] ||
               triangle.points[2] == super_triangle.points[0] || triangle.points[2] == super_triangle.points[1] || triangle.points[2] == super_triangle.points[2];*/
    }

    //============================================================================
    /// use delaunay triangulation points
    ///
    /// @param points
    /// @param triangles output
    ///
    template <class PT = Point>
    void AIS_EXPORT delaunay_triangulation(std::vector<PT> points, std::vector<Triangle<PT>> &triangles)
    {
        if (points.size() < 3)
            return;

        // ensure the range of test area
        auto x = getAxisValues(points, Axis3DType::X);
        auto y = getAxisValues(points, Axis3DType::Y);
        auto [xMin, xMax] = getMinMax(x);
        auto [yMin, yMax] = getMinMax(y);

        double delta = std::max(xMax - xMin, yMax - yMin) * 5;
        Point3D p1{xMin - delta, yMin - delta, INFINITY};
        Point3D p2{xMin - delta, yMax + delta, INFINITY};
        Point3D p3{xMax + delta, yMax + delta, INFINITY};
        Point3D p4{xMax + delta, yMin - delta, INFINITY};

        Triangle<PT> super_triangle1(p1, p2, p3);
        Triangle<PT> super_triangle2(p3, p4, p1);

        std::map<size_t, Triangle<PT>> triangles_map;

        // build the super_triangles
        triangles_map.insert({super_triangle1.hash, super_triangle1});
        triangles_map.insert({super_triangle2.hash, super_triangle2});

        // Add each point to the triangulation
        for (auto &point : points)
        {
            std::map<size_t, Triangle<PT>> bad_triangles_map;
            std::map<size_t, std::pair<Edge<PT>, bool>> polygon;

            // Find the triangles that are no longer valid with the addition of the new point
            for (auto &tri_kv : triangles_map)
            {
                auto &triangle = tri_kv.second;
                if (triangle.inside_circle(point))
                    bad_triangles_map.insert({triangle.hash, triangle});
            }

            // Find the boundary edges of the polygonal hole
            for (auto &bt_kv : bad_triangles_map)
            {
                auto &t = bt_kv.second;
                Edge<PT> e0{t.points[0], t.points[1]};
                Edge<PT> e1{t.points[1], t.points[2]};
                Edge<PT> e2{t.points[2], t.points[0]};

                auto contact_super = is_contact_super_triangle(t, super_triangle1) || is_contact_super_triangle(t, super_triangle2);

                add_unique_edge(polygon, e0, contact_super);
                add_unique_edge(polygon, e1, contact_super);
                add_unique_edge(polygon, e2, contact_super);
            }

            // Remove the bad triangles from the triangulation
            for (auto &bt_kv : bad_triangles_map)
                triangles_map.erase(bt_kv.first);

            // Add new triangles formed by the boundary edges and the new point
            for (auto &kv : polygon)
            {
                auto edge = kv.second;
                Triangle<PT> t{edge.first.points[0], edge.first.points[1], point};
                if (triangles_map.find(t.hash) == triangles_map.end())
                    triangles_map.insert({t.hash, t});
            }
        }

        // Remove the triangles that contain vertices of the super triangle
        for (auto iter = triangles_map.begin(); iter != triangles_map.end();)
        {
            auto triangle = (*iter).second;
            if (is_contact_super_triangle(triangle, super_triangle1) || is_contact_super_triangle(triangle, super_triangle2))
                iter = triangles_map.erase(iter);
            else
                iter++;
        }

        // finally copy the result to output
        for (auto &kv : triangles_map)
        {
            triangles.push_back(kv.second);
        }

        return;
    }

    //============================================================================
    /// get triangle info
    ///
    /// @param triangles
    /// @param distanceMin/distanceMax output
    ///
    template <class PT = Point>
    void AIS_EXPORT get_triangle_info(std::vector<Triangle<PT>> &triangles, std::map<size_t, std::set<size_t>> &pointLinksMap, double &distanceMin, double &distanceMax)
    {
        pointLinksMap.clear();

        // build the point links
        for (auto &tri : triangles)
        {
            // build point link
            auto points = tri.points;
            auto addedPointLink = [&points, &pointLinksMap](size_t hash)
            {
                if (pointLinksMap.find(hash) == pointLinksMap.end())
                {
                    // added a empty set to init map value while the point is not in map
                    pointLinksMap[hash] = {};
                }

                // add triangle point links
                for (int pi = 0; pi < 3; pi++)
                {
                    PT point = points[pi];
                    size_t pointHash = point.hash2D();
                    if (pointHash != hash)
                        pointLinksMap[hash].insert(pointHash);
                }
            };

            PT p0 = points[0];
            PT p1 = points[1];
            PT p2 = points[2];

            double distance01 = p0.distance2D(p1);
            double distance12 = p1.distance2D(p2);
            double distance20 = p2.distance2D(p0);

            distanceMin = std::min({distance01, distance12, distance20, distanceMin});
            distanceMax = std::max({distance01, distance12, distance20, distanceMax});

            addedPointLink(points[0].hash2D());
            addedPointLink(points[1].hash2D());
            addedPointLink(points[2].hash2D());
        }
    }

    //============================================================================
    /// filter triangles with special point
    ///
    /// @param point
    /// @param source triangles
    /// @param output triangles with special point
    ///
    template <class PT = Point3D>
    void AIS_EXPORT filter_triangles(const PT &point, std::vector<Triangle<PT>> &source, std::vector<Triangle<PT>> &output)
    {
        output.clear();
        for (auto &t : source)
        {
            if (point == t.points[0] || point == t.points[1] || point == t.points[2])
                output.push_back(t);
        }

        return;
    }
}