大地坐标转化直角坐标系迭代C++代码

以下是C++代码实现大地坐标转化直角坐标系迭代：

#include <iostream>
#include <cmath>

using namespace std;

const double pi = 3.1415926535897932384626;
const double a = 6378137.0; // 长半轴
const double b = 6356752.3142; // 短半轴
const double f = 1 / 298.257223563; // 扁率
const double e1 = sqrt(2 * f - f * f); // 第一偏心率
const double e2 = sqrt(f * (2 - f) / (1 - f) / (1 - f)); // 第二偏心率

double rad(double d) {
    return d * pi / 180.0;
}

double deg(double r) {
    return r * 180.0 / pi;
}

// 大地坐标转直角坐标系，输入经度、纬度、高程，输出X、Y、Z
void geodetic_to_rectangular(double lon, double lat, double h, double &X, double &Y, double &Z) {
    double N = a / sqrt(1 - e1 * e1 * sin(rad(lat)) * sin(rad(lat)));
    X = (N + h) * cos(rad(lat)) * cos(rad(lon));
    Y = (N + h) * cos(rad(lat)) * sin(rad(lon));
    Z = (N * (1 - e1 * e1) + h) * sin(rad(lat));
}

// 直角坐标系转大地坐标，输入X、Y、Z，输出经度、纬度、高程
void rectangular_to_geodetic(double X, double Y, double Z, double &lon, double &lat, double &h) {
    double p = sqrt(X * X + Y * Y);
    double theta = atan2(Z * a, p * b);
    lat = atan2(Z + e2 * e2 * b * pow(sin(theta), 3), p - e1 * e1 * a * pow(cos(theta), 3));
    lon = atan2(Y, X);
    double N = a / sqrt(1 - e1 * e1 * sin(lat) * sin(lat));
    h = p / cos(lat) - N;
    while (fabs(lat - lat0) > 1e-10 || fabs(lon - lon0) > 1e-10 || fabs(h - h0) > 1e-10) {
        lat0 = lat;
        lon0 = lon;
        h0 = h;
        double N = a / sqrt(1 - e1 * e1 * sin(lat0) * sin(lat0));
        h0 = p / cos(lat0) - N;
        lat0 = atan2(Z + e2 * e2 * N * sin(lat0), p - e1 * e1 * a * cos(lat0));
        lon0 = atan2(Y, X);
    }
    lat = lat0;
    lon = lon0;
    h = h0;
}

int main() {
    double lon = 120.0; // 经度
    double lat = 30.0; // 纬度
    double h = 0.0; // 高程
    double X, Y, Z;
    geodetic_to_rectangular(lon, lat, h, X, Y, Z);
    cout << "X: " << X << ", Y: " << Y << ", Z: " << Z << endl;
    rectangular_to_geodetic(X, Y, Z, lon, lat, h);
    cout << "lon: " << lon << ", lat: " << lat << ", h: " << h << endl;
    return 0;
}

注意，这里的经度和纬度都是以度数为单位的。