c语言下建立直角坐标系,大地坐标系和地心地固直角坐标ECEF转换公式和C语言函数代码...

ECEF(x,y,z)转成大地坐标系(φ,λ,h)

λ=tan^(-1)⁡〖(y/x)〗

h=p/cos⁡∅ -N

∅=tan^(-1)⁡〖[z/p 〖(1-e^2 N/(N+h))〗^(-1)]〗

p=√(x2+y2 )

#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */

#define FE_WGS84 (1.0/298.257223563) /* earth flattening (WGS84) */

#define PI 3.1415926535897932 /* pi */

/* inner product ---------------------------------------------------------------

* inner product of vectors

* args : double *a,*b I vector a,b (n x 1)

* int n I size of vector a,b

* return : a'*b

*-----------------------------------------------------------------------------*/

extern double dot(const double *a, const double *b, int n)

{

volatile double c=0.0000;

while (--n>=0) c+=a[n]*b[n];

return c;

}

/* transform ecef to geodetic postion ------------------------------------------

* transform ecef position to geodetic position

* args : double *r I ecef position {x,y,z} (m)

* double *pos O geodetic position {lat,lon,h} (rad,m)

* return : none

* notes : WGS84, ellipsoidal height

*-----------------------------------------------------------------------------*/

extern void ecef2pos(const double *r, double *pos)

{

double e2=FE_WGS84*(2.0-FE_WGS84),r2=dot(r,r,2),z,zk,v=RE_WGS84,sinp;

for (z=r[2],zk=0.0;fabs(z-zk)>=1E-4;) {

zk=z;

sinp=z/sqrt(r2+z*z);

v=RE_WGS84/sqrt(1.0-e2*sinp*sinp);

z=r[2]+v*e2*sinp;

}

pos[0]=r2>1E-12?atan(z/sqrt(r2)):(r[2]>0.0?PI/2.0:-PI/2.0);

pos[1]=r2>1E-12?atan2(r[1],r[0]):0.0;

pos[2]=sqrt(r2+z*z)-v;

}