00001 #include <stdio.h>
00002 #include <math.h>
00003
00004
00005 #define PI 3.14159265358979323846
00006
00007 int geo_to_km(double lat1,double lon1,double lat2,double lon2,double* dist,double* azm)
00008 {
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 double a, b;
00031 double semi_major=a=6378.160;
00032 double semi_minor=b=6356.775;
00033 double torad, todeg;
00034 double aa, bb, cc, dd, top, bottom, lambda12, az, temp;
00035 double v1, v2;
00036 double fl, e, e2, eps, eps0;
00037 double b0, x2, y2, z2, z1, u1p, u2p, xdist;
00038 double lat1rad, lat2rad, lon1rad, lon2rad;
00039 double coslon1, sinlon1, coslon2, sinlon2;
00040 double coslat1, sinlat1, coslat2, sinlat2;
00041 double tanlat1, tanlat2, cosazm, sinazm;
00042
00043 double c0, c2, c4, c6;
00044
00045 double c00=1.0, c01=0.25, c02=-0.046875, c03=0.01953125;
00046 double c21=-0.125, c22=0.03125, c23=-0.014648438;
00047 double c42=-0.00390625, c43=0.0029296875;
00048 double c63=-0.0003255208;
00049
00050
00051 if( lat1 == lat2 && lon1 == lon2 ) {
00052 *azm = 0.0;
00053 *dist= 0.0;
00054 return(1);
00055 }
00056
00057
00058 torad = PI / 180.0;
00059 todeg = 1.0 / torad;
00060 fl = ( a - b ) / a;
00061 e2 = 2.0*fl - fl*fl;
00062 e = sqrt(e2);
00063 eps = e2 / ( 1.0 - e2);
00064
00065
00066
00067
00068 temp=lat1;
00069 if(temp == 0.) temp=1.0e-08;
00070 lat1rad=torad*temp;
00071 lon1rad=torad*lon1;
00072
00073 temp=lat2;
00074 if(temp == 0.) temp=1.0e-08;
00075 lat2rad=torad*temp;
00076 lon2rad=torad*lon2;
00077
00078
00079
00080
00081 coslon1 = cos(lon1rad);
00082 sinlon1 = sin(lon1rad);
00083 coslon2 = cos(lon2rad);
00084 sinlon2 = sin(lon2rad);
00085 tanlat1 = tan(lat1rad);
00086 tanlat2 = tan(lat2rad);
00087 sinlat1 = sin(lat1rad);
00088 coslat1 = cos(lat1rad);
00089 sinlat2 = sin(lat2rad);
00090 coslat2 = cos(lat2rad);
00091
00092
00093
00094
00095
00096 v1 = a / sqrt( 1.0 - e2*sinlat1*sinlat1 );
00097 v2 = a / sqrt( 1.0 - e2*sinlat2*sinlat2 );
00098 aa = tanlat2 / ((1.0+eps)*tanlat1);
00099 bb = e2*(v1*coslat1)/(v2*coslat2);
00100 lambda12 = aa + bb;
00101 top = sinlon2*coslon1 - coslon2*sinlon1;
00102 bottom = lambda12*sinlat1-coslon2*coslon1*sinlat1-sinlon2*sinlon1*sinlat1;
00103 az = atan2(top,bottom)*todeg;
00104 if( az < 0.0 ) az = 360 + az;
00105 *azm = az;
00106 az = az * torad;
00107 cosazm = cos(az);
00108 sinazm = sin(az);
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120 if( lat2rad < 0.0 ) {
00121 temp = lat1rad;
00122 lat1rad = lat2rad;
00123 lat2rad = temp;
00124 temp = lon1rad;
00125 lon1rad = lon2rad;
00126 lon2rad = temp;
00127
00128 coslon1 = cos(lon1rad);
00129 sinlon1 = sin(lon1rad);
00130 coslon2 = cos(lon2rad);
00131 sinlon2 = sin(lon2rad);
00132 tanlat1 = tan(lat1rad);
00133 tanlat2 = tan(lat2rad);
00134 sinlat1 = sin(lat1rad);
00135 coslat1 = cos(lat1rad);
00136 sinlat2 = sin(lat2rad);
00137 coslat2 = cos(lat2rad);
00138
00139 v1 = a / sqrt( 1.0 - e2*sinlat1*sinlat1 );
00140 v2 = a / sqrt( 1.0 - e2*sinlat2*sinlat2 );
00141
00142 aa = tanlat2 / ((1.0+eps)*tanlat1);
00143 bb = e2*(v1*coslat1)/(v2*coslat2);
00144 lambda12 = aa + bb;
00145
00146 top = sinlon2*coslon1 - coslon2*sinlon1;
00147 bottom =lambda12*sinlat1-coslon2*coslon1*sinlat1-
00148 sinlon2*sinlon1*sinlat1;
00149 az = atan2(top,bottom);
00150 cosazm = cos(az);
00151 sinazm = sin(az);
00152
00153 }
00154
00155 eps0 = eps * ( coslat1*coslat1*cosazm*cosazm + sinlat1*sinlat1 );
00156 b0 = (v1/(1.0+eps0)) * sqrt(1.0+eps*coslat1*coslat1*cosazm*cosazm);
00157
00158 x2 = v2*coslat2*(coslon2*coslon1+sinlon2*sinlon1);
00159 y2 = v2*coslat2*(sinlon2*coslon1-coslon2*sinlon1);
00160 z2 = v2*(1.0-e2)*sinlat2;
00161 z1 = v1*(1.0-e2)*sinlat1;
00162
00163 c0 = c00 + c01*eps0 + c02*eps0*eps0 + c03*eps0*eps0*eps0;
00164 c2 = c21*eps0 + c22*eps0*eps0 + c23*eps0*eps0*eps0;
00165 c4 = c42*eps0*eps0 + c43*eps0*eps0*eps0;
00166 c6 = c63*eps0*eps0*eps0;
00167
00168 bottom = cosazm*sqrt(1.0+eps0);
00169 u1p = atan2(tanlat1,bottom);
00170
00171 top = v1*sinlat1+(1.0+eps0)*(z2-z1);
00172 bottom = (x2*cosazm-y2*sinlat1*sinazm)*sqrt(1.0+eps0);
00173 u2p = atan2(top,bottom);
00174
00175 aa = c0*(u2p-u1p);
00176 bb = c2*(sin(2.0*u2p)-sin(2.0*u1p));
00177 cc = c4*(sin(4.0*u2p)-sin(4.0*u1p));
00178 dd = c6*(sin(6.0*u2p)-sin(6.0*u1p));
00179
00180 xdist = fabs(b0*(aa+bb+cc+dd));
00181 *dist = xdist;
00182 return(1);
00183 }
00184
00185
00186
00187 int geo_to_km_deg (double lat1, double lon1, double lat2, double lon2,
00188 double *dist, double *xdeg, double *azm)
00189 {
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212 double a, b;
00213 double semi_major=a=6378.160;
00214 double semi_minor=b=6356.775;
00215 double torad, todeg;
00216 double aa, bb, cc, dd, top, bottom, lambda12, az, temp;
00217 double v1, v2;
00218 double fl, e, e2, eps, eps0;
00219 double b0, x2, y2, z2, z1, u1p, u2p, xdist;
00220 double lat1rad, lat2rad, lon1rad, lon2rad;
00221 double coslon1, sinlon1, coslon2, sinlon2;
00222 double coslat1, sinlat1, coslat2, sinlat2;
00223 double tanlat1, tanlat2, cosazm, sinazm;
00224 double onemec2;
00225 double aa2, bb2, cc2, dd2, ee2, ff2, aa3, bb3, cc3, dd3, ee3, ff3;
00226 double aminus, bminus, cminus, aplus, bplus, cplus;
00227 double thg, sd, sc, deg;
00228
00229 double c0, c2, c4, c6;
00230
00231 double c00=1.0, c01=0.25, c02=-0.046875, c03=0.01953125;
00232 double c21=-0.125, c22=0.03125, c23=-0.014648438;
00233 double c42=-0.00390625, c43=0.0029296875;
00234 double c63=-0.0003255208;
00235
00236
00237
00238 if( lat1 == lat2 && lon1 == lon2 ) {
00239 *azm = 0.0;
00240 *dist= 0.0;
00241 return(0);
00242 }
00243
00244
00245 torad = PI / 180.0;
00246 todeg = 1.0 / torad;
00247 fl = ( a - b ) / a;
00248 e2 = 2.0*fl - fl*fl;
00249 e = sqrt(e2);
00250 eps = e2 / ( 1.0 - e2);
00251 onemec2 = 1.0 - e2;
00252
00253
00254
00255
00256
00257 temp=lat1;
00258 if(temp == 0.) temp=1.0e-08;
00259 lat1rad=torad*temp;
00260 lon1rad=torad*lon1;
00261
00262 temp=lat2;
00263 if(temp == 0.) temp=1.0e-08;
00264 lat2rad=torad*temp;
00265 lon2rad=torad*lon2;
00266
00267
00268
00269
00270 coslon1 = cos(lon1rad);
00271 sinlon1 = sin(lon1rad);
00272 coslon2 = cos(lon2rad);
00273 sinlon2 = sin(lon2rad);
00274 tanlat1 = tan(lat1rad);
00275 tanlat2 = tan(lat2rad);
00276 sinlat1 = sin(lat1rad);
00277 coslat1 = cos(lat1rad);
00278 sinlat2 = sin(lat2rad);
00279 coslat2 = cos(lat2rad);
00280
00281
00282
00283
00284
00285
00286 v1 = a / sqrt( 1.0 - e2*sinlat1*sinlat1 );
00287 v2 = a / sqrt( 1.0 - e2*sinlat2*sinlat2 );
00288
00289 aa = tanlat2 / ((1.0+eps)*tanlat1);
00290 bb = e2*(v1*coslat1)/(v2*coslat2);
00291 lambda12 = aa + bb;
00292
00293 top = sinlon2*coslon1 - coslon2*sinlon1;
00294 bottom = lambda12*sinlat1-coslon2*coslon1*sinlat1-sinlon2*sinlon1*sinlat1;
00295 az = atan2(top,bottom)*todeg;
00296 if( az < 0.0 ) az = 360 + az;
00297 *azm = az;
00298 az = az * torad;
00299 cosazm = cos(az);
00300 sinazm = sin(az);
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313 if( lat2rad < 0.0 ) {
00314 temp = lat1rad;
00315 lat1rad = lat2rad;
00316 lat2rad = temp;
00317 temp = lon1rad;
00318 lon1rad = lon2rad;
00319 lon2rad = temp;
00320
00321 coslon1 = cos(lon1rad);
00322 sinlon1 = sin(lon1rad);
00323 coslon2 = cos(lon2rad);
00324 sinlon2 = sin(lon2rad);
00325 tanlat1 = tan(lat1rad);
00326 tanlat2 = tan(lat2rad);
00327 sinlat1 = sin(lat1rad);
00328 coslat1 = cos(lat1rad);
00329 sinlat2 = sin(lat2rad);
00330 coslat2 = cos(lat2rad);
00331
00332 v1 = a / sqrt( 1.0 - e2*sinlat1*sinlat1 );
00333 v2 = a / sqrt( 1.0 - e2*sinlat2*sinlat2 );
00334
00335 aa = tanlat2 / ((1.0+eps)*tanlat1);
00336 bb = e2*(v1*coslat1)/(v2*coslat2);
00337 lambda12 = aa + bb;
00338
00339 top = sinlon2*coslon1 - coslon2*sinlon1;
00340 bottom =lambda12*sinlat1-coslon2*coslon1*sinlat1-
00341 sinlon2*sinlon1*sinlat1;
00342 az = atan2(top,bottom);
00343 cosazm = cos(az);
00344 sinazm = sin(az);
00345
00346 }
00347
00348 eps0 = eps * ( coslat1*coslat1*cosazm*cosazm + sinlat1*sinlat1 );
00349 b0 = (v1/(1.0+eps0)) * sqrt(1.0+eps*coslat1*coslat1*cosazm*cosazm);
00350
00351 x2 = v2*coslat2*(coslon2*coslon1+sinlon2*sinlon1);
00352 y2 = v2*coslat2*(sinlon2*coslon1-coslon2*sinlon1);
00353 z2 = v2*(1.0-e2)*sinlat2;
00354 z1 = v1*(1.0-e2)*sinlat1;
00355
00356 c0 = c00 + c01*eps0 + c02*eps0*eps0 + c03*eps0*eps0*eps0;
00357 c2 = c21*eps0 + c22*eps0*eps0 + c23*eps0*eps0*eps0;
00358 c4 = c42*eps0*eps0 + c43*eps0*eps0*eps0;
00359 c6 = c63*eps0*eps0*eps0;
00360
00361 bottom = cosazm*sqrt(1.0+eps0);
00362 u1p = atan2(tanlat1,bottom);
00363
00364 top = v1*sinlat1+(1.0+eps0)*(z2-z1);
00365 bottom = (x2*cosazm-y2*sinlat1*sinazm)*sqrt(1.0+eps0);
00366 u2p = atan2(top,bottom);
00367
00368 aa = c0*(u2p-u1p);
00369 bb = c2*(sin(2.0*u2p)-sin(2.0*u1p));
00370 cc = c4*(sin(4.0*u2p)-sin(4.0*u1p));
00371 dd = c6*(sin(6.0*u2p)-sin(6.0*u1p));
00372
00373 xdist = fabs(b0*(aa+bb+cc+dd));
00374 *dist = xdist;
00375
00376
00377 thg=atan(onemec2*tan(lat1rad));
00378 dd2=sin(lon1rad);
00379 ee2=-cos(lon1rad);
00380 ff2=-cos(thg);
00381 cc2=sin(thg);
00382 aa2= ff2*ee2;
00383 bb2=-ff2*dd2;
00384
00385
00386
00387 thg=atan(onemec2*tan(lat2rad));
00388 dd3=sin(lon2rad);
00389 ee3=-cos(lon2rad);
00390 ff3=-cos(thg);
00391 cc3=sin(thg);
00392 aa3=ff3*ee3;
00393 bb3=-ff3*dd3;
00394 sc=aa2*aa3+bb2*bb3+cc2*cc3;
00395
00396
00397
00398 aminus = aa2 - aa3;
00399 bminus = bb2 - bb3;
00400 cminus = cc2 - cc3;
00401 aplus = aa2 + aa3;
00402 bplus = bb2 + bb3;
00403 cplus = cc2 + cc3;
00404
00405 sd=0.5*sqrt((aminus*aminus + bminus*bminus+cminus*cminus)*(aplus*aplus
00406 + bplus*bplus + cplus*cplus));
00407 deg=atan2(sd,sc)*todeg;
00408 *xdeg = deg;
00409
00410 return(1);
00411 }
00412
00413 distaz_geo(olat,olon,dist,azimuth,lat,lon)
00414 double olat, olon, azimuth, dist, *lat, *lon;
00415 {
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434 double C, C_sqr;
00435 double torad, todeg;
00436 double a, b, aa, bb, a0, b0;
00437 double g0, g2, g4, g6;
00438 double latrad, lonrad, azmrad;
00439 double ec, ec2, eps, eps0, eps2, fl;
00440 double tanu1p, tanmu, sinu1, sinu2;
00441 double tanlat, coslat, sinlat, tanazm, sinazm, cosazm;
00442 double v1, u2, u1p, u2p, u1pbot, x2, y2, z2, ztop, zbot;
00443 double arg, temp, delta, mu, sd, sig1, sig2;
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453 double semi_major=a=6378.16, semi_minor=b=6356.775;
00454 double a1=0.25, b1=-0.125, c1=-0.0078125;
00455 double g00=1.0, g01=-0.25, g02=0.109375, g03=-0.058139535;
00456 double g21=0.125, g22=-0.06250, g23=0.034667969;
00457 double g42=0.01953125, g43=-0.01953125;
00458 double g63=0.004720052;
00459
00460
00461
00462 fl = (a - b)/a;
00463 ec2 = 2.0*fl-fl*fl;
00464 ec = sqrt(ec2);
00465 eps = ec2/(1.0-ec2);
00466 torad = PI / 180.0;
00467 todeg = 1.0 / torad;
00468
00469
00470
00471 temp=olat;
00472 if(temp == 0.) temp=1.0e-08;
00473 latrad=torad*temp;
00474 lonrad=torad*olon;
00475 azmrad=azimuth*torad;
00476
00477
00478
00479 coslat = cos(latrad);
00480 sinlat = sin(latrad);
00481 cosazm = cos(azmrad);
00482 sinazm = sin(azmrad);
00483 tanazm = tan(azmrad);
00484 tanlat = tan(latrad);
00485
00486 C_sqr = coslat*coslat*cosazm*cosazm+ sinlat*sinlat;
00487 C = sqrt(C_sqr);
00488 eps0 = C_sqr * eps;
00489 v1 = a/sqrt(1.0-ec2*sinlat*sinlat);
00490 b0 = v1*sqrt(1.0+eps*coslat*coslat*cosazm*cosazm)/(1.0+eps0);
00491
00492 g0 = g00 + g01*eps0 + g02*eps0*eps0 + g03*eps0*eps0*eps0;
00493 g2 = g21*eps0 + g22*eps0*eps0 + g23*eps0*eps0*eps0;
00494 g4 = g42*eps0*eps0 + g43*eps0*eps0*eps0;
00495 g6 = g63*eps0*eps0*eps0;
00496
00497
00498 tanu1p = tanlat / (cosazm*sqrt(1.0+eps0));
00499 u1pbot = cosazm * sqrt(1.0+eps0);
00500 u1p=atan2(tanlat,u1pbot);
00501 sig1 = 0.5*( 2.0*u1p-(a1*eps0+b1*eps0*eps0)*sin(2.0*u1p)+
00502 c1*eps0*eps0*sin(4.0*u1p));
00503
00504
00505
00506
00507 aa = (dist*g0)/b0;
00508 bb = 2.0 * sig1 + aa;
00509 u2p=u1p+aa+2.0*g2*sin(aa)*cos(bb)+2.0*g4*sin(2.*aa)*cos(2.*bb)+
00510 2.*g6*sin(3.*aa)*cos(3.*bb);
00511 sinu1=tanlat/sqrt(1.0+eps+tanlat*tanlat);
00512 sinu2 = ((b0*C)/b)*sin(u2p)-((eps-eps0)/(1+eps0))*sinu1;
00513 u2 = asin(sinu2);
00514
00515
00516
00517
00518
00519
00520 a0 = b0*sqrt(1.0+eps0);
00521 tanmu = sinlat*tanazm;
00522 mu = atan(tanmu);
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534 sd=(ec2*v1*sinlat*coslat*sinazm)/(1.-ec2*coslat*coslat*sinazm*sinazm);
00535 x2=a0*cos(u2p)*cos(mu)+b0*sin(u2p)*sin(mu)*coslat*sinazm+
00536 sd*sinlat*sinazm;
00537 y2=-a0*cos(u2p)*sin(mu)+b0*sin(u2p)*cos(mu)*coslat*sinazm+
00538 sd*cosazm;
00539 z2=b0*C*sin(u2p)-(sd*coslat*sinazm)/(1.0+eps);
00540 ztop = (1.0+eps)*z2;
00541 zbot = sqrt(x2*x2+y2*y2);
00542 *lat = atan2(ztop,zbot)*todeg;
00543 *lon = (atan(y2/x2)+lonrad)*todeg;
00544 if((atan(y2/x2)+lonrad)<0.0) *lon=((atan(y2/x2)+lonrad)*todeg);
00545 arg = atan2(y2,x2)*todeg;
00546 }