我们知道GPS坐标是由经度,纬度,海拔组成,精度和纬度都是角度,海报是高度。
在进行基于地理的搜索的时候,常用到KDTree,在构建在KDTree的时候,不能直接用GPS的坐标,要将GPS坐标转换成笛卡尔坐标才能用于构建KDTree。下面就是相关的转换算法。
注:GPS信息由几种标准,这里的采用的是google map的经纬度信息。
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 ; } // ======================================================================= //-->
就是根据将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