c++ 给定两个经纬度，求方位算法

在C++中，为了计算两点之间的方向（通常是角度），我们可以使用Haversine公式来估算大圆距离，然后通过一定的数学转换得到方位角。Haversine公式主要用于地球表面两点间的弧度距离，适用于球形坐标系。

以下是一个简化的Haversine函数示例，用于计算两个经纬度之间的角度：

#include <cmath>

const double EarthRadiusKm = 6371; // 地球平均半径，单位为公里

// 计算两点之间的大圆航程（弧度）
double haversine(double lat1, double lon1, double lat2, double lon2) {
    double dLat = degToRad(lat2 - lat1);
    double dLon = degToRad(lon2 - lon1);

    double a = sin(dLat / 2) * sin(dLat / 2) + cos(degToRad(lat1)) * cos(degToRad(lat2)) *
                sin(dLon / 2) * sin(dLon / 2);
    double c = 2 * atan2(sqrt(a), sqrt(1 - a));

    return EarthRadiusKm * c;
}

// 将度数转换为弧度
double degToRad(double degrees) {
    return degrees * M_PI / 180.0;
}

// 计算方位角（假设第一点在第二点的北边，逆时针为正）
double bearing(double lat1, double lon1, double lat2, double lon2) {
    double dLon = lon2 - lon1;
    if (dLon > M_PI)
        dLon -= 2 * M_PI;
    else if (dLon < -M_PI)
        dLon += 2 * M_PI;

    return atan2(sin(dLon) * cos(lat2), cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon));
}

// 示例用法
int main() {
    double lat1 = 39.9042, lon1 = 116.4074; // 北京经纬度
    double lat2 = 40.0130, lon2 = 116.3905; // 上海经纬度
    double distance = haversine(lat1, lon1, lat2, lon2);
    double bearing_angle = bearing(lat1, lon1, lat2, lon2);

    std::cout << "Distance: " << distance << " km" << std::endl;
    std::cout << "Bearing angle: " << bearing_angle * 180 / M_PI << "°" << std::endl;

    return 0;
}