大地直角坐标转gps坐标代码c++
时间: 2023-10-23 20:23:00 浏览: 45
以下是C++代码,可以将大地直角坐标转换为GPS坐标:
```c++
#include <iostream>
#include <math.h>
using namespace std;
const double a = 6378137.0; // 长半轴
const double b = 6356752.3142; // 短半轴
const double f = (a - b) / a; // 扁率
const double e = sqrt(2 * f - f * f); // 第一偏心率
const double e2 = e * e;
const double pi = 3.1415926535898; // 圆周率
double rad(double d)
{
return d * pi / 180.0;
}
void geodetic2ecef(double B, double L, double H, double &X, double &Y, double &Z)
{
double N = a / sqrt(1 - e2 * sin(rad(B)) * sin(rad(B)));
X = (N + H) * cos(rad(B)) * cos(rad(L));
Y = (N + H) * cos(rad(B)) * sin(rad(L));
Z = (N * (1 - e2) + H) * sin(rad(B));
}
void ecef2enu(double X, double Y, double Z, double B0, double L0, double &E, double &N, double &U)
{
double cosB0 = cos(rad(B0));
double sinB0 = sin(rad(B0));
double cosL0 = cos(rad(L0));
double sinL0 = sin(rad(L0));
double X0 = -(sinL0 * X) + (cosL0 * Y); // 旋转矩阵
double Y0 = -(sinB0 * cosL0 * X) - (sinB0 * sinL0 * Y) + (cosB0 * Z);
double Z0 = (cosB0 * cosL0 * X) + (cosB0 * sinL0 * Y) + (sinB0 * Z);
E = -sinL0 * X0 + cosL0 * Y0;
N = -sinB0 * cosL0 * X0 - sinB0 * sinL0 * Y0 + cosB0 * Z0;
U = cosB0 * cosL0 * X0 + cosB0 * sinL0 * Y0 + sinB0 * Z0;
}
void geodetic2gps(double B, double L, double H, double &lat, double &lon, double &alt)
{
double X, Y, Z, E, N, U;
geodetic2ecef(B, L, H, X, Y, Z);
ecef2enu(X, Y, Z, B, L, E, N, U);
double R = sqrt(E * E + N * N);
double cosA = E / R;
double sinA = N / R;
double K = sqrt(1 - e2 * sin(rad(B)) * sin(rad(B)));
double Kx = cosA * cos(B) * K;
double Ky = sinA * cos(B) * K;
double Kz = sin(B) * K;
double delta = 1e-9;
if (fabs(Kx) < delta && fabs(Ky) < delta)
{
lat = B;
lon = L;
}
else
{
lat = atan(Kz / sqrt(Kx * Kx + Ky * Ky));
lon = atan2(Ky, Kx);
}
double N2 = a / sqrt(1 - e2 * sin(lat) * sin(lat));
alt = R / cos(lat) - N2;
lat = lat * 180.0 / pi;
lon = lon * 180.0 / pi;
}
int main()
{
double B = 39.908859;
double L = 116.397392;
double H = 20.0;
double lat, lon, alt;
geodetic2gps(B, L, H, lat, lon, alt);
cout << "GPS Coordinates: " << lat << ", " << lon << ", " << alt << endl;
return 0;
}
```
其中,`geodetic2ecef`函数将地心经纬度和高程转换为地心直角坐标系(ECEF)坐标,`ecef2enu`函数将ECEF坐标转换为东北天(ENU)坐标,`geodetic2gps`函数将地心经纬度和高程转换为GPS坐标。