#include #include #include #include //------------------------------------------------------------------- // This code is the C++ code released in conjunction with // the publication // Hehl(2005): C++ and Java code for recursion formulas in mathematical geodesy. // GPS Solutions, Springer, Vol. 9, pp. 51-58. // algorithm is NOT limited by accuracy or length of geodesic // // contributed by: // Dr. Klaus Hehl, University of Applied Sciences, Berlin, Germany // (c) hehl@tfh-berlin.de, May 2005 // //------------------------------------------------------------------- typedef double REAL; const REAL RHODEG = 180./M_PI; //------------------------------------------------------------------- template T ln(T x) { return(log(x)/log(M_E)); } // end ln() complex tanh(complex z) { return((exp(2.*z)-1.)/(exp(2.*z)+1.)); } // end tanh() complex asin(complex z) { complex unit(0.,1.); return(-unit * ln(unit*z + sqrt(1.-z*z))); } // end asin() template T atanh(T z) { return(0.5*ln((1.+z)/(1.-z))); } // end atanh() complex tan(complex z) { return(sin(z)/cos(z)); } // end tan() complex atan(complex z) { complex unit(0.,1.); return(0.5/unit*ln((1.+unit*z)/(1.-unit*z))); } // end atan() //------------------------------------------------------------------- class GL { // this class implements the calculation of geodesics on an ellipsoid // using recursions (theory by J. Klotz, 1993) // re-formulation, C++ template and Java-code by hehl@tfh-berlin.de // class GL is the basis for the calculation of // 1) meridian arc lengths from latitude (back and forth), h = 0 // 2) direct and reverse problem, h <> 0, abs(h) <= 1 // 3) Gauss-Krueger and UTM (using complex calls in meridian computation, h=0) // 4) Soldner coordinates public: GL(REAL a = 6377397.155, REAL f = 1./299.15281285, // default is Bessel ellipsoid REAL h = 0.0, // default is meridian int N = N_MAX, int n = -1); GL(REAL a, REAL f, REAL phi1, REAL phi2, REAL dlam, int N = N_MAX); GL(REAL a, REAL f, REAL phi, REAL dlam, int N = N_MAX); int order(void) { return(_N); } REAL h(void) { return(_h); } REAL a(void) { return(_a); } REAL f(void) { return(_f); } REAL e2(void) { return(f()*(2.-f())); } REAL alpha0(void) { return(asin(h())); } REAL betaMax(void) { return(acos(h())); } REAL koeff1(void) { return(_k2[0]+sumc()); } REAL koeff3(void) { return(_k4[0]+sumc()-1.); } REAL sMax(void) { return(a()*M_PI/2.*koeff1()); } REAL LamMax(void) { return(M_PI/2.*(1.+h()*koeff3())); } template T koeff2(T v); template T koeff4(T v); template T arc(T b); template T lat(T G); complex philam2b(complex); complex b2philam(complex); REAL Lam(REAL phi); void print_k24(void); private: REAL binom(int n, int m); // binomial coefficients REAL koeff_c(int n, REAL e2); REAL koeff_d(int n, REAL h2); template T koeff_k(int n, T s2); REAL & sumc(void) { return(_k4[order()-1]); } void koeff(int = -1); // recursion void koeff_loop(void); // using for()-loop private: int _N; // selected order REAL _h,_a,_f; // parameters of geodesic, and ellipsoid enum { N_MAX = 8 }; REAL _k2[N_MAX],_k4[N_MAX]; // internal coefficients }; // end class GL //------------------------------------------------------------------- void deg2dms(REAL x, int & d, int & m, REAL & s) { s = fabs(fmod(3600.*x,60.-1.E-12)); d = (int)floor(x); m = (int)floor((x-d)*60.-s/60.+5.E-10); } // deg2dms char * cdeg2dms(REAL x) { static char cdms[25];// = "+123d 45m 6.78910s"; int d,m; REAL s; deg2dms(x,d,m,s); sprintf(cdms,"%5dd%3dm%10.6lfs",d,m,(double)s); return(cdms); } void direct_problem(REAL a, REAL f, REAL phi1, REAL lam1, REAL s12, REAL alpha1, REAL & phi2, REAL & lam2, REAL & alpha2) { REAL beta1 = atan(tan(phi1)*(1.-f)); REAL h = sin(alpha1)*cos(beta1); GL HA1(a,f,h); // --- REAL s2 = HA1.arc(phi1) + s12; phi2 = HA1.lat(s2); REAL beta2 = atan(tan(phi2)*(1.-f)); alpha2 = h/cos(beta2); lam2 = lam1 + (HA1.Lam(phi2)-HA1.Lam(phi1)); } void indirect_problem(REAL a, REAL f, REAL phi1, REAL lam1, REAL phi2, REAL lam2, REAL & s12, REAL & alpha1, REAL & alpha2) { GL HA2(a,f,phi1,phi2,lam2-lam1); // --- REAL beta1 = atan(tan(phi1)*(1.-f)); REAL beta2 = atan(tan(phi2)*(1.-f)); alpha1 = asin(HA2.h()/cos(beta1)); alpha2 = asin(HA2.h()/cos(beta2)); s12 = HA2.arc(phi2)-HA2.arc(phi1); } void ell2GKUTM(REAL a, REAL f, REAL phi, REAL dlam, REAL & east, REAL & north, REAL scale = 1.0) { GL meridian(a,f,0.0); // --- complex b = meridian.philam2b(complex(phi,dlam)); complex GK = scale*meridian.arc(b); north = real(GK); east = imag(GK); } void GKUTM2ell(REAL a, REAL f, REAL east, REAL north, REAL & phi, REAL & dlam,REAL scale = 1.0) { GL meridian(a,f,0.0); // --- complex GK(north,east); complex b = meridian.lat(GK/scale); complex philam = meridian.b2philam(b); phi = real(philam); dlam = imag(philam); } void tests(void) { char line[512]; REAL a = 6378000.0, // artificial data for checking formulas f = 1.-sqrt(1-0.008); // e2 = 0.008 REAL h = 1./3.*sqrt(3.); REAL phi = atan(tan(45./RHODEG)/(1.-f)); // beta = 45 deg GL testline(a,f,h,8); // --- cout << "Tests:" << endl; sprintf(line," %s a= %.5lf m", testline.f() ? "ellipsoid" : "sphere",(double)testline.a()); cout << line; if(testline.e2()) { sprintf(line,", e2=%.8E, 1/f=%.1lf",(double)testline.e2(), double(1./testline.f())); cout << line; } cout << endl; cout << " N= " << testline.order() << ", h= " << testline.h() << endl; cout << " alpha0= " << cdeg2dms(testline.alpha0()*RHODEG); cout << ", betamax= " << cdeg2dms(testline.betaMax()*RHODEG) << endl; testline.print_k24(); sprintf(line," koeff1=%25.18E",(double)testline.koeff1()); cout << line << endl; sprintf(line," s_max=%25.18lf m",testline.sMax()); cout << line << endl; sprintf(line," s(beta=45deg) =%25.12lf m", (double)testline.arc(phi)); cout << line << endl; sprintf(line," Lam(beta=45deg)=%25.12lf deg", double(RHODEG*testline.Lam(phi))); cout << line << endl; GL iter(6378388.0,1./297., 52./RHODEG,(52.+3./60.)/RHODEG, (6./60.+44.315445/3600.0)/RHODEG); cout << " h_iter= " << iter.h() << ", (error= " << (0.5-iter.h()) << ")" << endl; cout << endl; } void meridian_arclength(void) { char line[512]; int d,m; REAL s; REAL a = 6377397.155, f = 1./299.15281285; // Bessel GL meridian(a,f,0.0); // --- cout << "(1) calculate arc length of meridian " << " (after Hofmann-Wellenhof), BESSEL" << endl; REAL phi = (47.+7./60.+10.19550/3600.); deg2dms(phi,d,m,s); REAL beta = atan(tan(phi/RHODEG)*(1.-meridian.f())); REAL v = asin(sin(beta)/sqrt(1.-meridian.h()*meridian.h())); sprintf(line,"arc(): v=%.12lf deg, koeff2=%.25E", double(RHODEG*v),(double)meridian.koeff2(v)); // cout << line << endl; sprintf(line," G(%3dd %02dm %011.8lfs)= %.8lf m", d,m,double(s),(double)meridian.arc(phi/RHODEG)); cout << line << endl; sprintf(line," quadrant=%18.8lf m",(double)meridian.sMax()); // 123 cout << line << endl; REAL G = 5220000.55136; REAL phi_new = meridian.lat(G); sprintf(line," check: phi-phi_new= %.E deg", double(phi-phi_new*RHODEG)); cout << line << endl; cout << endl; } // end meridian_arclength() void GKUTM_coordinates(void) { char line[512]; cout << "(2) Gauss-Krueger- / UTM- coordinates " << "(after Grossmann), BESSEL" << endl; // 999 REAL a = 6377397.155, f = 1./299.15281285; // Bessel GL meridian(a,f,0.0); // --- REAL phi = (54.+13./60.+15.2891/3600.0)/RHODEG; REAL lam = (16.+30./60.+47.243/3600.0)/RHODEG; REAL lam01 = 15./RHODEG, lam02 = 18./RHODEG; REAL scale = (1 ? 1.0 : 0.9996); complex b = meridian.philam2b(complex(phi,lam-lam01)); sprintf(line,"gkutm: b= <%.12lf deg, %.12lf deg>",(double)real(b*RHODEG), (double)imag(b*RHODEG)); //cout << line << endl; complex GK = scale*meridian.arc(b); sprintf(line," center meridian lam0=%.1lf deg: %s= [%.8lf m, %.8lf m]", double(lam01*RHODEG),(scale == 1.0 ? "GK" : "UTM"), double(real(GK)),double(imag(GK))); cout << line << endl; GK=complex(6010941.18235,98682.40127); b = meridian.lat(GK/scale); b = meridian.b2philam(b); cout << " check: phi-phi_new= " << (phi-real(b))*RHODEG << " deg, " << "lam-lam_new= " << ((lam-lam01)-imag(b))*RHODEG << " deg" << endl; b = meridian.philam2b(complex(phi,lam-lam02)); GK = scale*meridian.arc(b); sprintf(line," center meridian lam0=%.1lf deg: %s= [%.8lf m, %.8lf m]", double(lam02*RHODEG),(scale == 1.0 ? "GK" : "UTM"), double(real(GK)),double(imag(GK))); cout << line << endl << endl; REAL north = 6010941.18235, east = 98682.40127; REAL phinew,dlamnew; GKUTM2ell(a,f,east,north, phinew, dlamnew); } void direct_indirect_problem(void) { char line[512]; // int d,m; REAL s; cout << "(3) direct and indirect problem on the ellipsoid" << " (after Grossmann), BESSEL" << endl; REAL a = 6377397.155, f = 1./299.15281285; // Bessel ellipsoid REAL phi1 = (53.+50./60.+2.8809/3600.)/RHODEG; REAL lam1 = (10.+12./60.+4.1772/3600.)/RHODEG; REAL alpha1 = (25.+16./60.+31.96/3600.)/RHODEG; REAL s12 = 47652.597; cout << " given: phi1= " << cdeg2dms(phi1*RHODEG); cout << ", lam1= " << cdeg2dms(lam1*RHODEG) << endl; cout << " alpha1= " << cdeg2dms(alpha1*RHODEG); sprintf(line,", s12=%12.4lf m",(double)s12); cout << line << endl; // direct problem: given point, azimut and length of a geodesic REAL beta1 = atan(tan(phi1)*(1.-f)); REAL h = sin(alpha1)*cos(beta1); GL HA1(a,f,h); // --- REAL s2 = HA1.arc(phi1) + s12; REAL phi2 = HA1.lat(s2); REAL beta2 = atan(tan(phi2)*(1.-f)); REAL alpha2 = h/cos(beta2); REAL lam2 = lam1 + (HA1.Lam(phi2)-HA1.Lam(phi1)); cout << " solution: phi2= " << cdeg2dms(phi2*RHODEG); cout << ", lam2= " << cdeg2dms(lam2*RHODEG) << endl; cout << " alpha2= " << cdeg2dms(alpha2*RHODEG) << endl; // indirect problem: given coordinates of two points, look for azimuths and // shortest distance GL HA2(a,f,phi1,phi2,lam2-lam1); // --- cout << " check: h-h_new = " << (h-HA2.h()) << endl; sprintf(line," s12 - (s2-s1) = %.1E m", double(s12-(HA2.arc(phi2)-HA2.arc(phi1)))); cout << line << endl; cout << endl; GL sphere(6370000.0,0.0,39./RHODEG,39./RHODEG,68./RHODEG); s12 = 2.*(sphere.sMax()-sphere.arc(39./RHODEG)); // result: s12 = 5727468.116_8491 m } void Soldner_coordinates(void) { char line[512]; cout << "(4) Soldner coordinates" << " (after Hofmann-Wellenhof), BESSEL" << endl; REAL a = 6377397.155, f = 1./299.15281285; // Bessel REAL phi = (48.+24./60.+37.91590/3600)/RHODEG; REAL dlam = (1.+43./60.+16.98120/3600.0)/RHODEG; cout << " given: phi= " << cdeg2dms(phi*RHODEG); cout << ", dlam= " << cdeg2dms(dlam*RHODEG) << endl; GL ordinate(a,f,phi,dlam); // --- REAL y = ordinate.sMax()-ordinate.arc(phi); GL abszisse(a,f,0.0); // meridian REAL betaF = acos(ordinate.h()); REAL phiF = atan(tan(betaF)/(1.-f)); REAL x = abszisse.arc(phiF); sprintf(line," solution: y= %.8lf m, x= %.8lf m",(double)y,(double)x); cout << line << endl; REAL beta = atan(tan(phi)*(1.-f)); REAL gamma = acos(cos(betaF)/cos(beta)); // Meridiankonvergenz cout << " Meridiankonvergenz: gamma= " << cdeg2dms(gamma*RHODEG) << endl; x = 5364960.713; y = 127410.166; GL newline(a,f,0.0); // --- phiF = newline.lat(x); betaF = atan((1.-f)*tan(phiF)); // h = cos(betaF); newline = GL(a,f,cos(betaF)); y = newline.sMax() - y; REAL phi_new = newline.lat(y); REAL dlam_new = newline.LamMax() - newline.Lam(phi_new); cout << " check: phi-phi_new= " << (phi-phi_new)*RHODEG << ", dlam-dlam_new= " << (dlam-dlam_new)*RHODEG << endl; } int main(int, char **) { cout << "--- computation of geodesics" << " (hehl@tfh-berlin.de, May 2004) ---" << endl << endl; try { tests(); // examples from literature: // Grossmann // Hofmann-Wellenhof meridian_arclength(); GKUTM_coordinates(); direct_indirect_problem(); Soldner_coordinates(); return 0; } catch(char * msg) { cerr << endl << "!!! " << msg << endl; return(-1); } catch(...) { cerr << endl << "!!! oops, there was an unexpected problem" << endl; return(-1); } } // end main() GL::GL(REAL aa, REAL ff, REAL hh, int NN,int test) { if(NN < 2) NN = 2; if(NN > N_MAX) NN = N_MAX; _N = NN; _a = aa; _f = ff; _h = hh; if(a() < 6.E6 || a() > 7.E6) throw "(GL::GL) invalid semimajor axis"; if(f() && (e2() < 0.0059 || e2() > 0.0081)) throw "(GL::GL) invalid flattening"; if(1.-h()*h() < 0.0) throw "(GL::GL) invalid parameter h"; koeff(test); } // end constructor 1 GL::GL(REAL aa, REAL ff, REAL phi1, REAL phi2, REAL dlam, int NN) { if(NN < 2) NN = 2; if(NN > N_MAX) NN = N_MAX; _N = NN; _a = aa; _f = ff; REAL beta1 = atan(tan(phi1)*(1.-f())), beta2 = atan(tan(phi2)*(1.-f())); REAL h_old = 1.E30; _h = 0.0; int iter = 0; while(++iter < 15 && fabs(h()-h_old) > 1.E-14) { REAL dLam,v1,v2; koeff(); v1 = asin(sin(beta1)/sqrt(1.-h()*h())); v2 = asin(sin(beta2)/sqrt(1.-h()*h())); dLam = dlam - h() * (koeff3()*(v2-v1) - 0.5 * sin(2.*v2)*koeff4(v2) + 0.5 * sin(2.*v1)*koeff4(v1)); h_old = _h; _h = 1./sqrt(1.+tan(beta1)*tan(beta1)+ (pow(tan(beta2)-tan(beta1)*cos(dLam),2)/sin(dLam)/sin(dLam))); } if(iter >= 15 || _h < 0.0 || _h > 1.) throw "(GL::GL) convergence problem for h"; if(fabs(_h) < 1.E-15) _h = 0.0; } // end constructor 2 GL::GL(REAL aa, REAL ff, REAL phi, REAL dlam, int NN) { if(NN < 2) NN = 2; if(NN > N_MAX) NN = N_MAX; _N = NN; _a = aa; _f = ff; REAL beta = atan(tan(phi)*(1.-f())); REAL h_old = 1.E30; _h = 0.0; int iter = 0; while(++iter < 15 && fabs(h()-h_old) > 1.E-14) { REAL dLam,v; koeff(); v = asin(sin(beta)/sqrt(1.-h()*h())); dLam = dlam - h() * (koeff3()*(M_PI/2.-v) + 0.5 * sin(2.*v)*koeff4(v)); h_old = _h; REAL betaF = atan(tan(beta)/cos(dLam)); _h = cos(betaF); } // end while() if(iter >= 15 || _h < 0.0 || _h > 1.) throw "(GL::GL) divergence for parameter h"; if(_h < 1.E-15) _h = 0.0; } // end constructor 3 template T GL::arc(T b) { // real : calculates arclength from latitude // complex : calculates GK/UTM from complex latitude T beta = atan(tan(b)*(1.-f())); T v = asin(sin(beta)/sqrt(1.-h()*h())); return(a()*(v*koeff1() - 0.5*sin(2.*v)*koeff2(v))); } // end arc() #ifdef fdfafafa REAL GL::lat(REAL len) { // real : calculates latitude from arc-length // complex : calculates complex latitude from GK/UTM if(fabs(sumc()-sqrt(1.-e2())) > 1.E-6) throw "(GL::lat) error" ; REAL k1 = koeff1(); REAL v0 = len/a()/k1; char line[512]; sprintf(line,"lat(REAL): v0= %.12lf deg",(double)(v0*RHODEG)); cout << line << endl; REAL v = v0, v_old = 1.E30; int iter = 0; while(++iter < 10 // && cabs(v-v_old) > 1.E-15 ) { v_old = v; v = v0 + koeff2(v)/k1/2.*sin(2.*v); sprintf(line,"lat(REAL): v[%2d]= %.12lf deg, v-v_old= %.3E deg", iter,(double)(v*RHODEG),double((v-v_old)*RHODEG)); cout << line << endl; } REAL beta = asin(sin(v)*sqrt(1.-h()*h())); REAL phi = atan(tan(beta)/(1.-f())); int d,m; REAL s; deg2dms(phi*RHODEG,d,m,s); sprintf(line,"lat: phi= %4d%3d%15.10lf",d,m,double(s)); cout << line << endl; // return(iter < 10 ? atan(tan(beta)/(1.-f())) : T(0.0)); return(atan(tan(beta)/(1.-f()))); } // end lat() #endif template T GL::lat(T len) { // real : calculates latitude from arc-length // complex : calculates complex latitude from GK/UTM if(fabs(sumc()-sqrt(1.-e2())) > 1.E-6) throw "(GL::lat) error" ; REAL k1 = koeff1(); T v0 = len/a()/k1; T v = v0;//, v_old = 1.E30; int iter = 0; while(++iter < 10 // && cabs(v-v_old) > 1.E-15 ) { //v_old = v; v = v0 + koeff2(v)/k1/2.*sin(2.*v); } T beta = asin(sin(v)*sqrt(1.-h()*h())); // return(iter < 10 ? atan(tan(beta)/(1.-f())) : T(0.0)); return(atan(tan(beta)/(1.-f()))); } // end lat() REAL GL::Lam(REAL phi) { REAL beta = atan((1.-f())*tan(phi)); REAL v = asin(sin(beta)/sqrt(1.-h()*h())); return(asin(h()/cos(beta)*sin(v)) + h()*v*koeff3() - h()/2.*sin(2.*v)*koeff4(v)); } // end Lam() REAL GL::koeff_c(int n, REAL e2) { if(n<1) return(1.0); else return(koeff_c(n-1,e2)*(2.*n-3.)/2./n*e2); } // end koeff_c() REAL GL::koeff_d(int n, REAL h2) { if(n<1) return(1.0); else return(-koeff_d(n-1,h2)*(2.*n-1.)/2./n*h2); } // end koeff_d() template T GL::koeff_k(int n, T s2) { if(n<1) return(1.0); else return(koeff_k(n-1,s2)*(2.*n)/(2.*n+1.)*s2); } // end koeff_k() REAL GL::binom(int n, int m) { if(n == 0 || m == 0 || n == m) return(1.0); else return(binom(n-1,m)+binom(n-1,m-1)); } // end binom() void GL::koeff(int test) { REAL h2 = 1.-h()*h(); for(int i=0;i T GL::koeff2(T v) { T sin2 = sin(v)*sin(v); T k2 = _k2[0]; for(int n=1;n T GL::koeff4(T v) { T sin2 = sin(v)*sin(v); T k4 = _k4[0]; for(int n=1;n GL::b2philam(complex b) { REAL e = sqrt(e2()); complex w = atanh(sin(b)) - e * atanh(e*sin(b)); REAL q = real(w); REAL phi = asin(tanh(q)); for(int i=1;i<=6;i++) phi = asin(tanh(q+e*atanh(e*sin(phi)))); return(complex(phi,imag(w))); } // end b2philam complex GL::philam2b(complex philam) { REAL e = sqrt(e2()); REAL q = atanh(sin(real(philam))) - e * atanh(e * sin(real(philam))); complex w(q,imag(philam)); complex b = asin(tanh(w)); for(int i=1;i<=6;i++) b = asin(tanh(w+e*atanh(e*sin(b)))); return(b); } // end philam2b() void GL::print_k24(void) { char line[512]; for(int i=0;i