首页 > 代码库 > GPS坐标转笛卡尔坐标

GPS坐标转笛卡尔坐标

提要

我们知道GPS坐标是由经度,纬度,海拔组成,精度和纬度都是角度,海报是高度。

在进行基于地理的搜索的时候,常用到KDTree,在构建在KDTree的时候,不能直接用GPS的坐标,要将GPS坐标转换成笛卡尔坐标才能用于构建KDTree。下面就是相关的转换算法。

注:GPS信息由几种标准,这里的采用的是google map的经纬度信息。


js实现

geodecy.js

/*             geodesy routines in JavaScript
                 James R. Clynch NPS / 2003
               
          Done for support of web education pages


          == must convert inputs to numbers for safety ==
          == if string comes in - sometimes works, sometimes not !==
*/



<!--

// =======================================================================

     function geodGBL()

//             test and ensure geodesy globals loaded
     {

     var  tstglobal

     tstglobal = typeof EARTH_A;
     if ( tstglobal == "undefined" )  wgs84() 

     }

// =======================================================================

       function earthcon(ai,bi)

/*        Sets Earth Constants as globals
             --  input a,b
             --  Leaves Globals 
                 EARTH_A      EARTH_B   EARTH_F  EARTH_Ecc
*/

     {
           var  f,ecc, eccsq, a,b

           a        =  Number(ai);
           b        =  Number(bi);

           f        =  1-b/a;
           eccsq    =  1 - b*b/(a*a);
           ecc      =  Math.sqrt(eccsq);

           EARTH_A  =  a;
           EARTH_B  =  b;
           EARTH_F  =  f;
           EARTH_Ecc=  ecc;
           EARTH_Esq=  eccsq;
     }


// =======================================================================

     function wgs84()
   
/*        WGS84 Earth Constants
             --  returns a,b,f,e  --
             --  Leaves Globals 
                 EARTH_A      EARTH_B   EARTH_F  EARTH_Ecc

*/

     {
          var  wgs84a, wgs84b, wgs84f

          wgs84a         =  6378.137;
          wgs84f         =  1.0/298.257223563;
          wgs84b         =  wgs84a * ( 1.0 - wgs84f );

          earthcon (wgs84a, wgs84b );

     }          


// =======================================================================

    function  radcur(lati)

/*
       compute the radii at the geodetic latitude lat (in degrees)
     
     input:
               lat       geodetic latitude in degrees
     output:   
               rrnrm     an array 3 long
                         r,  rn,  rm   in km

*/

{

     var rrnrm = new Array(3)

     var dtr   = Math.PI/180.0

     var  a,b,lat
     var  asq,bsq,eccsq,ecc,clat,slat
     var  dsq,d,rn,rm,rho,rsq,r,z

//        -------------------------------------

     geodGBL();

     a     = EARTH_A;
     b     = EARTH_B;

     asq   = a*a;
     bsq   = b*b;
     eccsq  =  1 - bsq/asq;
     ecc = Math.sqrt(eccsq);

     lat   =  Number(lati);

     clat  =  Math.cos(dtr*lat);
     slat  =  Math.sin(dtr*lat);

     dsq   =  1.0 - eccsq * slat * slat;
     d     =  Math.sqrt(dsq);

     rn    =  a/d;
     rm    =  rn * (1.0 - eccsq ) / dsq;

     rho   =  rn * clat;
     z     =  (1.0 - eccsq ) * rn * slat;
     rsq   =  rho*rho + z*z;
     r     =  Math.sqrt( rsq );

     rrnrm[0]  =  r;
     rrnrm[1]  =  rn;
     rrnrm[2]  =  rm;

     return ( rrnrm );

   }

// =======================================================================

//        physical radius of earth from geodetic latitude

     function  rearth (lati)
     {
          var    rrnrm, r,lat

          lat   =  Number(lati);

          rrnrm =  radcur ( lat );
          r     =  rrnrm[0];

          return ( r );

     }


// =======================================================================

     function  gc2gd (flatgci, altkmi )

/*        geocentric latitude to geodetic latitude

     Input:
               flatgc    geocentric latitude deg.
               altkm     altitide in km
     ouput:
               flatgd    geodetic latitude in deg

*/

     {

     var dtr   = Math.PI/180.0;
     var rtd   = 1/dtr;

     var  flatgd,flatgc,altkm
     var  rrnrm = new Array(3)
     var  re,rn,ecc, esq;
     var  slat,clat,tlat
     var  altnow,ratio

     geodGBL();

     flatgc=  Number(flatgci);
     altkm =  Number(altkmi);
     
     ecc   =  EARTH_Ecc;
     esq   =  ecc*ecc;

//             approximation by stages
//             1st use gc-lat as if is gd, then correct alt dependence

     altnow  =  altkm;

     rrnrm   =  radcur (flatgc);
     rn      =  rrnrm[1];
     
     ratio   = 1 - esq*rn/(rn+altnow);

     tlat    = Math.tan(dtr*flatgc) / ratio;
     flatgd  = rtd * Math.atan(tlat);

//        now use this approximation for gd-lat to get rn etc.

     rrnrm   =  radcur ( flatgd );
     rn      =  rrnrm[1];

     ratio   =  1  - esq*rn/(rn+altnow)
     tlat    =  Math.tan(dtr*flatgc)/ratio;
     flatgd  =  rtd * Math.atan(tlat);

     return  flatgd

     }


// =======================================================================


     function  gd2gc (flatgdi, altkmi )

/*        geodetic latitude to geocentric latitude

     Input:
               flatgd    geodetic latitude deg.
               altkm     altitide in km
     ouput:
               flatgc    geocentric latitude in deg

*/

     {

     var dtr   = Math.PI/180.0;
     var rtd   = 1/dtr;

     var  flatgc,flatgd,altkm
     var  rrnrm = new Array(3)
     var  re,rn,ecc, esq;
     var  slat,clat,tlat
     var  altnow,ratio

     geodGBL();

     flatgd=  Number(flatgdi);
     altkm =  Number(altkmi);
     
     ecc   =  EARTH_Ecc;
     esq   =  ecc*ecc;

     altnow  =  altkm;

     rrnrm   =  radcur (flatgd);
     rn      =  rrnrm[1];
     
     ratio   = 1 - esq*rn/(rn+altnow);

     tlat    = Math.tan(dtr*flatgd) * ratio;
     flatgc  = rtd * Math.atan(tlat);

     return  flatgc;

     }


// =======================================================================

     function  llenu ( flati,floni)

/*        latitude longitude to east,north,up unit vectors

     input:
               flat      latitude in degees N
                         [ gc -> gc enu,  gd usual enu ]
               flon      longitude in degrees E
     output:
               enu[3[3]]  packed 3-unit vectors / each a 3 vector

*/

     {

     var  flat,flon;
     var  dtr,clat,slat,clon,slon  ;
     var  ee = new Array(3);
     var  en = new Array(3);
     var  eu = new Array(3);

     var  enu = new Array(3);

     var dtr   = Math.PI/180.0


//             --------------------------------

     flat = Number(flati);
     flon = Number(floni);

     clat = Math.cos(dtr*flat);
     slat = Math.sin(dtr*flat);
     clon = Math.cos(dtr*flon);
     slon = Math.sin(dtr*flon);

     ee[0]  =  -slon;
     ee[1]  =   clon;
     ee[2]  =   0.0;

     en[0]  =  -clon * slat;
     en[1]  =  -slon * slat;
     en[2]  =          clat;

     eu[0]  =   clon * clat;
     eu[1]  =   slon * clat;
     eu[2]  =          slat;

     enu[0] =  ee;
     enu[1] =  en;
     enu[2] =  eu;

     return  enu ;

     }

// =======================================================================

       function llhxyz (flati,floni, altkmi )

/*        lat,lon,height to xyz vector

     input:
          flat      geodetic latitude in deg
          flon      longitude in deg
          altkm     altitude in km
     output:
          returns vector x 3 long ECEF in km

*/

     {

      var  dtr =  Math.PI/180.0;
      var  flat,flon,altkm;
      var  clat,clon,slat,slon;
      var  rrnrm = new Array(3);
      var  rn,esq;
      var  x,y,z;
      var  xvec = new Array(3);

     geodGBL();

     flat  = Number(flati);
     flon  = Number(floni);
     altkm = Number(altkmi);

     clat = Math.cos(dtr*flat);
     slat = Math.sin(dtr*flat);
     clon = Math.cos(dtr*flon);
     slon = Math.sin(dtr*flon);
          
     rrnrm  = radcur (flat);
     rn     = rrnrm[1];
     re     = rrnrm[0];

     ecc    = EARTH_Ecc;
     esq    = ecc*ecc

     x      =  (rn + altkm) * clat * clon;
     y      =  (rn + altkm) * clat * slon;
     z      =  ( (1-esq)*rn + altkm ) * slat;

     xvec[0]  = x;
     xvec[1]  = y;
     xvec[2]  = z;

     return  xvec ;

     }



// =======================================================================

       function xyzllh ( xvec )

/*        xyz vector  to  lat,lon,height

     input:
          xvec[3]   xyz ECEF location
     output:

          llhvec[3] with components

          flat      geodetic latitude in deg
          flon      longitude in deg
          altkm     altitude in km

*/

     {

      var  dtr =  Math.PI/180.0;
      var  flatgc,flatn,dlat;
      var  rnow,rp;
      var  x,y,z,p;
      var  tangc,tangd;

      var  testval,kount;

      var  rn,esq;
      var  clat,slat;
      var  rrnrm = new Array(3);

      var  flat,flon,altkm;
      var  llhvec = new Array(3);


     geodGBL();

     esq    =  EARTH_Esq;

     x      = xvec[0];
     y      = xvec[1];
     z      = xvec[2];

     x      = Number(x);
     y      = Number(y);
     z      = Number(z);

     rp     = Math.sqrt ( x*x + y*y + z*z );

     flatgc = Math.asin ( z / rp )/dtr;

     testval= Math.abs(x) + Math.abs(y);
     if ( testval < 1.0e-10)
         {flon = 0.0 }
     else
         {flon = Math.atan2 ( y,x )/dtr } 
     if (flon < 0.0 )  { flon = flon + 360.0 }

     p      =  Math.sqrt( x*x + y*y );

//             on pole special case

     if ( p < 1.0e-10 )
       {  
          flat = 90.0
          if ( z < 0.0 ) { flat = -90.0 }

          altkm = rp - rearth(flat);
          llhvec[0]  = flat;
          llhvec[1]  = flon;
          llhvec[2]  = altkm;

          return  llhvec;
        }

//        first iteration, use flatgc to get altitude 
//        and alt needed to convert gc to gd lat.

     rnow  =  rearth(flatgc);
     altkm =  rp - rnow;
     flat  =  gc2gd (flatgc,altkm);
          
     rrnrm =  radcur(flat);
     rn    =  rrnrm[1];

     for ( var kount = 0; kount< 5 ; kount++ )
       {
           slat  =  Math.sin(dtr*flat);
           tangd =  ( z + rn*esq*slat ) / p;
           flatn =  Math.atan(tangd)/dtr;

           dlat  =  flatn - flat;
           flat  =  flatn;
           clat  =  Math.cos( dtr*flat );

           rrnrm =  radcur(flat);
           rn    =  rrnrm[1];

           altkm =  (p/clat) - rn;

           if ( Math.abs(dlat) < 1.0e-12 ) { break }

       }
     
          llhvec[0]  = flat;
          llhvec[1]  = flon;
          llhvec[2]  = altkm;

          return  llhvec ;

     }


// =======================================================================

//-->


C++实现

就是根据将JS代码转成C艹.

GPS 转笛卡尔

void GPSTransform::gpsPoint2DescartesPoint(const double latitude, const double longitude, const double altitude, double &x, double &y, double &z)
{
	//wgs84 WGS84 Earth Constants
	double wgs84a = 6378.137;
	double wgs84f = 1.0 / 298.257223563;
	double wgs84b = wgs84a * (1.0 - wgs84f);

	//earthcon
	double f = 1 - wgs84b / wgs84a;
	double eccsq = 1 - (wgs84b* wgs84b) / (wgs84a * wgs84a);
	double ecc = sqrt(eccsq);
	double esq = ecc * ecc;

	//llhxyz
	double dtr = M_PI / 180.0;
	//qDebug() << dtr << gpsPoint.latitude << endl;
	double clat = cos(dtr * latitude);
	double slat = sin(dtr * latitude);
	double clon = cos(dtr * longitude);
	double slon = sin(dtr * longitude);
	//qDebug() << clat << slon << endl;

	//radcur compute the radii at the geodetic latitude lat (in degrees)
	double dsq = 1.0 - eccsq * slat *slat;
	double d = sqrt(dsq);
	//qDebug() << d;
	double rn = wgs84a / d;
	double rm = rn * (1.0 - eccsq) / dsq;

	double rho = rn * clat;
	double zz = (1.0 - eccsq) * rn *slat;
	double rsq = rho * rho + zz*zz;
	double r = sqrt(rsq);

	x = (rn + altitude) * clat * clon;
	y = (rn + altitude) * clat * slon;
	z = ((1 - esq)*rn + altitude) * slat;
}


运行结果





验证网站

Geodetic to Cartesian Converter - http://www.apsalin.com/convert-geodetic-to-cartesian.aspx

atitude,Longitude,Height to/from ECEF (X,Y,Z) - http://www.oc.nps.edu/oc2902w/coord/llhxyz.htm



GPS坐标转笛卡尔坐标