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

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

#include <string.h>

#include <algorithm>
#include <cfloat>
#include <climits>
#include <cmath>
#include <execution>
#include <iomanip>
#include <iostream>
#include <regex>
#include <stdexcept>
#include <vector>

#include "_Define.h"
#include "core/String.h"
#include "utils/EssentiallyEqual.h"

namespace ais
{
    inline int dcmp(double x, double eps = DBL_EPSILON)
    {
        if (fabs(x) < eps)
            return 0;
        else
            return x < 0 ? -1 : 1;
    };

    template <typename dtype = double>
    inline bool between(dtype x, dtype a, dtype b)
    {
        return (a <= x && x <= b) || (a >= x && x >= b);
    };

    struct AIS_EXPORT Point2D
    {
    public:
        double x{0};
        double y{0};

        Point2D() : x(std::nan("1")), y(std::nan("1")) {}
        Point2D(double x_, double y_) : x(x_), y(y_) {}
        Point2D(const std::pair<double, double> &p) : x(p.first), y(p.second) {}
        Point2D(const char *buffer)
        {
            std::string line(buffer);
            auto tokens = ais::str_split_all(line, ",");

            if (!tokens.empty())
            {
                x = tokens.size() >= 2 ? std::stod(tokens[0]) : std::nan("1");
                y = tokens.size() >= 2 ? std::stod(tokens[1]) : std::nan("1");
            }
        }

        inline double distance(const Point2D &p) const
        {
            return std::sqrt((x - p.x) * (x - p.x) + (y - p.y) * (y - p.y));
        }

        inline double distance(const std::pair<double, double> &p) const
        {
            return std::sqrt((x - p.first) * (x - p.first) + (y - p.second) * (y - p.second));
        }

        inline double distance(double px, double py) const
        {
            return std::sqrt((x - px) * (x - px) + (y - py) * (y - py));
        }

        inline double distance2D(const Point2D &p) const
        {
            return distance(p);
        }

        inline bool operator<(const Point2D &rhs) const
        {
            return x < rhs.x - 1e-6 || (std::abs(x - rhs.x) < 1e-6 && y < rhs.y);
        }
        inline bool operator==(const Point2D &rhs) const
        {
            // return utils::essentially_equal(x, rhs.x, std::numeric_limits<double>::epsilon()) &&
            //        utils::essentially_equal(y, rhs.y, std::numeric_limits<double>::epsilon());

            return (std::abs(x - rhs.x) < 1e-6 && std::abs(y - rhs.y) < 1e-6);
        }

        inline Point2D operator+(const Point2D &rhs) const
        {
            return Point2D(x + rhs.x, y + rhs.y);
        }

        inline Point2D operator-(const Point2D &rhs) const
        {
            return Point2D(x - rhs.x, y - rhs.y);
        }

        inline double operator*(const Point2D &rhs) const
        {
            return x * rhs.x + y * rhs.y;
        }

        // Cross product
        // P^Q > 0, P in the clockwise direction of Q
        // < 0, P in the counterclockwise direction of Q
        // = 0, P and Q Co-linear, possibly in the same or opposite direction
        inline double operator^(const Point2D &rhs) const
        {
            return x * rhs.y - rhs.x * y;
        }

        inline bool has_value() const
        {
            return !(std::isnan(x) || std::isnan(y));
        }

        inline bool isNan() const
        {
            return std::isnan(x) || std::isnan(y);
        }

        //============================================================================
        /// Test point in the line of point p1 and p2, use quick compare
        ///
        /// @param p1
        /// @param p2
        ///
        /// @return bool
        ///
        inline bool in_line(const Point2D &p1, const Point2D &p2, bool segment = true) const
        {
            bool inSegment = segment ? (between(x, p1.x, p2.x) && between(y, p1.y, p2.y)) : true;

            if (!inSegment)
                return false;

            auto d1 = (x - p1.x) * (p2.y - y);
            auto d2 = (y - p1.y) * (p2.x - x);
            // bool inLine = utils::essentially_equal(d1, d2);
            bool inLine = abs(d1 - d2) < 1e-6;

            return inLine;
        }

        template <typename PT>
        inline bool in_box(const std::vector<PT> &points) const
        {
            double xMin = std::nan("1");
            double xMax = std::nan("1");
            double yMin = std::nan("1");
            double yMax = std::nan("1");

            if (points.size() == 5)
            {
                auto &pointMin = points[0];
                auto &pointMax = points[2];
                xMin = pointMin.x;
                xMax = pointMax.x;
                yMin = pointMin.y;
                yMax = pointMax.y;
            }
            else
            {
                std::vector<double> xElements, yElements;

                std::for_each(std::execution::par_unseq, points.begin(), points.end(), [&xElements](const PT &p)
                              { xElements.push_back(p.x); });
                std::for_each(std::execution::par_unseq, points.begin(), points.end(), [&yElements](const PT &p)
                              { yElements.push_back(p.y); });

                auto xMinMax = std::minmax_element(std::execution::par_unseq, xElements.begin(), xElements.end());
                auto yMinMax = std::minmax_element(std::execution::par_unseq, yElements.begin(), yElements.end());

                xMin = *xMinMax.first;
                xMax = *xMinMax.second;
                yMin = *yMinMax.first;
                yMax = *yMinMax.second;
            }

            bool betweenX = (xMin <= x) && (x <= xMax);
            bool betweenY = (yMin <= y) && (y <= yMax);

            return betweenX && betweenY;
        }

        template <typename PT>
        inline bool in_box(const PT &p_min, const PT &p_max) const
        {
            double xMin = p_min.x;
            double xMax = p_max.x;
            double yMin = p_min.y;
            double yMax = p_max.y;

            bool betweenX = (xMin <= x) && (x <= xMax);
            bool betweenY = (yMin <= y) && (y <= yMax);

            return betweenX && betweenY;
        }

        template <typename PT>
        inline bool in_polylines(const std::vector<PT> &points) const
        {
            if (points.size() < 2)
                return false;

            auto s = points.size();
            for (int i = 0; i < s - 1; i++)
            {
                auto p1 = points[i];
                auto p2 = points[i + 1];

                if (in_line(p1, p2))
                    return true;
            }

            return false;
        }

        //============================================================================
        /// Test point in the polygon of points
        ///
        /// @param points, which to build polygon, included 1 duplicates point to build a closed polygon
        /// @param controlPoints, test point in fault loose or strict, if >0 the point's corner points in fault must >= controlPoints
        ///                       the node can be identified to a fault node
        /// @param xSize, ySize: grid space size in x y coordinate
        ///
        /// @return bool
        ///
        template <class PT = Point2D>
        bool in_polygon(const std::vector<PT> &points, const size_t controlPoints = 0, const double xSize = 0.0, const double ySize = 0.0) const
        {
            if (points.size() < 2)
            {
                // single point can't build a polygon
                return false;
            }
            else if (points.size() == 2)
            {
                return in_line(points[0], points[1]);
            }
            else
            {
                // fault point box, used to control the fault point in fault or not
                std::vector<PT> faultPointBox = {
                    {x - 0.5 * xSize, y - 0.5 * ySize},
                    {x - 0.5 * xSize, y + 0.5 * ySize},
                    {x + 0.5 * xSize, y + 0.5 * ySize},
                    {x + 0.5 * xSize, y - 0.5 * ySize}};
                bool flag = false;
                bool boxPointFlags[4] = {false * 4};

                // use ray casting algorithm
                // for (auto iter = points.begin(); iter != points.end() - 1; ++iter)
                for (auto i = 0; i < points.size() - 1; i++)
                {
                    auto p1 = points[i];
                    auto p2 = points[i + 1];

                    if (in_line(p1, p2) && controlPoints == 0)
                    {
                        // only loose test return turn while node on fault line
                        return true;
                    }

                    if ((dcmp(p1.y - y) > 0 != dcmp(p2.y - y) > 0) && dcmp(x - (y - p1.y) * (p1.x - p2.x) / (p1.y - p2.y) - p1.x) < 0)
                    {
                        flag = !flag;
                    }

                    if (controlPoints > 0)
                    {
                        for (int fbi = 0; fbi < 4; fbi++)
                        {
                            // test the fault box for fault point
                            auto p = faultPointBox[fbi];
                            if ((dcmp(p1.y - p.y) > 0 != dcmp(p2.y - p.y) > 0) && dcmp(p.x - (p.y - p1.y) * (p1.x - p2.x) / (p1.y - p2.y) - p1.x) < 0)
                            {
                                boxPointFlags[fbi] = !boxPointFlags[fbi];
                            }
                        }
                    }
                }

                if (controlPoints > 0)
                {
                    size_t hits = 0;
                    for (int i = 0; i < 4; i++)
                    {
                        if (boxPointFlags[i])
                            hits++;
                    }
                    flag = flag && (hits >= controlPoints);
                };

                return flag;
            }

            return false;
        }

        inline std::string str() const
        {
            std::stringstream ss;
            ss << std::fixed << std::setprecision(4);
            ss << "(" << x << ", " << y << ")";
            return ss.str();
        }

        inline size_t hash() const
        {

            std::stringstream ss;
            ss << std::fixed << std::setprecision(6) << x << "," << y;
            std::string s = ss.str();
            size_t h = std::hash<std::string>{}(s);

            return h;
        }

        inline size_t hash2D() const
        {
            return hash();
        }

        friend std::ostream &operator<<(std::ostream &stream, const Point2D &point)
        {
            stream << point.str();
            return stream;
        }
    };

    inline double distance(const Point2D &p1, const Point2D &p2) { return p1.distance(p2); }

    inline std::pair<double, double> line_slope_intercept(const Point2D &p1, const Point2D &p2)
    {
        if (abs(p2.x - p1.x) > 1e-6)
        {
            double slope = (p2.y - p1.y) / (p2.x - p1.x);
            double intercept = p1.y - slope * p1.x;

            return std::make_pair(slope, intercept);
        }
        else
            return std::make_pair(INFINITY, INFINITY);
    }

    inline Point2D intersection_point(const std::pair<double, double> &l1_si, const std::pair<double, double> &l2_si)
    {
        double x = (l2_si.second - l1_si.second) / (l1_si.first - l2_si.first);
        double y = l1_si.first * x + l1_si.second;

        return Point2D(x, y);
    }

    struct AIS_EXPORT Point3D : public ais::Point2D
    {
    public:
        double z{0};

        Point3D() : z(std::nan("1")) {}
        Point3D(const Point2D &p) : Point2D(p), z(INFINITY) {}
        Point3D(const Point2D &p, double z) : Point2D(p), z(z) {}
        Point3D(double x_, double y_) : Point2D(x_, y_), z(INFINITY) {}
        Point3D(double x_, double y_, double z_) : Point2D(x_, y_), z(z_) {}
        Point3D(const char *buffer)
        {
            char temp_buff[64] = { 0 };
            std::string temp_str;
            double ret[3] = { std::nan("1"), std::nan("1") , std::nan("1") };

            int index = 0;
            const char* buffIndex = buffer;
            try
            {
                while (*buffIndex != 0x0)
                {
                    const char* result = strchr(buffIndex, ',');
                    if (result != NULL)
                    {
                        memset(temp_buff, 0, 64);
                        strncpy(temp_buff, buffIndex, result - buffIndex);
                        temp_str = std::string(temp_buff);
                        ret[index++] = std::stod(temp_str);
                        buffIndex = result + 1;
                    }
                    else
                    {
                        if (index < 1)
                            // not a valid string
                            ret[0] = ret[1] = ret[2] = std::nan("1");
                        else
                        {
                            // finially part
                            memset(temp_buff, 0, 64);
                            strcpy(temp_buff, buffIndex);
                            temp_str = std::string(temp_buff);
                            ret[index++] = std::stod(temp_str);
                        }
                        break;
                    }
                }
            }
            catch (const std::invalid_argument& e)
            {
                //std::cout << "Build point failed: " << str << std::endl;
                ret[0] = ret[1] = ret[2] = std::nan("1");
            }

            x = ret[0]; y = ret[1]; z = ret[2];
        }

        inline double distance(const Point2D &p) const
        {
            double dx = x - p.x;
            double dy = y - p.y;
            return std::sqrt(dx * dx + dy * dy);
        }

        inline double distance(double px, double py) const
        {
            double dx = x - px;
            double dy = y - py;
            return std::sqrt(dx * dx + dy * dy);
        }

        inline double distance2D(const Point3D &p) const
        {
            return distance(p.x, p.y);
        }

        inline double distance(const Point3D &p) const
        {
            double dx = x - p.x;
            double dy = y - p.y;
            double dz = z - p.z;

            // skip delta z while some z is nan or infinity
            dz = std::isnan(dz) || std::isinf(dz) ? 0 : dz;

            return std::sqrt(dx * dx + dy * dy + dz * dz);
        }

        inline bool operator<(const Point3D &rhs) const
        {
            return (x < rhs.x - 1e-6) || (utils::essentially_equal(x, rhs.x) && y < rhs.y);
        }
        inline bool operator==(const Point3D &rhs) const
        {
            // return utils::essentially_equal(x, rhs.x, std::numeric_limits<double>::epsilon()) &&
            //        utils::essentially_equal(y, rhs.y, std::numeric_limits<double>::epsilon()) &&
            //        utils::essentially_equal(z, rhs.z, std::numeric_limits<double>::epsilon());
            return (std::abs(x - rhs.x) < 1e-6) && (std::abs(y - rhs.y) < 1e-6) && (std::abs(z - rhs.z) < 1e-6);
        }

        //============================================================================
        /// linear interpolation z value for specified point which should be on the line of 2 points
        ///
        /// @param points p1 and p2
        ///
        /// @return double point z value
        ///
        double linear_interpolation_z_value(const Point3D &p1, const Point3D &p2)
        {
            double ret = INFINITY;

            if ((p1.distance(p2) / distance(p1) < 0.0001) || (p1.distance(p2) / distance(p2) < 0.0001))
                // return infinity while the p1 and p2 are too close, but too far away target p
                return ret;

            if (in_line(p1, p2))
                // p is in the statement of p1 and p2
                ret = p1.z + (p2.z - p1.z) * distance(p1) / p1.distance(p2);
            else if (in_line(p1, p2, false))
            {
                // p is not in the statement of p1 and p2
                if (distance(p1) > distance(p2))
                {
                    ret = p2.z - (p1.z - p2.z) * distance(p2) / p1.distance(p2);
                }
                else
                {
                    ret = p1.z + (p1.z - p2.z) * distance(p1) / p1.distance(p2);
                }
            }

            z = ret;
            return ret;
        }

        //============================================================================
        /// interpolation z value for this point by other 3 points
        ///
        ///         p1                     p1
        ///         |   \                /   \
        ///   p-----+-----p3    or    p2 --p--+
        ///         |   /                \     \
        ///         p2                     \    \
        ///                                   \ p3
        ///
        /// @param points p1 p2 and p3
        ///
        /// @return double point z value
        ///
        double point_interpolate(const Point3D &p1, const Point3D &p2, const Point3D &p3)
        {
            double ret = INFINITY;

            // get the slope and intercept pairs for p1/p2 and p/p3
            auto p12_ls = line_slope_intercept(p1, p2);
            auto pp3_ls = line_slope_intercept(*this, p3);

            if (abs(p12_ls.first - pp3_ls.first) < 1e-6)
            {
                // line p1/p2 and p/p3 have same slope, which means two line are parallel,
                // that will NOT get the interpolate in this condition
                return INFINITY;
            }

            if (std::isinf(p12_ls.first) || std::isinf(pp3_ls.first))
            {
                // one line is vertical line
                if ((std::abs(p1.x - p2.x) < 1e-6) && (std::abs(p3.x - x) > 1e-6))
                {
                    // line p1--p2 is vertical line
                    auto x = p1.x;
                    auto y = pp3_ls.first * x + pp3_ls.second;
                    Point3D pi{x, y, INFINITY};
                    pi.linear_interpolation_z_value(p1, p2);

                    ret = linear_interpolation_z_value(pi, p3);
                }
                else if ((std::abs(p1.x - p2.x) > 1e-6) && (std::abs(p3.x - x) < 1e-6))
                {
                    // line p--p3 is vertical line
                    auto x = p3.x;
                    auto y = p12_ls.first * x + p12_ls.second;
                    Point3D pi{x, y, INFINITY};
                    pi.linear_interpolation_z_value(p1, p2);

                    ret = linear_interpolation_z_value(pi, p3);
                }
            }
            else
            {
                // get the intersection point for line p1/p2 and p/p3, which is on both line
                Point3D pi{intersection_point(p12_ls, pp3_ls)};
                pi.linear_interpolation_z_value(p1, p2);

                ret = linear_interpolation_z_value(pi, p3);
            }
            return ret;
        }

        //============================================================================
        /// interpolation z value for this point by point with Gaussian operation
        ///
        ///                point
        ///               /
        ///    wGaussian = exp((x * x + y * y) / (- u * weight)<29><>
        ///            /
        ///           p
        ///
        /// @param point source point
        ///
        /// @return double interpolate point z value
        ///
        inline double gaussian_interpolation_z_value(double pointDistance, double pointZ, size_t rows, size_t columns, size_t cornerWeight = 64)
        {
            double ret = INFINITY;

            if (!std::isnan(pointZ))
            {
                double u = std::sqrt(rows * rows + columns * columns);
                double weight = exp(pointDistance * pointDistance / (-u * cornerWeight));

                ret = pointZ * weight;
            }

            return ret;
        }

        inline bool has_value() const
        {
            return Point2D::has_value() && (!std::isnan(z));
        }

        inline bool isNan() const
        {
            return std::isnan(z) || std::isnan(x) || std::isnan(y);
        }

        inline std::string str() const
        {
            std::stringstream ss;
            ss << std::fixed << std::setprecision(4);
            ss << "(" << x << ", " << y << ", " << z << ")";
            return ss.str();
        }

        inline size_t hash2D() const
        {
            size_t h1 = std::hash<double>{}(x);
            size_t h2 = std::hash<double>{}(y);
            return h1 ^ (h2 << 1);
        }

        inline size_t hash() const
        {
            size_t h1 = std::hash<double>{}(x);
            size_t h2 = std::hash<double>{}(y);
            size_t h3 = std::hash<double>{}(z);
            return h1 ^ (h2 << 1) ^ (h3 << 2);
        }

        friend std::ostream &operator<<(std::ostream &stream, const Point3D &point)
        {
            stream << point.str();
            return stream;
        }
    };

    using Point = AIS_EXPORT ais::Point2D;
    using PointXYZ = AIS_EXPORT ais::Point3D;

} // namespace ais