c++BLH转换ENU
时间: 2023-08-04 19:13:37 浏览: 54
BLH(B:纬度,L:经度,H:高度)坐标系转换为ENU(E:东向,N:北向,U:天向)坐标系的方法如下:
1. 首先需要将BLH坐标系转换为地心直角坐标系(ECEF坐标系)。
2. 然后,计算出BLH坐标系的切平面与ECEF坐标系的交点,得到参考点的坐标。
3. 将所求点的ECEF坐标系减去参考点的ECEF坐标系得到所求点相对于参考点的ECEF坐标系坐标。
4. 最后,将相对于参考点的ECEF坐标系坐标转换为ENU坐标系坐标。
C++代码实现如下:
```cpp
#include <cmath>
// 地球半径
const double R = 6378137.0;
// 弧度转角度
double rad2deg(double rad) {
return rad / M_PI * 180.0;
}
// 角度转弧度
double deg2rad(double deg) {
return deg / 180.0 * M_PI;
}
// 计算地球表面两点间的距离
double distance(double lat1, double lng1, double lat2, double lng2) {
double radLat1 = deg2rad(lat1);
double radLng1 = deg2rad(lng1);
double radLat2 = deg2rad(lat2);
double radLng2 = deg2rad(lng2);
double a = sin((radLat1 - radLat2) / 2.0) * sin((radLat1 - radLat2) / 2.0) + cos(radLat1) * cos(radLat2) * sin((radLng1 - radLng2) / 2.0) * sin((radLng1 - radLng2) / 2.0);
double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));
return R * c;
}
// BLH坐标系转ECEF坐标系
void blh2ecef(double lat, double lng, double alt, double &x, double &y, double &z) {
double radLat = deg2rad(lat);
double radLng = deg2rad(lng);
double N = R / sqrt(1.0 - 0.00669437999014 * sin(radLat) * sin(radLat));
x = (N + alt) * cos(radLat) * cos(radLng);
y = (N + alt) * cos(radLat) * sin(radLng);
z = (N * (1.0 - 0.00669437999014) + alt) * sin(radLat);
}
// ECEF坐标系转ENU坐标系
void ecef2enu(double lat, double lng, double alt, double &x, double &y, double &z, double refLat, double refLng, double refAlt) {
double radRefLat = deg2rad(refLat);
double radRefLng = deg2rad(refLng);
double radLat = deg2rad(lat);
double radLng = deg2rad(lng);
double dx = (R + refAlt) * cos(radRefLat) * sin(radLng - radRefLng);
double dy = (R + refAlt) * (sin(radLat) * cos(radRefLat) - cos(radLat) * sin(radRefLat) * cos(radLng - radRefLng));
double dz = (R + refAlt) * (cos(radLat) * cos(radRefLat) * cos(radLng - radRefLng) + sin(radLat) * sin(radRefLat));
double x0, y0, z0;
blh2ecef(refLat, refLng, refAlt, x0, y0, z0);
x = dx - (x - x0);
y = dy - (y - y0);
z = dz - (z - z0);
}
// BLH坐标系转ENU坐标系
void blh2enu(double lat, double lng, double alt, double &x, double &y, double &z, double refLat, double refLng, double refAlt) {
double x0, y0, z0;
blh2ecef(lat, lng, alt, x, y, z);
ecef2enu(lat, lng, alt, x, y, z, refLat, refLng, refAlt);
}
```
其中,`lat`、`lng`、`alt`为待转换的BLH坐标系坐标,`x`、`y`、`z`为转换后的ENU坐标系坐标,`refLat`、`refLng`、`refAlt`为参考点的BLH坐标系坐标。