//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);

}