使用BDCS参考椭球模型XYZ，BLH，ENU坐标系统互相转化，具备XYZ坐标的减法和求模，C++分头文件，主函数，测试函数

以下是参考椭球模型XYZ，BLH，ENU坐标系统互相转化的C++代码：

头文件 `coordinates.h`：

#ifndef COORDINATES_H
#define COORDINATES_H

#include <math.h>

const double PI = 3.14159265358979323846;
const double a = 6378137.0; // WGS-84椭球长半轴
const double b = 6356752.3142; // WGS-84椭球短半轴
const double e = sqrt(1.0 - (b * b) / (a * a)); // WGS-84椭球第一偏心率
const double ep = sqrt((a * a) / (b * b) - 1.0); // WGS-84椭球第二偏心率

// BLH坐标系结构体
struct BLH
{
    double B; // 纬度，单位：弧度
    double L; // 经度，单位：弧度
    double H; // 高程，单位：米
};

// XYZ坐标系结构体
struct XYZ
{
    double X;
    double Y;
    double Z;
};

// ENU坐标系结构体
struct ENU
{
    double E;
    double N;
    double U;
};

// 弧度转角度
double rad2deg(double rad);

// 角度转弧度
double deg2rad(double deg);

// 计算子午圈半径
double getRn(double B);

// 计算卯酉圈半径
double getRm(double B);

// BLH坐标系转XYZ坐标系
XYZ BLH2XYZ(BLH blh);

// XYZ坐标系转BLH坐标系
BLH XYZ2BLH(XYZ xyz);

// XYZ坐标系减法
XYZ XYZminus(XYZ xyz1, XYZ xyz2);

// XYZ坐标系求模
double XYZmod(XYZ xyz);

// BLH坐标系转ENU坐标系
ENU BLH2ENU(BLH blh1, BLH blh2);

#endif // COORDINATES_H

`coordinates.cpp` 实现了上述头文件中定义的各个函数：

#include "coordinates.h"

// 弧度转角度
double rad2deg(double rad)
{
    return rad * 180.0 / PI;
}

// 角度转弧度
double deg2rad(double deg)
{
    return deg * PI / 180.0;
}

// 计算子午圈半径
double getRn(double B)
{
    return a / sqrt(1.0 - e * e * sin(B) * sin(B));
}

// 计算卯酉圈半径
double getRm(double B)
{
    return a * (1.0 - e * e) / pow(1.0 - e * e * sin(B) * sin(B), 1.5);
}

// BLH坐标系转XYZ坐标系
XYZ BLH2XYZ(BLH blh)
{
    XYZ xyz;
    double N = getRn(blh.B);
    xyz.X = (N + blh.H) * cos(blh.B) * cos(blh.L);
    xyz.Y = (N + blh.H) * cos(blh.B) * sin(blh.L);
    xyz.Z = (N * (1 - e * e) + blh.H) * sin(blh.B);
    return xyz;
}

// XYZ坐标系转BLH坐标系
BLH XYZ2BLH(XYZ xyz)
{
    BLH blh;
    double B0 = atan2(xyz.Z, sqrt(xyz.X * xyz.X + xyz.Y * xyz.Y));
    double H = 0.0;
    double N;
    do
    {
        N = getRn(B0);
        H = sqrt(xyz.X * xyz.X + xyz.Y * xyz.Y) / cos(B0) - N;
        B0 = atan2(xyz.Z, sqrt(xyz.X * xyz.X + xyz.Y * xyz.Y) * (1.0 - e * e * N / (N + H)));
    } while (fabs(H) > 1e-6); // 迭代计算高程
    blh.B = B0;
    blh.L = atan2(xyz.Y, xyz.X);
    blh.H = H;
    return blh;
}

// XYZ坐标系减法
XYZ XYZminus(XYZ xyz1, XYZ xyz2)
{
    XYZ xyz;
    xyz.X = xyz1.X - xyz2.X;
    xyz.Y = xyz1.Y - xyz2.Y;
    xyz.Z = xyz1.Z - xyz2.Z;
    return xyz;
}

// XYZ坐标系求模
double XYZmod(XYZ xyz)
{
    return sqrt(xyz.X * xyz.X + xyz.Y * xyz.Y + xyz.Z * xyz.Z);
}

// BLH坐标系转ENU坐标系
ENU BLH2ENU(BLH blh1, BLH blh2)
{
    XYZ xyz1 = BLH2XYZ(blh1);
    XYZ xyz2 = BLH2XYZ(blh2);
    XYZ dxyz = XYZminus(xyz2, xyz1);
    double B = blh1.B;
    double L = blh1.L;
    ENU enu;
    enu.E = -sin(L) * dxyz.X + cos(L) * dxyz.Y;
    enu.N = -sin(B) * cos(L) * dxyz.X - sin(B) * sin(L) * dxyz.Y + cos(B) * dxyz.Z;
    enu.U = cos(B) * cos(L) * dxyz.X + cos(B) * sin(L) * dxyz.Y + sin(B) * dxyz.Z;
    return enu;
}

`main.cpp` 中包含主函数和测试函数：

#include "coordinates.h"
#include <iostream>
using namespace std;

int main()
{
    BLH blh1 = { deg2rad(39.986244), deg2rad(116.304717), 50.0 };
    BLH blh2 = { deg2rad(39.986501), deg2rad(116.305184), 60.0 };
    XYZ xyz1 = BLH2XYZ(blh1);
    XYZ xyz2 = BLH2XYZ(blh2);
    XYZ dxyz = XYZminus(xyz2, xyz1);
    double dist = XYZmod(dxyz);
    ENU enu = BLH2ENU(blh1, blh2);
    cout << "xyz1: (" << xyz1.X << ", " << xyz1.Y << ", " << xyz1.Z << ")" << endl;
    cout << "xyz2: (" << xyz2.X << ", " << xyz2.Y << ", " << xyz2.Z << ")" << endl;
    cout << "dxyz: (" << dxyz.X << ", " << dxyz.Y << ", " << dxyz.Z << ")" << endl;
    cout << "dist: " << dist << " meters" << endl;
    cout << "enu: (" << enu.E << ", " << enu.N << ", " << enu.U << ")" << endl;
    return 0;
}

测试函数中计算了两个 BLH 坐标系点的距离，转化为 XYZ 坐标系后计算差值和模长，同时计算了这两个点在 ENU 坐标系中的坐标。