//ECEF ---> WGS84
//pcg为WGS-84坐标系结构体指针，pcc为ECEF坐标系结构体指针
void ECEFToWGS(PWGS pcg, PECEF pcc)
{
double B0, R, N;
double B_, L_;
double X = pcc->x;
double Y = pcc->y;
double Z = pcc->z;
R = sqrt(X * X + Y * Y);
B0 = atan2(Z, R);
while (1)
{
N = dSemiMajorAxis / sqrt(1.0 - e2 * sin(B0) * sin(B0));
B_ = atan2(Z + N * e2 * sin(B0), R);
if (fabs(B_ - B0) < 1.0e-10)
break;
B0 = B_;
}
L_ = atan2(Y, X);
pcg->height = R / cos(B_) - N;
//弧度转换成经纬度
pcg->latitude = B_ * 180 / M_PI;
pcg->longitude = L_ * 180 / M_PI;
}
//ECEF ---> ENU
//pcc为ECEF坐标系结构体指针，center为东北天坐标原点的指针，pct为东北天坐标系结构体指针
//坐标原点center要用GPS采到的第一个点的数据
void ECEFToENU(PECEF pcc, PWGS center, PENU pct)
{
double dX, dY, dZ;
PECEF Geodetic;
Geodetic = (PECEF)malloc(sizeof(ECEF));
WGSToECEF(center, Geodetic);
dX = pcc->x - Geodetic->x;
dY = pcc->y - Geodetic->y;
dZ = pcc->z - Geodetic->z;
double B, L, H;
B = center->latitude;
L = center->longitude;
H = center->height;
pct->easting = -sin(L) * dX + cos(L) * dY; //X轴
pct->northing = -sin(B) * cos(L) * dX - sin(B) * sin(L) *
dY + cos(B) * dZ; //Y轴
pct->upping = cos(B) * cos(L) * dX + cos(B) * sin(L) * dY +
sin(B) * dZ; //Z轴
free(Geodetic);
}
//ENU ---> ECEF
//pcc为ECEF坐标系结构体指针，center为东北天坐标原点的指针，pct为东北天坐标系结构体指针
//坐标原点center要用GPS采到的第一个点的数据
void ENUToECEF(PECEF pcc, PWGS center, PENU pct)
{
PECEF Geodetic;
Geodetic = (PECEF)malloc(sizeof(ECEF));
WGSToECEF(center, Geodetic);
double B, L, H;
B = center->latitude;
L = center->longitude;
H = center->height;
pcc->x = -sin(B) * cos(L) * pct->northing - sin(L) *
pct->easting + cos(B) * cos(L) * pct->upping +
Geodetic->x;
pcc->y = -sin(B) * sin(L) * pct->northing + cos(L) *
pct->easting + cos(B) * sin(L) * pct->upping +
Geodetic->y;
pcc->z = cos(B) * pct->northing + sin(B) *
pct->upping + Geodetic->z;
free(Geodetic);
}
转播
