Calculate distance, bearing and more between Latitude/Longitude points

该博客介绍了如何使用C++实现计算两个经纬度点之间的距离、方位角，并根据给定方位角和距离计算新的经纬度坐标。内容涉及到地理坐标转换、三角函数应用以及地球曲率的考虑。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

参考链接：Calculate distance, bearing and more between Latitude/Longitude points

#include <string>
using namespace std;

class PointLatLngAlt
{
public:
    double lat;
    double lng;
    double alt;
    string tag;

public:
    PointLatLngAlt();
    PointLatLngAlt(double dLat, double dLng, double dAlt, string strTag);
    double getDistance(PointLatLngAlt &p2);
    double getBearing(PointLatLngAlt &p2);
    PointLatLngAlt newPos(double bearing, double distance);
	double rad2deg(double dRad);
	double deg2rad(double dDeg);
};

#include <math.h>
#include <iostream>
using namespace  std;

PointLatLngAlt::PointLatLngAlt()
{

}
/**
* 带初始化的构造函数
*/
PointLatLngAlt::PointLatLngAlt(double dLat, double dLng, double dAlt, string strTag)
{
    lat = dLat;
    lng = dLng;
    alt = dAlt;
    tag = strTag;
}
/**
* function：返回两对经纬度之间的距离
*/
double PointLatLngAlt::getDistance(PointLatLngAlt &p2)
{
    double d = lat * 0.017453292519943295;
    double num2 = lng * 0.017453292519943295;
    double num3 = p2.lat * 0.017453292519943295;
    double num4 = p2.lng * 0.017453292519943295;
    double num5 = num4 - num2;
    double num6 = num3 - d;
    double num7 = pow(sin(num6 / 2.0), 2.0) + ((cos(d) * cos(num3)) * pow(sin(num5 / 2.0), 2.0));
    double num8 = 2.0 * atan2(sqrt(num7), sqrt(1.0 - num7));
    return (6371 * num8) * 1000.0; // M
}
/**
* function：返回当前经纬度与p2经纬度之间方位角，方位角为与正北方向的夹角
*/
double PointLatLngAlt::getBearing(PointLatLngAlt &p2)
{
    double latitude1 = this->deg2rad(lat);
    double latitude2 = this->deg2rad(p2.lat);
    double longitudeDifference = this->deg2rad(p2.lng - lng);

    double y = sin(longitudeDifference) * cos(latitude2);
    double x = cos(latitude1) * sin(latitude2) - sin(latitude1) * cos(latitude2) * cos(longitudeDifference);

    double dDeg = this->rad2deg(atan2(y,x));
    if(dDeg < 0)
    {
        dDeg += 360;
    }

    return dDeg;
}
/**
* function：计算当前经纬度在给定方位角和距离下的经纬度
*/
PointLatLngAlt PointLatLngAlt::newPos(double bearing, double distance)
{
    // '''extrapolate latitude/longitude given a heading and distance
    //   thanks to http://www.movable-type.co.uk/scripts/latlong.html
    //  '''
    // from math import sin, asin, cos, atan2, radians, degrees
    double radius_of_earth = 6378100.0;//# in meters

    double lat1 = this->deg2rad(lat);
    double lon1 = this->deg2rad(lng);
    double brng = this->deg2rad(bearing);
    double dr = distance / radius_of_earth;

    double lat2 = asin(sin(lat1) * cos(dr) +
                cos(lat1) * sin(dr) * cos(brng));
    double lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
                        cos(dr) - sin(lat1) * sin(lat2));

    double latout = this->rad2deg(lat2);
    double lngout = this->rad2deg (lon2);

    return PointLatLngAlt(latout, lngout, alt, tag);
}
/**
* function：角度转弧度
*/
double PointLatLngAlt::deg2rad(double dDeg)
{
    return dDeg * M_PI / 180;
}
/**
* 弧度转角度
*/
double PointLatLngAlt::rad2deg(double dRad)
{
    return dRad * 180 / M_PI;
}