平面坐标转经纬度代码_轻量易学BLHXYZ坐标形式互转代码

【写在最前】

常用伙伴问你：有坐标系转换工具吗，转换方法是什么，客户想用~

你的回答：有，工具网很多；算法在GPS工具书《GPS测量与数据处理》(第二版)第296页-网平差原理及质量控制，详述基本数学模型：空间直角坐标系与大地坐标间的微分关系、空间直角坐标与站心直角坐标间的转换。

然后，伙伴横扫坐标系转换公式、WHAT卯酉圈(Prime Vertical)？这怎么搞，然后就没有然后了

[小朋友，你是不是有很多问号？？？]

这不是我们想要的结局，那就来点干货-轻量代码，110行，直接用，一看就会【公式还豪横吗？】

【正文】

一、方法

//1.经纬高-XYZ形式互转，计算用户位置/原点向量ECEF.

ECEF2blh();

blh2ECEF();

//2.XYZ向量转ENU

ecef2enu();

二、轻量代码-130行(参考rtklib源码)

#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 */

#define D2R         (PI/180.0)          /* 度转弧度 deg to rad */

#define R2D         (180.0/PI)          /* 弧度转度 rad to deg */

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

{

    double c=0.0;    

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

    return c;

}

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

* transform ecef position to geodetic position

* 将XYZ转换为经纬高BLH

* args   : double *r        输入I   ecef position {x,y,z} (XYZ数组,单位：m) 

*          double *pos      输出O   geodetic position {lat,lon,h} (BLH数组,单位：度gre,m)

* return : none

* 测试用例

* 输入ECEF -2160908.9091,4384024.5909,4084144.5367

* 输出BLH 40.071723677 ,116.238931614 ,95.531821

* notes  : WGS84, ellipsoidal height

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

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

{

  double FE_WGS84 =  (1.0/298.257223563);

  double RE_WGS84 =  6378137.0;

  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;

    }

     //计算BLH，其中pos[0]为纬度B，pos[1]为经度L，pos[2]为H。

    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;

     //弧度专为度单位

    pos[0]=pos[0]*R2D;

    pos[1]=pos[1]*R2D;

}

/* transform geodetic to ecef position -----------------------------------------

* transform geodetic position to ecef position

* args   : double *pos      输入I  geodetic position {lat,lon,h} (经纬高，单位：度,m)

*           double *r          输出O ecef position {x,y,z} (XYZ,单位:m)

* return : none

* BLH转XYZ

* 测试用例

* 输入BLH 40.071723677 ,116.238931614 ,95.531821

* 输出ECEF -2160908.9091,4384024.5909,4084144.5367

* notes  : WGS84, ellipsoidal height

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

extern void blh2ECEF(const double *blh, double *r)

{

    double pos[3]={0,0,0};

    /* 度转弧度 */

    pos[0]=blh[0]*D2R;

    pos[1]=blh[1]*D2R;

    pos[2]=blh[2];

    double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]);

    double e2=FE_WGS84*(2.0-FE_WGS84),v=RE_WGS84/sqrt(1.0-e2*sinp*sinp);

    //计算XYZ

    r[0]=(v+pos[2])*cosp*cosl;

    r[1]=(v+pos[2])*cosp*sinl;

    r[2]=(v*(1.0-e2)+pos[2])*sinp;

}

/* 矩阵相乘 multiply matrix -----------------------------------------------------------*/

extern void matmul(const char *tr, int n, int k, int m, double alpha,

                   const double *A, const double *B, double beta, double *C)

{

    double d;

    int i,j,x,f=tr[0]=='N'?(tr[1]=='N'?1:2):(tr[1]=='N'?3:4);

    for (i=0;i

        d=0.0;

        switch (f) {

            case 1: for (x=0;x

            case 2: for (x=0;x

            case 3: for (x=0;x

            case 4: for (x=0;x

        }

        if (beta==0.0) C[i+j*n]=alpha*d; else C[i+j*n]=alpha*d+beta*C[i+j*n];

    }

}

/* ECEF转ENU参数 ecef to local coordinate transfromation matrix ------------------------------

* compute ecef to local coordinate transfromation matrix

* args   : double *pos      I   geodetic position {lat,lon} (rad)

*          double *E        O   ecef to local coord transformation matrix矩阵 (3x3)

* return : none

* notes  : matirix stored by column-major order (fortran convention)

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

extern void xyz2enu(const double *pos, double *E)

{

    double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]);   

    E[0]=-sinl;      E[3]=cosl;       E[6]=0.0;

    E[1]=-sinp*cosl; E[4]=-sinp*sinl; E[7]=cosp;

    E[2]=cosp*cosl;  E[5]=cosp*sinl;  E[8]=sinp;

}

/* transform ecef vector to local tangental coordinate -------------------------

* transform ecef vector to local tangental coordinate

* args   : double *pos      I   geodetic position {lat,lon} (rad) 原点经纬度(单位：弧度、弧度、m)

*          double *r        I   vector in ecef coordinate {x,y,z} 用户位置与原点向量ECEF(单位：m)

*          double *e        O   vector in local tangental coordinate {e,n,u} 返回值：ENU(单位：m)

* return : none

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

extern void ecef2enu(const double *pos, const double *r, double *e)

{

    double E[9];  

    xyz2enu(pos,E);

    matmul("NN",3,1,3,1.0,E,r,0.0,e);

}

你不是很豪横吗？

[下载链接]

正文代码链接

链接: https://pan.baidu.com/s/1Pwmj53-FaD4tkeoLXfCFQg 提取码: bh8c

坐标系转换小工具(笑脸)链接：

链接: https://pan.baidu.com/s/1fdkUGhYZOpS4ob5pqDSmyw 提取码: 13r6