toys/gps/docs/source/math/ns_geom_math.cpp

#include "basic/basic.h"
#include "geo/private/math/math.h"
#include "geo/private/math/angle.h"
#include "geo/private/math/distance.h"
#include "geo/private/math/ns_geom_math.h"

#ifdef OS_WINDOWS
#include <time.h>
#elif defined(OS_LINUX)
#include <sys/time.h>
#endif

#include <cfloat>
#include <cmath>
#include <cstdint>
#include <ctime>
#include <limits>
#include <string>

using namespace std;

namespace ns
{
namespace geo
{

static double g_maxLon = 180.0;
static double g_minLon = -180.0;
static double g_maxLat = 90.0;
static double g_minLat = -90.0;
double clipLongitude(double lon)
{
    return max(g_minLon, min(g_maxLon, lon));
}
double clipLatitude(double lat)
{
    return max(g_minLat, min(g_maxLat, lat));
}

bool segmentIntersection(const PointD& p1, const PointD& p2, const PointD& q1,
                         const PointD& q2)
{
    if (max(p1.x, p2.x) < min(q1.x, q2.x) || max(q1.x, q2.x) < min(p1.x, p2.x) ||
        max(p1.y, p2.y) < min(q1.y, q2.y) || max(q1.y, q2.y) < min(p1.y, p2.y))
        return false;

    double res1 = (p1.x - p2.x) * (q1.y - p2.y) - (p1.y - p2.y) * (q1.x - p2.x);
    double res2 = (p1.x - p2.x) * (q2.y - p2.y) - (p1.y - p2.y) * (q2.x - p2.x);
    double res3 = (q1.x - q2.x) * (p1.y - q2.y) - (q1.y - q2.y) * (p1.x - q2.x);
    double res4 = (q1.x - q2.x) * (p2.y - q2.y) - (q1.y - q2.y) * (p2.x - q2.x);

    return res1 * res2 <= 0 && res3 * res4 <= 0;
}

bool segmentIntersectionWithRect(const PointD& p1, const PointD& p2, const RectD& rct)
{
    if (rct.contain(p1) || rct.contain(p2))
        return true;

    PointD q1(rct.minX, rct.minY);
    PointD q2(rct.minX, rct.maxY);
    PointD q3(rct.maxX, rct.maxY);
    PointD q4(rct.maxX, rct.minY);

    return segmentIntersection(p1, p2, q1, q2) || segmentIntersection(p1, p2, q2, q3) ||
           segmentIntersection(p1, p2, q3, q4) || segmentIntersection(p1, p2, q4, q1);
}

int judgeShapePointDirectionBothEnds(const std::vector<PointD>& line1,
                                     const std::vector<PointD>& line2)
{
    if (line1.size() >= 2 && line2.size() >= 2)
    {
        const PointD& p1 = line1[0];
        const PointD& p2 = line1[1];
        const PointD& p3 = line1[line1.size() - 2];
        const PointD& p4 = line1[line1.size() - 1];

        const PointD& q1 = line2[0];
        const PointD& q2 = line2[1];
        const PointD& q3 = line2[line2.size() - 2];
        const PointD& q4 = line2[line2.size() - 1];
        // head two points
        double hAngle1 = cal_east_angle(p1, p2);
        double hAngle2 = cal_east_angle(q1, q2);
        double hDiff = diff_angle(hAngle1, hAngle2);
        // tail two points
        double tAngle1 = cal_east_angle(p3, p4);
        double tAngle2 = cal_east_angle(q3, q4);
        double tDiff = diff_angle(tAngle1, tAngle2);
        // 开始的方向和结尾的角度相差小于90度,方向就认为是相同的
        if (hDiff < 90 && tDiff < 90)
        {
            return 1;
        }
        // first and last points
        double angle1 = cal_east_angle(p1, p4);
        double angle2 = cal_east_angle(q1, q4);
        double diff = diff_angle(angle1, angle2);
        // 或者两条线的第一个点与最后一个点的连线角度小于90，方向就认为是相同的
        return diff < 90 ? 1 : -1;
    }
    return 0;
}

double calProj2Vector(const PointD& pos, const PointD& p1, const PointD& p2,
                      PointD& proj, int& inVector)
{
    double n_x = p2.x - p1.x;
    double n_y = p2.y - p1.y;
    double x_x = pos.x - p1.x;
    double x_y = pos.y - p1.y;

    if (p1 == p2)
    {
        proj = p1;
        inVector = 0;
        return pos.distance(proj);
    }

    double proj_norm = (x_x * n_x + x_y * n_y) / (n_x * n_x + n_y * n_y);
    double proj_x = proj_norm * n_x;
    double proj_y = proj_norm * n_y;
    proj.x = p1.x + proj_x;
    proj.y = p1.y + proj_y;

    inVector = proj_norm >= 0 && proj_norm <= 1;

    double dis = std::sqrt((proj_x - x_x) * (proj_x - x_x) + (proj_y - x_y) * (proj_y - x_y));
    int32_t side = JudgeSideness(p1, p2, pos);
    if (side < 0)
    {
        dis *= -1;
    }
    return dis;
}

bool projInVector(const PointD& pos, const PointD& p1, const PointD& p2)
{
    double n_x = p2.x - p1.x;
    double n_y = p2.y - p1.y;
    double x_x = pos.x - p1.x;
    double x_y = pos.y - p1.y;
    double proj_norm = (x_x * n_x + x_y * n_y);
    double length = (n_x * n_x + n_y * n_y);
    return proj_norm >= 0 && proj_norm <= length;
}

PointD pointInSegment(const PointD& p1, const PointD& p2, double s,
                      double length)
{
    PointD pos;

    double len;
    if (length > 0)
        len = length;
    else
        len = sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y));

    if (len > 0 && s >= 0 && s <= len)
    {
        double ratio = s / len;
        pos.x = p1.x + ratio * (p2.x - p1.x);
        pos.y = p1.y + ratio * (p2.y - p1.y);
        pos.z = p1.z + ratio * (p2.z - p1.z);
    }

    return pos;
}

bool pointInPolygon(const PointD& p, const std::vector<PointD>& pts)
{
    double x = p.x;
    double y = p.y;
    int polySides = static_cast<int>(pts.size());
    int i, j = polySides - 1;
    bool oddNodes = false;

    for (i = 0; i < polySides; i++)
    {
        // 只针对平面比较是否在polygon中, 没有对z值作比较
        // if (pts[i] == p)
        if (are_equal(pts[i].x, p.x) && are_equal(pts[i].y, p.y))
            return true;

        if (pts[i].x <= x || pts[j].x <= x)
        {
            // if已经保证下面的除数不会为0
            if ((pts[i].y < y && pts[j].y >= y) ||
                (pts[j].y < y && pts[i].y >= y)) // y大于等于都认为大于
            {
                oddNodes ^=
                    (pts[i].x + (y - pts[i].y) / (pts[j].y - pts[i].y) * (pts[j].x - pts[i].x) < x);
            }
        }

        j = i;
    }

    return oddNodes;
}

static PointD _HadMath_getRelatedPoint(const PointD* p, double distance, double angle)
{
    PointD dst;
    // Point64_invalidate(&dst);

    static const double Ea = 6378137; //   赤道半径
    static const double Eb = 6356725; //   极半径

    double lon = p->x;
    double lat = p->y;

    //正北朝上
    double dx = distance * sin(angle * c_pi() / 180.0);
    double dy = distance * cos(angle * c_pi() / 180.0);
    ////正东朝上
    // double dx = distance * cos(angle * c_pi() / 180.0);
    // double dy = distance * sin(angle * c_pi() / 180.0);

    // double ec = 6356725 + 21412 * (90.0 - GLAT) / 90.0;
    // 21412 是赤道半径与极半径的差

    // ec:修正因为纬度不断变化的球半径长度，ed:lat所在纬度的纬度圈的半径
    double ec = Eb + (Ea - Eb) * (90.0 - lat) / 90.0;
    double ed = ec * cos(lat * c_pi() / 180);

    double dstLon = (dx / ed + lon * c_pi() / 180.0) * 180.0 / c_pi();
    double dstLat = (dy / ec + lat * c_pi() / 180.0) * 180.0 / c_pi();

    dst.x = dstLon;
    dst.y = dstLat;

    return dst;
}

bool calcCircleCenter(double curvature, double heading, const PointD* position,
                      PointD* center)
{
    if ((position == NULL) || (center == NULL))
    {
        return false;
    }

    if (curvature == 0.0)
    {
        return false;
    }

    if (curvature < 0)
    {
        heading += 90;
    }
    else
    {
        heading -= 90;
    }

    *center = _HadMath_getRelatedPoint(position, std::abs(1.0 / curvature), heading);

    return true;
}

PointD getPointInTangentLine(double heading, const PointD* pt, double dis)
{
    double arc = heading * c_pi() / 180;
    return PointD(pt->x + dis * cos(arc), pt->y + dis * sin(arc));
}
namespace
{
// 不对外接口，写到这个命名空间里面
bool genCenterLine(const std::vector<PointDST>& l, const std::vector<PointDST>& r,
                   std::vector<PointD>& c)
{
    auto getXYByS = [&](const std::vector<PointDST>& position, double s) -> PointDST {
        // 获取position上s值对应点处的xy笛卡尔坐标
        PointDST pp;
        if (!position.empty())
        {
            double length = position[position.size() - 1].s;
            if (s >= 0 && s <= length)
            {
                for (size_t i = 1; i < position.size(); i++)
                {
                    if (s <= position[i].s)
                    {
                        pp.p =
                            pointInSegment(position[i - 1].p, position[i].p, s - position[i - 1].s,
                                           position[i].s - position[i - 1].s);
                        pp.s = s;
                        pp.t = 0;
                        break;
                    }
                }
            }
        }

        return pp;
    };

    c.clear();
    if (l.size() >= 2 && r.size() >= 2)
    {
        double len1 = l[l.size() - 1].s;
        double len2 = r[r.size() - 1].s;
        if (len1 > 0 && len2 > 0)
        {
            for (auto& p : l)
            {
                double len = p.s / len1 * len2;
                auto q = getXYByS(r, len);

                PointD cp((p.p.x + q.p.x) / 2.0, (p.p.y + q.p.y) / 2.0,
                          (p.p.z + q.p.z) / 2.0);
                c.emplace_back(cp);
            }
        }
        return true;
    }
    return false;
}
} // namespace

bool genCenterLine(const std::vector<PointD>& l, const std::vector<PointD>& r,
                   std::vector<PointD>& c)
{
    auto computeS = [&](const std::vector<PointD>& pts,
                        std::vector<PointDST>& position) -> double {
        PointDST pos;
        pos.p = pts[0];
        pos.t = pos.s = 0;
        position.emplace_back(pos);

        for (size_t i = 1; i < pts.size(); i++)
        {
            pos.p = pts[i];
            pos.t = 0;
            PointD pre = position[i - 1].p;
            PointD cur = pos.p;
            pos.s = cur.distance(pre) + position[i - 1].s;

            position.emplace_back(pos);
        }

        return position[position.size() - 1].s;
    };

    if (l.size() >= 2 && r.size() >= 2)
    {
        std::vector<PointDST> position1, position2;
        computeS(l, position1);
        computeS(r, position2);
        position1.size() > position2.size() ? genCenterLine(position1, position2, c)
                                            : genCenterLine(position2, position1, c);
        return true;
    }

    return false;
}

} // namespace geo
} // namespace ns