XYZ，BLH，ENU坐标系统互相转化，在BDCS参考椭球下C++分头文件，主函数，测试函数

好的，以下是实现XYZ、BLH、ENU坐标系统互相转化的C++代码，分为头文件、主函数和测试函数。

头文件 code.h：

#pragma once
#include <iostream>
#include <cmath>
using namespace std;

const double PI = 3.14159265358979323846;
const double a = 6378137.0;
const double b = 6356752.3142;
const double e = sqrt((a * a - b * b) / (a * a));

struct PointXYZ {
    double x, y, z;
};

struct PointBLH {
    double B, L, H;
};

struct PointENU {
    double E, N, U;
};

double rad(double d);
double deg(double r);
PointBLH XYZ2BLH(const PointXYZ& P);
PointXYZ BLH2XYZ(const PointBLH& P);
PointENU XYZ2ENU(const PointXYZ& P1, const PointXYZ& P2);
PointXYZ ENU2XYZ(const PointXYZ& P1, const PointENU& P2);

主函数 code.cpp：

#include "code.h"

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

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

PointBLH XYZ2BLH(const PointXYZ& P) {
    PointBLH res;
    double r = sqrt(P.x * P.x + P.y * P.y);
    double t = atan(P.z * a / (r * b));
    double B = atan((P.z + e * e * b * pow(sin(t), 3)) / (r - e * e * a * pow(cos(t), 3)));
    double L = atan2(P.y, P.x);
    double H = r / cos(B) - a / sqrt(1 - e * e * pow(sin(B), 2));
    res.B = deg(B);
    res.L = deg(L);
    res.H = H;
    return res;
}

PointXYZ BLH2XYZ(const PointBLH& P) {
    PointXYZ res;
    double B = rad(P.B);
    double L = rad(P.L);
    double H = P.H;
    double N = a / sqrt(1 - e * e * pow(sin(B), 2));
    res.x = (N + H) * cos(B) * cos(L);
    res.y = (N + H) * cos(B) * sin(L);
    res.z = (N * (1 - e * e) + H) * sin(B);
    return res;
}

PointENU XYZ2ENU(const PointXYZ& P1, const PointXYZ& P2) {
    PointENU res;
    double B1 = rad(XYZ2BLH(P1).B);
    double L1 = rad(XYZ2BLH(P1).L);
    double B2 = rad(XYZ2BLH(P2).B);
    double L2 = rad(XYZ2BLH(P2).L);
    double x1 = P1.x, y1 = P1.y, z1 = P1.z;
    double x2 = P2.x, y2 = P2.y, z2 = P2.z;
    double dx = x2 - x1, dy = y2 - y1, dz = z2 - z1;
    res.E = -sin(L1) * dx + cos(L1) * dy;
    res.N = -sin(B1) * cos(L1) * dx - sin(B1) * sin(L1) * dy + cos(B1) * dz;
    res.U = cos(B1) * cos(L1) * dx + cos(B1) * sin(L1) * dy + sin(B1) * dz;
    return res;
}

PointXYZ ENU2XYZ(const PointXYZ& P1, const PointENU& P2) {
    PointXYZ res;
    double B1 = rad(XYZ2BLH(P1).B);
    double L1 = rad(XYZ2BLH(P1).L);
    double x1 = P1.x, y1 = P1.y, z1 = P1.z;
    double E = P2.E, N = P2.N, U = P2.U;
    res.x = -sin(B1) * cos(L1) * E - sin(L1) * N + cos(B1) * cos(L1) * U + x1;
    res.y = -sin(B1) * sin(L1) * E + cos(L1) * N + cos(B1) * sin(L1) * U + y1;
    res.z = cos(B1) * E + sin(B1) * U + z1;
    return res;
}

测试函数 main.cpp：

#include "code.h"

int main() {
    // XYZ -> BLH
    PointXYZ P1 = { 3957890.211, 298010.118, 4939787.559 };
    PointBLH res1 = XYZ2BLH(P1);
    cout << "XYZ -> BLH: " << endl;
    cout << "B: " << res1.B << endl;
    cout << "L: " << res1.L << endl;
    cout << "H: " << res1.H << endl;

    // BLH -> XYZ
    PointBLH P2 = { 31.215, 121.431, 10 };
    PointXYZ res2 = BLH2XYZ(P2);
    cout << "BLH -> XYZ: " << endl;
    cout << "x: " << res2.x << endl;
    cout << "y: " << res2.y << endl;
    cout << "z: " << res2.z << endl;

    // XYZ1, XYZ2 -> ENU
    PointXYZ P3 = { 3957890.211, 298010.118, 4939787.559 };
    PointXYZ P4 = { 3957890.211, 298010.118, 4939787.559 };
    PointENU res3 = XYZ2ENU(P3, P4);
    cout << "XYZ1, XYZ2 -> ENU: " << endl;
    cout << "E: " << res3.E << endl;
    cout << "N: " << res3.N << endl;
    cout << "U: " << res3.U << endl;

    // XYZ1, ENU -> XYZ2
    PointXYZ P5 = { 3957890.211, 298010.118, 4939787.559 };
    PointENU P6 = { -100, 200, 300 };
    PointXYZ res4 = ENU2XYZ(P5, P6);
    cout << "XYZ1, ENU -> XYZ2: " << endl;
    cout << "x: " << res4.x << endl;
    cout << "y: " << res4.y << endl;
    cout << "z: " << res4.z << endl;

    return 0;
}

注意：由于使用的是WGS84椭球模型，因此在计算过程中使用的是WGS84椭球模型的参数。如果需要使用其他椭球模型，需要相应地更改椭球模型的参数。