C++实现站心坐标系转地心坐标系

站心坐标系和地心坐标系都是地理坐标系的一种，其中站心坐标系是以某个固定站点为原点建立的坐标系，而地心坐标系是以地球质心为原点建立的坐标系。因此，站心坐标系与地心坐标系之间的转换需要知道站点的经纬度和海拔高度等信息。

以下是C++实现站心坐标系转地心坐标系的示例代码：

#include <iostream>
#include <cmath>

using namespace std;

const double pi = 3.14159265358979323846;
const double a = 6378137.0;  // 赤道半径
const double f = 1 / 298.257223563;  // 扁率

// 计算子午圈曲率半径
double Rm(double lat) {
    double sin_lat = sin(lat * pi / 180.0);
    return a * (1 - f) / pow(1 - f * pow(sin_lat, 2), 1.5);
}

// 计算卯酉圈曲率半径
double Rn(double lat) {
    double sin_lat = sin(lat * pi / 180.0);
    return a / sqrt(1 - f * pow(sin_lat, 2));
}

// 站心坐标系转地心坐标系
void station2earth(double lon0, double lat0, double h0, double x, double y, double z, double& lon, double& lat, double& h) {
    double sin_lat0 = sin(lat0 * pi / 180.0);
    double cos_lat0 = cos(lat0 * pi / 180.0);
    double sin_lon0 = sin(lon0 * pi / 180.0);
    double cos_lon0 = cos(lon0 * pi / 180.0);

    // 计算站心坐标系到地心坐标系的旋转矩阵
    double R[3][3] = {
        {-sin_lat0 * cos_lon0, -sin_lat0 * sin_lon0, cos_lat0},
        {-sin_lon0, cos_lon0, 0},
        {-cos_lat0 * cos_lon0, -cos_lat0 * sin_lon0, -sin_lat0}
    };

    // 计算站心坐标系到地心坐标系的平移向量
    double T[3] = {
        (Rm(lat0) + h0) * cos_lat0 * cos_lon0,
        (Rm(lat0) + h0) * cos_lat0 * sin_lon0,
        (Rn(lat0) / (1 - pow(f, 2)) + h0) * sin_lat0
    };

    // 矩阵乘法
    double p[3] = {x, y, z};
    double q[3] = {0};
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            q[i] += R[i][j] * p[j];
        }
        q[i] += T[i];
    }

    // 计算地心坐标系的经纬度和海拔高度
    double r = sqrt(pow(q[0], 2) + pow(q[1], 2) + pow(q[2], 2));
    double sin_lat = q[2] / r;
    lat = asin(sin_lat) * 180.0 / pi;
    double cos_lat = cos(lat * pi / 180.0);
    double sin_lon = q[1] / (r * cos_lat);
    double cos_lon = q[0] / (r * cos_lat);
    lon = atan2(sin_lon, cos_lon) * 180.0 / pi;
    h = r - a / sqrt(1 - f * pow(sin_lat, 2));
}

int main() {
    double lon0 = 116.3975;  // 北京市经度
    double lat0 = 39.9086;  // 北京市纬度
    double h0 = 43;  // 北京市海拔高度，单位为米
    double x = 1000;  // 站心坐标系X坐标，单位为米
    double y = 2000;  // 站心坐标系Y坐标，单位为米
    double z = 3000;  // 站心坐标系Z坐标，单位为米
    double lon, lat, h;
    station2earth(lon0, lat0, h0, x, y, z, lon, lat, h);
    cout << "经度：" << lon << "，纬度：" << lat << "，海拔高度：" << h << endl;
    return 0;
}

在上述示例代码中，函数`Rm`和`Rn`分别用于计算子午圈曲率半径和卯酉圈曲率半径，这两个函数都是基于WGS84椭球体模型的计算公式。函数`station2earth`用于将站心坐标系中的点`(x, y, z)`转换为地心坐标系中的点`(lon, lat, h)`，其中`lon`表示经度，`lat`表示纬度，`h`表示海拔高度。在函数中，首先计算站心坐标系到地心坐标系的旋转矩阵和平移向量，然后将旋转矩阵和平移向量应用到点`(x, y, z)`上，得到点`(q[0], q[1], q[2])`，最后根据点`(q[0], q[1], q[2])`计算地心坐标系的经纬度和海拔高度。