hpm6750/controller_yy_app/controlware/geomatry.c

#include "geomatry.h"
#include "helpler_funtions.h"
#include "math.h"
#include "my_math.h"
#include "pilot_navigation.h"
#include "soft_time.h"
#include <stdlib.h>
#include <string.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// @brief 给定平面两条直线求交点坐标
// @params const Vector3d* la //直线
//         sconst Vector3d* lb //直线
// @retval Coords2d cp 二维交点的坐标
Coords2df lineInterLine(const Vector3ddouble *la, const Vector3ddouble *lb)
{
    Coords2df cp;
    double a, b, c, A, B, C;
    a = la->x;
    b = la->y;
    c = la->z;
    if (b == 0.0f)
    {
        c = c / a;
        a = 1;
    }
    else
    {
        a = a / b;
        c = c / b;
        b = 1;
    }
    A = lb->x;
    B = lb->y;
    C = lb->z;
    if (B == 0.0f)
    {
        C = C / A;
        A = 1;
    }
    else
    {
        A = A / B;
        C = C / B;
        B = 1;
    }
    // 不平行才有交点
    if (a != A || b != B)
    {
        cp.x = -(b * C - B * c) / (A * b - a * B);
        cp.y = -(A * c - a * C) / (A * b - a * B);
    }
    return cp;
}

// 函数：求两点直线
// 功能：给定两个点a\b，求所在直线参数
// 参数：const Coords2d* pa //点a
//       const Vector2d* pb //点b
// 返回：直calLineEquation线方程参数
Vector3ddouble calLineEquation(const Coords2ddouble *pa,
                               const Coords2ddouble *pb)
{
    double a, b, c;
    if (pa->x == pb->x)
    {
        a = 1;
        b = 0;
        c = -pa->x;
    }
    else
    {
        a = -(double)(pa->y - pb->y) / (double)(pa->x - pb->x);
        b = 1;
        c = (double)(pa->y * pb->x - pa->x * pb->y) / (double)(pa->x - pb->x);
    }
    Vector3ddouble params;
    params.x = a;
    params.y = b;
    params.z = c;
    return params;
}

// 函数：点在直线上的垂直投影
// 功能：给定一点和一条直线，求该点在该直线上的垂直投影
// 参数：const Coords2d* p //点p
//       const Vector3d* lineParam //直线方程参数
// 返回：投影点坐标
Coords2df pointProjection2Line(const Coords2df *p, const Vector3ddouble *params)
{
    Coords2df pointProjection;
    //如果点在线上，投影点就是该点
    if ((int)(params->x * p->x + params->y * p->y + params->z) == 0)
        pointProjection = *p;
    else
    {
        //通过该点的与给定直线垂直的直线
        double lineParamTemp[3] = {-params->y, params->x,
                                   params->y * p->x - params->x * p->y};
        //两条线的交点就是垂直投影点
        const Coords3ddouble *param1, *param2;
        param1 = (Coords3ddouble *)params;
        param2 = (Coords3ddouble *)lineParamTemp;
        pointProjection = lineInterLine(param1, param2);
    }
    return pointProjection;
}

// @brief 判断点在多边形内部
// @param const Coords2dint *point 点坐标指针
// @param const Coords2dint *polygon 多变形顶点坐标指针
// @param int count 多边形顶点个数
// @retval
bool PointInPolygon(const Coords3dint *point, const Coords3dint *polygon,
                    int count)
{
    int cross = 0;

    int x_max = polygon[0].x, y_max = polygon[0].y;
    int x_min = polygon[0].x, y_min = polygon[0].y;
    for (int i = 1; i < count; ++i)
    {
        if (polygon[i].x > x_max)
            x_max = polygon[i].x;
        if (polygon[i].x < x_min)
            x_min = polygon[i].x;
        if (polygon[i].y > y_max)
            y_max = polygon[i].y;
        if (polygon[i].y < y_min)
            y_min = polygon[i].y;
    }
    if (point->x > x_max || point->x < x_min || point->y > y_max ||
        point->y < y_min)
    {
        return false;
    }

    for (int i = 0; i < count; ++i)
    {
        const Coords3dint *p1, *p2;
        p1 = &polygon[i];
        p2 = &polygon[(i + 1) % count];

        // 以 y = point.y 为射线，求其与多变形的交点个数
        // p1, p2 连线与 x 轴平行，则为无交点
        if (p1->y == p2->y)
            continue;
        // 交点落在 p1 p2 延长线上
        else if (point->y < (p1->y < p2->y ? p1->y : p2->y))
            continue;
        else if (point->y >= (p1->y > p2->y ? p1->y : p2->y))
            continue;
        else
        {
            double x;
            x = (double)(point->y - p1->y) * (double)(p2->x - p1->x) /
                    (double)(p2->y - p1->y) +
                p1->x;
            if (x > point->x)
            {
                cross++;
            }
        }
    }

    if (cross % 2)
        return true;
    else
        return false;
}

// @brief 求二维平面点到线段的最短距离，以及改点到最短距离点的方位角
// @param *p 点的坐标数据指针
// @param *segment_a 线段端点 a 的坐标指针
// @param *segment_b 线段端点 b 的坐标指针
// @param *dist 点到线段上最近点的距离
// @param *angle 点到线段上最近点的方位角
void PointToSegmentDist(const Coords2dint *p, const Coords2dint *segment_a,
                        const Coords2dint *segment_b, float *dist, float *angle)
{
    // 计算向量 AB, AP
    Vector2dint AB = {.x = segment_b->x - segment_a->x,
                      .y = segment_b->y - segment_a->y};
    Vector2dint AP = {.x = p->x - segment_a->x, .y = p->y - segment_a->y};

    // 计算 AB * AP
    float AB_dot_AP = (float)AB.x * AP.x + (float)AB.y * AP.y;

    // 计算 |AB|^2
    float AB_dist_squar = (float)AB.x * AB.x + (float)AB.y * (float)AB.y;

    // 如果 AB AP 的点积小于零，说明二者夹角大于 90 deg，PA 为最短距离
    if (AB_dot_AP < 0.0f)
    {
        *dist = get_norm(segment_a->x - p->x, segment_a->y - p->y);
        *angle = atan2f(segment_a->x - p->x, segment_a->y - p->y) * RAD_TO_DEG;
    }
    // 如果 AB AP 的夹角小于 90 deg
    else
    {
        if (AB_dot_AP > AB_dist_squar)
        {
            // 如果 AP 在 AB 上的投影超过了 AB，那么最短距离为 PB
            *dist = get_norm(segment_b->x - p->x, segment_b->y - p->y);
            *angle =
                atan2f(segment_b->x - p->x, segment_b->y - p->y) * RAD_TO_DEG;
        }
        else
        {
            // 如果 AP 在 AB 上的投影在 AB 线段内，则最短距离为 AP 在 AB
            // 法线方向上的投影
            // 角度为 AB 方向 +- (pi / 2)
            float angle_AB = atan2f(segment_b->x - segment_a->x,
                                    segment_b->y - segment_a->y);
            float angle_AP = atan2f(p->x - segment_a->x, p->y - segment_a->y);

            float angle_AP_AB = wrap_pi(angle_AB - angle_AP);

            *dist = get_norm(p->x - segment_a->x, p->y - segment_a->y) *
                    sinf(angle_AP_AB);

            if (angle_AP_AB > 0.0f)
            {
                *angle = wrap_pi(angle_AB + M_PI_F / 2.0f) * RAD_TO_DEG;
            }
            else
            {
                *angle = wrap_pi(angle_AB - M_PI_F / 2.0f) * RAD_TO_DEG;
            }
        }
    }
}

// @brief 求点到多边形最短距离
// @param *p 点的坐标数据指针
// @param *polygon 多边形顶点坐标数据指针
// @param count 多边形顶点个数，从 1 开始数
// @param *dist 传出结果，点到多边形的最短距离
// @param *angle 传出结果，到多边形最近点的方位角，单位 deg
void PointToPolygonDist(const Coords3dint *p, const Coords3dint *polygon,
                        int count, float *dist, float *angle)
{
    float min_dist = INFINITY, min_dist_angle = 0.0f;
    for (int i = 0; i < count; ++i)
    {
        Coords2dint A = {.x = polygon[i].x, .y = polygon[i].y};
        Coords2dint B = {.x = polygon[(i + 1) / count].x,
                         .y = polygon[(i + 1) / count].y};
        Coords2dint P = {p->x, p->y};
        float tmp_dist, tmp_angle;
        PointToSegmentDist(&P, &A, &B, &tmp_dist, &tmp_angle);
        if (tmp_dist < min_dist)
        {
            min_dist = tmp_dist;
            min_dist_angle = tmp_angle;
        }
    }

    *dist = min_dist;
    *angle = min_dist_angle;
}

static double deg2rad(double deg) { return deg * M_PI / 180; }

/**
 * @brief Vincenty 经纬度转平面距离
 *
 * @param latA A 点纬度
 * @param lngA A 点经度
 * @param latB B 点纬度
 * @param lngB B 点经度
 * @return double 距离
 */
double distanceVincenty(double latA, double lngA, double latB, double lngB)
{
    double b = 6356752.3142;
    double f = 1 / 298.257223563;
    const double earthR = 6378137.0;

    double L = deg2rad(lngB - lngA);
    double U1 = atan((1 - f) * tan(deg2rad(latA)));
    double U2 = atan((1 - f) * tan(deg2rad(latB)));
    double sinU1 = sin(U1);
    double cosU1 = cos(U1);
    double sinU2 = sin(U2);
    double cosU2 = cos(U2);
    double lambda = L;
    double lambdaP = 2 * M_PI;
    double iterLimit = 20;

    double sinLambda, cosLambda;
    double sinSigma = 0;
    double cosSigma = 0;
    double sigma = 0;
    double alpha;
    double cosSqAlpha = 0;
    double cos2SigmaM = 0;
    double C;

    while (fabs(lambda - lambdaP) > 1e-12 && --iterLimit > 0)
    {
        sinLambda = sin(lambda);
        cosLambda = cos(lambda);
        sinSigma = sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) +
                        (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) *
                            (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
        if (sinSigma == 0)
        {
            return 0; // co-incident points
        }
        cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
        sigma = atan2(sinSigma, cosSigma);
        alpha = asin(cosU1 * cosU2 * sinLambda / sinSigma);
        cosSqAlpha = cos(alpha) * cos(alpha);
        cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
        C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
        lambdaP = lambda;
        lambda = L + (1 - C) * f * sin(alpha) *
                         (sigma + C * sinSigma *
                                      (cos2SigmaM +
                                       C * cosSigma *
                                           (-1 + 2 * cos2SigmaM * cos2SigmaM)));
    }
    if (iterLimit == 0)
    {
        return 0.0; // formula failed to converge
    }
    double uSq = cosSqAlpha * (earthR * earthR - b * b) / (b * b);
    double A =
        1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
    double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
    double deltaSigma =
        B * sinSigma *
        (cos2SigmaM + B / 4 *
                          (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
                           B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) *
                               (-3 + 4 * cos2SigmaM * cos2SigmaM)));
    return b * A * (sigma - deltaSigma);
}

/**
 * @brief 计算经纬度转平面坐标距离的系数
 *
 * @param lat 纬度
 * @param lng 经度
 * @param ratiox 经度转平面距离m系数
 * @param ratioy 纬度转平面距离m系数
 */
void Cal_LngLat_To_FlatM_Ratio(double lat, double lng, double *ratiox,
                               double *ratioy)
{
    double DIFF = 0.0001;
    double deltax = distanceVincenty(lat, lng, lat, lng + DIFF);
    double deltay = distanceVincenty(lat, lng, lat + DIFF, lng);
    *ratiox = deltax / DIFF;
    *ratioy = deltay / DIFF;
}

/**
 * @brief 根据经纬度计算距离和方位角
 *
 * @param lon1 经度1
 * @param lat1 纬度1
 * @param lon2 经度2
 * @param lat2 纬度2
 * @param lonToM 经度差转平面距离m系数
 * @param latToM 纬度差转平面距离m系数
 * @param dist_m 距离m
 * @param bearing_rad 1->2方位角rad
 */
void LanLat_To_Flat_Dist_Bearing(int lon1, int lat1, int lon2, int lat2,
                                 double lonToM, double latToM, float *dist_m,
                                 float *bearing_rad)
{
    double lon_e = wrap_double(lon2 - lon1, -180.0 * 1e7, 180.0 * 1e7);
    double lat_e = wrap_double(lat2 - lat1, -180.0 * 1e7, 180 * 1e7);
    double lon_delta = (double)lon_e * 1e-7;
    double lat_delta = (double)lat_e * 1e-7;

    float dist_e = lonToM * lon_delta;
    float dist_n = latToM * lat_delta;
    *dist_m = get_norm(dist_e, dist_n);
    *bearing_rad = atan2f(dist_e, dist_n);
}