ConcaveToConvexPolygon.h

#pragma once
#include <NDEVR/Polygon.h>
#include <NDEVR/Dictionary.h>
#include <NDEVR/OpenMP.h>
#include <vector>
#include <cmath>
#include <map>
#include <iostream>
namespace NDEVR
{
    template<class t_type, class t_vertex_type>
    struct SliceVertex : public t_vertex_type
    {
        SliceVertex() {}

        SliceVertex(t_vertex_type const& _position)
            : t_vertex_type{ _position } {}
 
        int index; 
        t_type distanceToSlice; 
    };

    template<class t_type, class t_vertex = Vertex<2, t_type>>
    class ConcavePolygon
    {
        typedef Polygon<t_type, t_vertex> VertexArray; 
        typedef Buffer<Polygon<t_type, t_vertex>> PolygonArray; 
        typedef LineSegment<2, t_type, Vertex<2, t_type>> Line; 
        VertexArray vertices; 
        PolygonArray processed_polygons; 
 

        static inline fltp08 Det(fltp08 a, fltp08 b, fltp08 c, fltp08 d)
        {
            return a * d - b * c;
        }

        static inline int orientation(const Vertex<2, t_type>& p, const Vertex<2, t_type>& q, const Vertex<2, t_type>& r)
        {
            fltp08 val = (q[Y] - p[Y]) * (r[X] - q[X]) - (q[X] - p[X]) * (r[Y] - q[Y]);
 
            if (abs(val) < 1E-10)
                return 0;  // collinear
 
            return (val > 0) ? 1 : 2; // clock or counterclock wise
        }

        static inline bool onSegment(const Vertex<2, t_type>& p, const Vertex<2, t_type>& q, const Vertex<2, t_type>& r)
        {
            if (q[X] <= getMax(p[X], r[X]) && q[X] >= getMin(p[X], r[X]) &&
                q[Y] <= getMax(p[Y], r[Y]) && q[Y] >= getMin(p[Y], r[Y]))
                return true;
 
            return false;
        }

        static inline bool intersects(const Line& s1, const Line& s2)
        {
            int o1 = orientation(s1.vertex(A), s1.vertex(B), s2.vertex(A));
            int o2 = orientation(s1.vertex(A), s1.vertex(B), s2.vertex(B));
            int o3 = orientation(s2.vertex(A), s2.vertex(B), s1.vertex(A));
            int o4 = orientation(s2.vertex(A), s2.vertex(B), s1.vertex(B));
 
            // General case
            if (o1 != o2 && o3 != o4)
                return true;
 
            // Special Cases
            if (o1 == 0 && onSegment(s1.vertex(A), s2.vertex(A), s1.vertex(B))) return true;
 
            // p1, q1 and q2 are collinear and q2 lies on segment p1q1
            if (o2 == 0 && onSegment(s1.vertex(A), s2.vertex(B), s1.vertex(B))) return true;
 
            // p2, q2 and p1 are collinear and p1 lies on segment p2q2
            if (o3 == 0 && onSegment(s2.vertex(A), s1.vertex(A), s2.vertex(B))) return true;
 
            // p2, q2 and q1 are collinear and q1 lies on segment p2q2
            if (o4 == 0 && onSegment(s2.vertex(A), s1.vertex(B), s2.vertex(B))) return true;
 
            return false; // Doesn't fall in any of the above cases
 
            /*const t_type TOLERANCE = 1e-10;
 
            Vertex<2, t_type> p1 = s1.vertex(A);
            Vertex<2, t_type> p2 = s2.vertex(A);
            Vertex<2, t_type> d1 = s1.ray();
            Vertex<2, t_type> d2 = s2.ray();
 
            if (abs(cross(d1, d2)) < 1e-30)
            {
                return false;
            }
            t_type t1 = cross(p2 - p1, d2) / cross(d1, d2);
 
            if ((t1 < (0.0f - TOLERANCE)) || (t1 > (1.0f + TOLERANCE)))
            {
                return false;
            }
            Vertex<2, fltp08> intersection_vertex = p1 + d1 * t1;
 
            t_type t2 = dot(intersection_vertex - p2, s2.vertex(B) - p2);
 
            if (t2 < (0.0f - TOLERANCE) || t2 / distanceSquared(s2.vertex(B).as<2, fltp08>(), p2) >= 1.0f - TOLERANCE)
            {
                return false;
            }
            return true;*/
 
            // Find the four orientations needed for general and
        }

        static uint04 mod(int x, int m)
        {
            if (x < 0)
                return m + x;
            else
                return x % m;
        }

        static t_type CalcHandedness(const t_vertex& v1, const t_vertex& v2, const t_vertex& v3)
        {
            t_vertex edge1 = v2 - v1;
            t_vertex edge2 = v3 - v2;
 
            return cross(edge1, edge2);
        }

        void flipPolygon(VertexArray& _verts)
        {
            _verts.flip();
        }

        static bool checkIfRightHanded(const VertexArray& _verts)
        {
            if (_verts.vertexCount() <= 2)
                return false;
            return !_verts.hasClockwiseWinding();
        }
 
 

        static bool isVertexInCone(const Vector<2, t_type>& ls1, const Vector<2, t_type>& ls2, const Vector<2, t_type>& origin, const Vector<2, t_type>& vert)
        {
            Vector<2, t_type> relativePos = vert - origin;
 
            t_type ls1Product = cross(relativePos, ls1);
            t_type ls2Product = cross(relativePos, ls2);
 
            if (ls1Product < t_type(0) && ls2Product > t_type(0))
                return true;
 
            return false;
        }
 
        typedef Buffer<uint04> IntArray; 

        static IntArray findVerticesInCone(const Vector<2, t_type>& ls1, const Vector<2, t_type>& ls2, const Vector<2, t_type>& origin, VertexArray const& inputVerts)
        {
            IntArray result;
            for (uint04 i = 0; i < inputVerts.vertexCount(); ++i)
            {
                if (isVertexInCone(ls1, ls2, origin, inputVerts.vertex(i).template as<2, t_type>()))
                    result.add(i);
            }
            return result;
        }

        static bool checkVisibility(const VertexArray& vertices, uint04 origin, uint04 vertex_to_check)
        {
            lib_assert(origin != vertex_to_check, "Bad visibility check");
            Line ls(vertices.vertex(vertex_to_check).template as<2, t_type>(), vertices.vertex(origin).template as<2, t_type>());
 
            const uint04 vertex_count = vertices.vertexCount();
            for (uint04 i = 0; i < vertex_count; ++i)
            {
                if (origin == i || ((i + 1) % vertex_count) == origin)
                    continue;
                if (vertex_to_check == i || ((i + 1) % vertex_count) == vertex_to_check)
                    continue;
                Line l = vertices.edge(i).template as<2, t_type>();
                if (intersects(ls, l))
                    return false;
            }
            auto center = ls.center();
            if (!vertices.contains(center))
                return false;
            //lib_assert(result.size() >= 1, "Bad intersection");
            return true;
        }

        static uint04 IsReflexPoint(uint04 index, const VertexArray& vertices)
        {
            const t_vertex& prevVert = vertices.vertex(mod(cast<sint04>(index - 1), vertices.vertexCount()));
            const t_vertex& currVert = vertices.vertex(index);
            const t_vertex& nextVert = vertices.vertex(mod(index + 1, vertices.vertexCount()));
            return CalcHandedness(prevVert, currVert, nextVert) < 0.0f;
 
        }

        static uint04 getBestVertexToConnect(const IntArray& indices, const VertexArray& polygonVertices, const Vector<2, t_type>& origin, uint04 origin_index)
        {
            uint04 vert_size = polygonVertices.vertexCount();
 
            t_type min_distance = Constant<t_type>::Max;
            uint04 min = Constant<uint04>::Invalid;
 
            if (indices.size() == 1)
            {
                if (checkVisibility(polygonVertices, origin_index, indices[0]))
                    return indices[0];
            }
            else if (indices.size() > 1)
            {
                //first try all reflex points to potentially eliminate 2 at once
                for (uint04 i = 0; i < indices.size(); ++i)
                {
                    if (indices[i] < origin_index)
                        continue;//can't be reflex point
                    if (!IsReflexPoint(indices[i], polygonVertices))
                        continue;
                    if (checkVisibility(polygonVertices, origin_index, indices[i]))
                    {
                        return indices[i];
                    }
                    else
                    {
                        t_type currDistance = distanceSquared(polygonVertices.vertex(i).template as<2, t_type>(), origin);
                        if (currDistance * 1000 < min_distance)
                        {
                            min_distance = currDistance;
                            min = indices[i];
                        }
                    }
                }
                for (uint04 i = 0; i < indices.size(); ++i)
                {
                    if (indices[i] > origin_index && IsReflexPoint(indices[i], polygonVertices))
                        continue;//we already know these aren't visible
                    if (checkVisibility(polygonVertices, origin_index, indices[i]))
                    {
                        return indices[i];
                    }
                    else
                    {
                        t_type currDistance = distanceSquared(polygonVertices.vertex(i).template as<2, t_type>(), origin);
                        if (currDistance * 1000 < min_distance)
                        {
                            min_distance = currDistance;
                            min = indices[i];
                        }
                    }
                }
            }
            uint04 neighbor_a = mod(cast<sint04>(origin_index) - 1, vert_size);
            uint04 neighbor_b = mod(cast<sint04>(origin_index) + 1, vert_size);
 
            for (uint04 i = origin_index + 2; i < polygonVertices.vertexCount(); ++i)
            {
                if (i == neighbor_a)
                    continue;
                if (indices.contains(i))
                    continue;
                if (IsReflexPoint(i, polygonVertices) && checkVisibility(polygonVertices, origin_index, i))
                    return i;
            }
 
            for (uint04 i = 0; i < polygonVertices.vertexCount(); ++i)
            {
                if (i == origin_index || i == neighbor_a || i == neighbor_b)
                    continue;
                if (i > origin_index && IsReflexPoint(i, polygonVertices))
                    continue;
                t_type currDistance = distanceSquared(polygonVertices.vertex(i).template as<2, t_type>(), origin);
                if (currDistance < min_distance)
                {
                    if (indices.contains(i))
                        continue;
                    if (checkVisibility(polygonVertices, origin_index, i))
                    {
                        min_distance = currDistance;
                        min = i;
                    }
                    else if(currDistance * 1000 < min_distance)
                    {
                        min_distance = currDistance;
                        min = i;
                    }
                }
 
            }
            //lib_assert(IsValid(max), "Bad max");
            return min;
        }
 
 

        static uint04 findFirstReflexVertex(const VertexArray& _vertices)
        {
            lib_assert(checkIfRightHanded(_vertices), "Should be right handed");
            for (uint04 i = 0; i < _vertices.vertexCount(); ++i)
            {
                if (IsReflexPoint(i, _vertices))
                    return i;
            }
 
            return Constant<uint04>::Invalid;
        }

        void flipPolygon()
        {
            flipPolygon(vertices);
        }
 
        typedef Dictionary<uint04, Vertex<2, fltp08>> VertexIntMap; 
        typedef std::pair<int, Vertex<2, fltp08>> VertexIntPair; 
 
 
    public:
        ConcavePolygon(const VertexArray& _vertices)
            : vertices{ _vertices }
        {
            if (vertices.vertexCount() >= 3)
                if (checkIfRightHanded() == false)
                    flipPolygon();
        }

        ConcavePolygon() {}

        bool checkIfRightHanded()
        {
            return checkIfRightHanded(vertices);
        }

        void convexDecomp()
        {
            Bounds<2, t_type> bounds = Constant<Bounds<2, t_type>>::Min;
            for (uint04 i = 0; i < vertices.vertexCount(); i++)
            {
                bounds.addToBounds(vertices.vertex(i).template as<2, t_type>());
            }
            Buffer<VertexArray> sub_polygons_to_process;
            Buffer<VertexArray> next_sub_polygons_to_process = { vertices };
            std::mutex mtx;
            while (next_sub_polygons_to_process.size() > 0)
            {
                sub_polygons_to_process.clear();
                for (uint04 i = 0; i < next_sub_polygons_to_process.size(); i++)
                {
                    if (next_sub_polygons_to_process[i].vertexCount() > 3)
                        sub_polygons_to_process.add(next_sub_polygons_to_process[i]);
                    else if(next_sub_polygons_to_process[i].vertexCount() == 3)
                        processed_polygons.add(next_sub_polygons_to_process[i]);
                }
                next_sub_polygons_to_process.clear();
                next_sub_polygons_to_process.setSize(2 * sub_polygons_to_process.size());
                int size = sub_polygons_to_process.size();
                _parallel_for(int i = 0; i < size; i++)
                {
                    std::unique_lock<std::mutex> lck(mtx, std::defer_lock);
                    const VertexArray& _vertices = sub_polygons_to_process[cast<uint04>(i)];
                    lib_assert(checkIfRightHanded(_vertices), "undexpected handiness");
                    uint04 reflexIndex = findFirstReflexVertex(_vertices);
                    if (IsInvalid(reflexIndex))
                    {
                        lib_assert(_vertices.isConvex(), "Should be convex");
                        continue;
                    }
                    Vertex<2, t_type> prevVertPos = _vertices.vertex(mod(cast<sint04>(reflexIndex) - 1, _vertices.vertexCount())).template as<2, t_type>();
                    Vertex<2, t_type> currVertPos = _vertices.vertex(reflexIndex).template as<2, t_type>();
                    Vertex<2, t_type> nextVertPos = _vertices.vertex(mod(reflexIndex + 1, _vertices.vertexCount())).template as<2, t_type>();
 
                    Vector<2, t_type> prev_ray = (currVertPos - prevVertPos);
                    Vector<2, t_type> next_ray = (currVertPos - nextVertPos);
 
                    IntArray vertsInCone = findVerticesInCone(prev_ray, next_ray, currVertPos, _vertices);
 
                    uint04 bestVert = getBestVertexToConnect(vertsInCone, _vertices, currVertPos, reflexIndex);
                    if (IsInvalid(bestVert))
                        continue;
                    lib_assert(IsValid(bestVert), "Not sure why this is true");
                    uint04 start = getMin(bestVert, reflexIndex);
                    uint04 end = getMax(bestVert, reflexIndex);
                    next_sub_polygons_to_process[2 * i + 0].addVertices(_vertices.begin(), start + 1);
                    next_sub_polygons_to_process[2 * i + 1].addVertices(_vertices.begin() + start, (end + 1) - start);
                    next_sub_polygons_to_process[2 * i + 0].addVertices(_vertices.begin() + end, _vertices.vertexCount() - end);
                    sub_polygons_to_process[cast<uint04>(i)].clear();//indicate we are finished processing
 
                    //add to polygons
                    lib_assert(next_sub_polygons_to_process[2 * i + 0].vertexCount() > 2, "Bad a size");
                    lib_assert(next_sub_polygons_to_process[2 * i + 1].vertexCount() > 2, "Bad b size");
                    lib_assert(checkIfRightHanded(next_sub_polygons_to_process[2 * i + 0]), "Vertex A not right");
                    lib_assert(checkIfRightHanded(next_sub_polygons_to_process[2 * i + 1]), "Vertex B not right");
                }
                //gather all polygons that are convex
                for (uint04 i = 0; i < sub_polygons_to_process.size(); i++)
                {
                    if (sub_polygons_to_process[i].vertexCount() > 3)
                    {
                        lib_assert(sub_polygons_to_process[i].isConvex(), "Should be convex");
                        processed_polygons.add(sub_polygons_to_process[i]);
                    }
                }
            }
        }

        const VertexArray& getVertices() const
        {
            return vertices;
        }
 

        void returnLowestLevelPolys(Buffer<Polygon<t_type, t_vertex>>& returnArr)
        {
            returnArr.addAll(processed_polygons);
        }

        t_vertex getPoint(uint04 index) const
        {
            if (index >= 0 && index < vertices.size())
                return vertices[index];
            lib_assert(false, "Bad vertex get");
            return t_vertex(0.0f);
        }

        int getPointCount() const
        {
            return vertices.vertexCount();
        }
    };
 
}