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

/****************************************************************************
 * Copyright (C) 2020 by Simulation Team, NavInfo Ltd.                      *
 *                                                                          *
 * This file is part of Simulation Core Software Stack.                     *
 *                                                                          *
 *   Simulation Core is a commercial autonomous driving simulation solution.*
 *                                                                          *
 *   You should have received a copy of the NavInfo EULA License along with *
 *   the software. If not, please contact us, <simulation@navinfo.com>.     *
 ****************************************************************************/

#include "geo/private/base/constant.h"
#include "geo/private/math/angle.h"
#include "geo/private/math/distance.h"
#include "geo/private/math/trigonometric.h"

namespace ns
{
namespace geo
{

double flat_distance(const PointD& p1, const PointD& p2)
{
    const double dx = p1.x - p2.x;
    const double dy = p1.y - p2.y;
    return std::sqrt(dx * dx + dy * dy);
}

/*
 * 计算pos点到由p1、p2组成的直线的距离，以及直线上距离pos最近的点foot
 */
double pointSegmentDistance(const PointD& p, const PointD& p1,
                            const PointD& p2, PointD& foot, CalDistanceFunc func)
{
    double deltaX = p2.x - p1.x;
    double deltaY = p2.y - p1.y;
    double d1 = deltaX * (p.x - p1.x) + deltaY * (p.y - p1.y);
    if (d1 <= 0)
    {
        foot = p1;
        return func(p, p1);
    }
    double d2 = deltaX * deltaX + deltaY * deltaY;
    if (d1 >= d2)
    {
        foot = p2;
        return func(p, p2);
    }
    double r = d1 / d2;
    foot.x = p1.x + deltaX * r;
    foot.y = p1.y + deltaY * r;
    foot.z = p1.z + (p2.z - p1.z) * r;
    return func(p, foot);
}

double calcPt2SegmentLonlat(const PointD& pos, const PointD& p1,
                            const PointD& p2, CalDistanceFunc func)
{
    double a = func(p1, p2);  //经纬坐标系中求两点的距离公式
    double b = func(p2, pos); //经纬坐标系中求两点的距离公式
    double c = func(p1, pos); //经纬坐标系中求两点的距离公式
    if (b * b >= c * c + a * a)
        return c;
    if (c * c >= b * b + a * a)
        return b;
    double l = (a + b + c) / 2;                                 //周长的一半
    double s = sqrt(std::abs(l * (l - a) * (l - b) * (l - c))); //海伦公式求面积
    return 2 * s / a;
}

// 美团采用的计算方法，在一个小范围内，例如北京市里面，把地球的经线和纬线当成是垂直的，直接采用勾股定理计算出距离
const double calDistanceSimple(const PointD& p1, const PointD& p2)
{
    double dx = Degree2Radian(p1.x - p2.x); // 经度差值
    double dy = Degree2Radian(p1.y - p2.y); // 纬度差值
    double cosb1 = ns_cos_round((p1.y + p2.y) * 0.5);
    double lx = dx * EARTH_EQUATOR_RADIUS * cosb1; // 东西距离
    // double b = Degree2Radian((p1.y + p2.y) * 0.5); // 平均纬度
    // double lx = dx * EARTH_EQUATOR_RADIUS * cos(b); // 东西距离
    double ly = dy * EARTH_EQUATOR_RADIUS; // 南北距离
    return sqrt(lx * lx + ly * ly);
}

// 已知两个经纬度坐标点，求两个经纬度之间的距离
const double calDistanceHaversine(const PointD& p1, const PointD& p2)
{
    // 所有的角度转化为弧度进行计算
    double dLat = Degree2Radian(p2.y - p1.y);
    double dLon = Degree2Radian(p2.x - p1.x);
    double tmpLat1 = Degree2Radian(p1.y);
    double tmpLat2 = Degree2Radian(p2.y);
    // 代入计算地球距离的Haversine公式中
    double a = pow(sin(dLat * 0.5), 2) + pow(sin(dLon * 0.5), 2) * cos(tmpLat1) * cos(tmpLat2);
    double c = 2 * asin(sqrt(a)); // 和2 * atan2(sqrt(a), sqrt(1-a));一样
    // double c = 2 * atan2(sqrt(a), sqrt(1-a));
    return EARTH_EQUATOR_RADIUS * c;
}

// 根据Vincenty的公式计算地球上两点间的距离，把地球当成椭圆形进行计算
const double calDistanceVincenty(const PointD& p1, const PointD& p2)
{
    static double a = EARTH_EQUATOR_RADIUS; // 椭球长轴半径
    static double f = 1 / 298.257223563;    // 椭球体变平
    static double b = (1 - f) * a;          // 椭球短轴半径

    double U1 = atan((1 - f) * tan(Degree2Radian(p1.y))); //降低纬度处理
    double U2 = atan((1 - f) * tan(Degree2Radian(p2.y))); //同时把经纬度转化为弧度
    double sinU1 = sin(U1), cosU1 = cos(U1);
    double sinU2 = sin(U2), cosU2 = cos(U2);
    double L = Degree2Radian(p2.x - p1.x); // 两点经度差

    double sinLambda;
    double cosLambda;
    double sinSigma;
    double cosSigma;
    double sigma;
    double cosSqAlpha;
    double cos2SigmaM;

    double lambda = L;
    double prevLambda;
    int iterationLimit = 100; // 迭代公式次数限制
    do
    {
        sinLambda = sin(lambda);
        cosLambda = cos(lambda);
        sinSigma =
            sqrt(pow(cosU2 * sinLambda, 2) + pow(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda, 2));
        if (sinSigma == 0) // 判断是否为重合点
            return 0;

        cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
        sigma = atan(sinSigma / cosSigma);
        double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
        cosSqAlpha = 1 - pow(sinAlpha, 2);

        if (cosSqAlpha == 0)
        {
            cos2SigmaM = 0;
        }
        else
        {
            cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
        }

        double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
        prevLambda = lambda;
        lambda = L + (1 - C) * f * sinAlpha *
                         (sigma + C * sinSigma *
                                      (cos2SigmaM + C * cosSigma * (-1 + 2 * pow(cos2SigmaM, 2))));
    } while (std::abs(lambda - prevLambda) > 1e-12 && --iterationLimit > 0);

    if (iterationLimit == 0)
        return 0;

    double uSq = cosSqAlpha * (pow(a, 2) - pow(b, 2)) / pow(b, 2);
    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 * pow(cos2SigmaM, 2)) -
                                           B / 6 * cos2SigmaM * (-3 + 4 * pow(sinSigma, 2)) *
                                               (-3 + 4 * pow(cos2SigmaM, 2))));
    double s = b * A * (sigma - deltaSigma);

    return s;
}

} // namespace geo
} // namespace ns