HdLibMath.cpp

#include "HdLibMath.h"

#include <algorithm>

#include "CoordSys.h"

using namespace jf;

/**
 * 判断一个点 是否在左右边界点所组成的区域内
 *
 * @param ptPoint
 * @param cvctvdLeftEdgeGeo
 * @param cvctvdRightEdgeGeo
 * @param isReverseFlag  左右边界点是否是逆向存储 逆向存储则为true
 * @return
 */
bool HdLibMath::IsInRecArea(const Point &ptPoint, const std::vector<std::vector<double>> &cvctvdLeftEdgeGeo, const std::vector<std::vector<double>> &cvctvdRightEdgeGeo, bool isReverseFlag) {
    int nRectCnt = std::min(cvctvdLeftEdgeGeo.size(), cvctvdRightEdgeGeo.size()) - 1;
    int nTriCnt = std::abs((int)cvctvdLeftEdgeGeo.size() - (int)cvctvdRightEdgeGeo.size());

    bool bIsInArea = false;

    XYZ xyz0 = P2XYZ(ptPoint);

    // 先将edge分成一个个的小矩形, 然后柴分成两个三角形
    for (int i = 0; (i < nRectCnt) && (false == bIsInArea); ++i) {
        if (!isReverseFlag) {
            XYZ xyz1 = P2XYZ(cvctvdLeftEdgeGeo[i]);
            XYZ xyz2 = P2XYZ(cvctvdLeftEdgeGeo[i + 1]);
            XYZ xyz3 = P2XYZ(cvctvdRightEdgeGeo[i + 1]);
            XYZ xyz4 = P2XYZ(cvctvdRightEdgeGeo[i]);
            bool bIsInArea1 = IsInTriangle(xyz1, xyz2, xyz4, xyz0);
            bool bIsInArea2 = IsInTriangle(xyz2, xyz3, xyz4, xyz0);
            if (bIsInArea1 || bIsInArea2) return true;
        } else {
            XYZ xyz1 = P2XYZ(cvctvdLeftEdgeGeo[cvctvdLeftEdgeGeo.size() - 1 - i]);
            XYZ xyz2 = P2XYZ(cvctvdLeftEdgeGeo[cvctvdLeftEdgeGeo.size() - 1 - 1 - i]);
            XYZ xyz3 = P2XYZ(cvctvdRightEdgeGeo[i + 1]);
            XYZ xyz4 = P2XYZ(cvctvdRightEdgeGeo[i]);
            bool bIsInArea1 = IsInTriangle(xyz1, xyz2, xyz4, xyz0);
            bool bIsInArea2 = IsInTriangle(xyz2, xyz3, xyz4, xyz0);
            if (bIsInArea1 || bIsInArea2) return true;
        }
    }
    //如果左右边界点的数量不同，则点数多的一方都和少的一方的最后一点连接
    if (nTriCnt != 0) {
        const std::vector<std::vector<double>> &cvctvdLongerEdgeGeo = cvctvdLeftEdgeGeo.size() > cvctvdRightEdgeGeo.size() ? cvctvdLeftEdgeGeo : cvctvdRightEdgeGeo;
        const std::vector<std::vector<double>> &cvctvdShotterEdgeGeo = cvctvdLeftEdgeGeo.size() < cvctvdRightEdgeGeo.size() ? cvctvdLeftEdgeGeo : cvctvdRightEdgeGeo;
        for (int i = nRectCnt; i < nRectCnt + nTriCnt; ++i) {
            if (!isReverseFlag) {
                XYZ xyzA = P2XYZ(cvctvdShotterEdgeGeo[nRectCnt]);
                XYZ xyzB = P2XYZ(cvctvdLongerEdgeGeo[i]);
                XYZ xyzC = P2XYZ(cvctvdLongerEdgeGeo[i + 1]);
                bIsInArea = IsInTriangle(xyzA, xyzB, xyzC, xyz0);
                if (bIsInArea) return true;
            } else {
                if (cvctvdLeftEdgeGeo.size() < cvctvdRightEdgeGeo.size()) {
                    XYZ xyzA = P2XYZ(cvctvdShotterEdgeGeo[0]);
                    XYZ xyzB = P2XYZ(cvctvdLongerEdgeGeo[i]);
                    XYZ xyzC = P2XYZ(cvctvdLongerEdgeGeo[i + 1]);
                    bIsInArea = IsInTriangle(xyzA, xyzB, xyzC, xyz0);
                    if (bIsInArea) return bIsInArea;
                } else {
                    XYZ xyzA = P2XYZ(cvctvdShotterEdgeGeo[nRectCnt]);
                    XYZ xyzB = P2XYZ(cvctvdLongerEdgeGeo[cvctvdLongerEdgeGeo.size() - 1 - i]);
                    XYZ xyzC = P2XYZ(cvctvdLongerEdgeGeo[cvctvdLongerEdgeGeo.size() - 1 - i - 1]);
                    bIsInArea = IsInTriangle(xyzA, xyzB, xyzC, xyz0);
                    if (bIsInArea) return bIsInArea;
                }
            }
        }
    }

    return bIsInArea;
}

bool HdLibMath::IsInTriangle(const Point &cptPoint, const Point &cptA, const Point &cptB, const Point &cptC) {
    //类定义：二维向量
    struct Vector2d {
        double x_;
        double y_;

        Vector2d(double x, double y) : x_(x), y_(y) {}
        Vector2d() : x_(0), y_(0) {}

        //二维向量叉乘, 叉乘的结果其实是向量，方向垂直于两个向量组成的平面，这里我们只需要其大小和方向
        double CrossProduct(const Vector2d vec) { return x_ * vec.y_ - y_ * vec.x_; }

        //二维向量点积
        double DotProduct(const Vector2d vec) { return x_ * vec.x_ + y_ * vec.y_; }

        //二维向量减法
        Vector2d Minus(const Vector2d vec) const { return Vector2d(x_ - vec.x_, y_ - vec.y_); }
    };

    XYZ xyzX = P2XYZ(cptPoint);
    XYZ xyzA = P2XYZ(cptA);
    XYZ xyzB = P2XYZ(cptB);
    XYZ xyzC = P2XYZ(cptC);

    // Vector2d PA = Vector2d(cptA.lon, cptA.lat).Minus(Vector2d(cptPoint.lon, cptPoint.lat));
    // Vector2d PB = Vector2d(cptB.lon, cptB.lat).Minus(Vector2d(cptPoint.lon, cptPoint.lat));
    // Vector2d PC = Vector2d(cptC.lon, cptC.lat).Minus(Vector2d(cptPoint.lon, cptPoint.lat));
    Vector2d PA = Vector2d(xyzA.x, xyzA.y).Minus(Vector2d(xyzX.x, xyzX.y));
    Vector2d PB = Vector2d(xyzB.x, xyzB.y).Minus(Vector2d(xyzX.x, xyzX.y));
    Vector2d PC = Vector2d(xyzC.x, xyzC.y).Minus(Vector2d(xyzX.x, xyzX.y));
    double t1 = PA.CrossProduct(PB);
    double t2 = PB.CrossProduct(PC);
    double t3 = PC.CrossProduct(PA);
    return t1 * t2 >= 0 && t1 * t3 >= 0;
}

bool HdLibMath::IsInTriangle(const XYZ &cxyz1, const XYZ &cxyz2, const XYZ &cxyz3, const XYZ &cxyz0) {
    // 链接：https://leetcode-cn.com/circle/discuss/7OldE4/
    auto func_Product = [](const XYZ &cxyz1, const XYZ &cxyz2, const XYZ &cxyz3) {
        //首先根据坐标计算p1p2和p1p3的向量，然后再计算叉乘
        // p1p2 向量表示为 (p2.x-p1.x,p2.y-p1.y)
        // p1p3 向量表示为 (p3.x-p1.x,p3.y-p1.y)
        return (cxyz2.x - cxyz1.x) * (cxyz3.y - cxyz1.y) - (cxyz2.y - cxyz1.y) * (cxyz3.x - cxyz1.x);
    };

    //保证p1，p2，p3是逆时针顺序
    if (func_Product(cxyz1, cxyz2, cxyz3) < 0) return IsInTriangle(cxyz1, cxyz3, cxyz2, cxyz0);
    if (func_Product(cxyz1, cxyz2, cxyz0) > 0 && func_Product(cxyz2, cxyz3, cxyz0) > 0 && func_Product(cxyz3, cxyz1, cxyz0) > 0) return true;
    return false;
}

/**
 * 求一个点 在点序列中的 垂点
 * @param ptPoint 输入的某一个点
 * @param vctvdGeoLink 点序列
 * @param ptOutPoint 输出的垂足点坐标
 * @param dOutDistance 垂线长度
 * @param nOutNxtIndex 如果将垂足点放入到点序列中，则这个垂足点在序列中的Index
 * @return
 */
bool HdLibMath::GetPerpendicularFoot(const Point &ptPoint, const std::vector<std::vector<double>> &vctvdGeoLink, Point &ptOutPoint, double &dOutDistance, int &nOutNxtIndex) {
    std::vector<XYZ> xyzs{vctvdGeoLink.size(), {0, 0, 0}};
    std::transform(vctvdGeoLink.begin(), vctvdGeoLink.end(), xyzs.begin(), [](const std::vector<double> &p) { return P2XYZ(p); });

    XYZ inxyz = P2XYZ(ptPoint);
    bool have_per = false;
    Point per_p;
    double per_d = std::numeric_limits<double>::max();
    int nNxtIndex = 0;
    for (int i = 0, j = 1; j < xyzs.size(); ++i, ++j) {
        XYZ p1 = xyzs[i];
        XYZ p2 = xyzs[j];
        XYZ pp1 = inxyz;
        XYZ pp2(pp1.x + (p1.y - p2.y), pp1.y - (p1.x - p2.x), inxyz.z);

        // 跨立实验
        bool valid = (Cross(p2 - pp1, pp2 - pp1) * Cross(pp2 - pp1, p1 - pp1) > 0);
        if (!valid) continue;

        double newDist = abs(Cross(p2 - pp1, p1 - pp1) / (p1 - p2).length_xy());
        if (newDist < per_d) {
            per_d = newDist;
            nNxtIndex = j;

            double a = p2.y - p1.y;
            double b = p1.x - p2.x;
            double c = p2.x * p1.y - p1.x * p2.y;
            double m = pp1.x;
            double n = pp1.y;
            XYZ out_xyz((b * b * m - a * b * n - a * c) / (a * a + b * b), (a * a * n - a * b * m - b * c) / (a * a + b * b), (vctvdGeoLink[i][2] + vctvdGeoLink[j][2]) / 2);
            per_p = XYZ2P(out_xyz);
            have_per = true;
        }
    }
    // if (!ret)
    // {
    Point near_p;
    double near_d = std::numeric_limits<double>::max();
    for (const XYZ &p : xyzs) {
        double d = DXYZ(p, inxyz);
        if (d <= near_d) {
            near_p = Point(XYZ2P(p));
            near_d = d;
        }
    }
    // }
    // assert(ret);
    if (!have_per || (have_per && per_d > near_d * 5)) {
        ptOutPoint = near_p;
        dOutDistance = near_d;
    } else {
        ptOutPoint = per_p;
        dOutDistance = per_d;
    }
    // 垂足在vctvdGeoLink之外，或者所给点就是垂足
    if (nNxtIndex == 0) {
        double dMinDistance = std::numeric_limits<double>::max();
        double dCurDis = 0.0;
        int nNearestNeighborIndex = 0;
        for (int i = 0; i < vctvdGeoLink.size(); ++i) {
            dCurDis = PointsDis(ptOutPoint, vctvdGeoLink.at(i));
            if (dMinDistance > dCurDis) {
                dMinDistance = dCurDis;
                nNearestNeighborIndex = i;
                ptOutPoint = vctvdGeoLink.at(i);
            }
        }
        // 垂足点在vctvdGeoLink的开头
        if (nNearestNeighborIndex == 0) {
            nNxtIndex = 1;
        } else {
            nNxtIndex = nNearestNeighborIndex;
        }
    }
    nOutNxtIndex = nNxtIndex;
    return true;
}

/**
 * 求垂足坐标（不返回序列Index位置）
 * @return
 */
bool HdLibMath::GetPerpendicularFoot(const Point &ptPoint, const std::vector<std::vector<double>> &vctvdGeoLink, Point &ptOutPoint, double &dOutDistance) {
    int nNxtIndex = 0;
    return HdLibMath::GetPerpendicularFoot(ptPoint, vctvdGeoLink, ptOutPoint, dOutDistance, nNxtIndex);
}
/**
 * 求某个点 距离 某点序列最近的点的距离
 * @return 返回距离最近的点在序列中的Index和坐标值
 */
bool HdLibMath::GetNearestPointFromArray(const Point &ptPoint, const std::vector<std::vector<double>> &vctvdGeoLink, Point &ptOutPoint, double &dOutDistance, int &nOutIndex) {
    Point ptTmpPoint = {};
    double dMinDistance = std::numeric_limits<double>::max();
    int nTmpIndex = 0;
    for (size_t i = 0; i < vctvdGeoLink.size(); ++i) {
        Point ptGeo = Point(vctvdGeoLink[i]);
        double d = PointsDis(ptGeo, ptPoint);
        if (d < dMinDistance) {
            ptTmpPoint = ptGeo;
            dMinDistance = d;
            nTmpIndex = i;
        }
    }
    ptOutPoint = ptTmpPoint;
    dOutDistance = dMinDistance;
    nOutIndex = nTmpIndex;
    return true;
}