00001 /***************************************************************************** 00002 * 00003 * $RCSfile: geo_8c-source.html,v $ 00004 * 00005 * GPS4Palm Geodesic/Geometric Utility Functions 00006 * 00007 * History: 00008 * 2003-07-06: first version 00009 * 00010 * This program is Copyright (C) 07/2003 Matthias Prinke 00011 * <matthias.prinke@surfeu.de> and covered by GNU's GPL. 00012 * In particular, this program is free software and comes WITHOUT 00013 * ANY WARRANTY. 00014 * 00015 * $Author: mp $ 00016 * 00017 * $Date: 2007-10-08 20:40:33 $ 00018 * 00019 * $Revision: 1.7.2.1 $ 00020 * 00021 * $Log: geo_8c-source.html,v $ 00021 * Revision 1.7.2.1 2007-10-08 20:40:33 mp 00021 * updated for gps4palm V0.9.5 beta 00021 * 00022 * Revision 1.15 2005-05-28 10:15:01 mp 00023 * fixed StrPrintF format in deg_to_d_str() 00024 * 00025 * Revision 1.14 2005/04/02 07:23:37 mp 00026 * added deg_to_geodb_str() 00027 * 00028 * Revision 1.13 2005/03/25 13:31:59 mp 00029 * added deg_to_str() 00030 * 00031 * Revision 1.12 2004/11/26 19:57:34 mp 00032 * added deg_to_d_str(), fixed deg_to_dm_str() and deg_to_dms_str() 00033 * 00034 * Revision 1.11 2004/04/30 16:30:39 mp 00035 * modified comments for doxygen 00036 * 00037 * Revision 1.10 2004/04/21 15:33:00 mp 00038 * fixed deg2semi() and semi2deg() 00039 * 00040 * Revision 1.9 2004/04/12 10:33:05 mp 00041 * modified init_radius() and calcR() 00042 * 00043 * Revision 1.8 2004/03/11 18:50:43 mp 00044 * fixed gc_course_sphere() 00045 * 00046 * Revision 1.7 2004/03/10 17:07:07 mp 00047 * Added rad2deg(), deg2rad(), gc_dist_sphere(), and gc_course_sphere(). 00048 * 00049 * Revision 1.6 2004/02/28 13:20:51 mp 00050 * added semi2deg(), deg2semi(), deg_to_dm_str(), and deg_to_dms_str() 00051 * 00052 * Revision 1.5 2004/01/18 19:53:32 mp 00053 * modified range check in Lat_To_UTM_Belt() 00054 * 00055 * Revision 1.4 2004/01/18 11:43:55 mp 00056 * added function Lat_To_UTM_Belt() 00057 * 00058 * Revision 1.3 2003/12/30 19:46:46 mp 00059 * replaced math.h by MathLib.h 00060 * 00061 * Revision 1.2 2003/10/20 19:12:11 mp 00062 * added map scale to calculation 00063 * 00064 * Revision 1.1.1.1 2003/07/14 18:59:29 mp 00065 * Imported GPS4Palm to CVS revision control. 00066 * 00067 * 00068 ****************************************************************************/ 00069 00070 /***************************************************************************** 00071 * 1° Latitude : 111120m 00072 * 1' Latitude : 1852m = 1nmi 00073 * 00074 * 1° Longitude: 111120m * cos(lat) 00075 * 1' Longitude: 1852m * cos(lat) 00076 * 00077 * d_map : distance on map (in m) 00078 * d_act : distance in real world (actual distance) (in m) 00079 * r_map : map resolution (in dpi) 00080 * 00081 * d_map = d_act / scale 00082 * 00083 * d_screen = d_map * r_map * (1/0.0254) in/m 00084 * 00085 ****************************************************************************/ 00086 00087 #include <PalmOS.h> 00088 #include <PalmCompatibility.h> 00089 #include "MathLib.h" 00090 #include "geo.h" 00091 #include "common.h" /* POS_D|POS_DM|POS_DMS|POS_UTM */ 00092 #include "utm.h" /* Convert_Geodetic_To_UTM() */ 00093 #include "fp.h" 00094 00095 /** \brief square, i.e. x^2 */ 00096 #define SQR(x) ((x) * (x)) 00097 00098 /** \brief epsilon, i.e. a really small number greater than zero */ 00099 #define EPS 0.00000000000000000000005 00100 00101 /** Earth's radius in meters at given latitude (in integer degrees) */ 00102 double Ra[101]; 00103 00104 /* 00105 * PIXELFACT = r_map [dpi] * 1/0.0254 in/m 00106 * = 72 dpi * 1/0.0254 in/m = 2834.65 pixel/m 00107 */ 00108 00109 00110 /****************************************************************************/ 00111 /** 00112 * \brief Calculate Earth radius at given latitude 00113 * according to GRS80/WGS84 Ellipsoid. 00114 * 00115 * see: 00116 * The Universal Grids: Universal Transverse Mercator (UTM) and 00117 * Universal Polar Stereographic (UPS) 00118 * Defense Mapping Agency 00119 * Technical Manual 8358.2 00120 * http://www.nima.mil/GandG/pubs.html 00121 * (Section 2-2.1 Ellipsoid Parameters) 00122 * 00123 * \param lat latitude in degrees 00124 * 00125 * \return Earth's radius at given latitude in meters 00126 ****************************************************************************/ 00127 double calcR(double lat) 00128 { 00129 double a = SEMIMAJOR_AXIS/1000.0; 00130 double r, sc, x, y, z; 00131 double e2 = 0.081082 * 0.081082; 00132 00133 /* 00134 the radius of curvature of an ellipsoidal Earth in the plane of the 00135 meridian is given by 00136 00137 R' = a * (1 - e^2) / (1 - e^2 * (sin(lat))^2)^(3/2) 00138 00139 where a is the equatorial radius, 00140 b is the polar radius, and 00141 e is the eccentricity of the ellipsoid = sqrt(1 - b^2/a^2) 00142 00143 a = 6378 km (3963 mi) Equatorial radius (surface to center distance) 00144 b = 6356.752 km (3950 mi) Polar radius (surface to center distance) 00145 e = 0.081082 Eccentricity 00146 */ 00147 00148 lat = lat * M_PI / 180.0; 00149 sc = sin (lat); 00150 x = a * (1.0 - e2); 00151 z = 1.0 - e2 * sc * sc; 00152 y = pow (z, 1.5); 00153 r = x / y; 00154 00155 r = r * 1000.0; 00156 00157 return r; 00158 } 00159 00160 00161 /****************************************************************************/ 00162 /** 00163 * Initialize look-up table with Earth's radii (in meters) 00164 * at given latitudes. To save memory and to reduce 00165 * time required for initialization, the symmetry is exploitet. 00166 * ( sin(90°+x) = sin(90°-x) ) 00167 * 00168 ****************************************************************************/ 00169 void init_radius(void) 00170 { 00171 Int16 i; 00172 00173 /* Build array for earth radii */ 00174 for (i = 0; i <= 100; i++) 00175 Ra[i] = calcR(i); 00176 } 00177 00178 /**********************************************************************/ 00179 /** 00180 * \brief Calculate screen position from current position and 00181 * map parameters. 00182 * 00183 * \param posx horizontal map position 00184 * \param posy vertical map position 00185 * \param lon current longitude 00186 * \param lat current latitude 00187 * \param zero_lon longitude of map's center 00188 * \param zero_lat latitude of map's center 00189 * \param scale scale factor 00190 * 00191 * Changes as compared to original version (GPSDrive): 00192 * - data types: gdouble to double, gint to UInt16 00193 * - removed screen specific calculations 00194 * - added zero_lon and zero_lat to parameters 00195 * - renamed zero_long to zero_lon for consistency 00196 * - changed type of posx/posy from double * to Int32 * 00197 * 00198 * TBD: Check geodetic background of formulae! 00199 **********************************************************************/ 00200 void calcxy(Int32 *posx, Int32 *posy, double lon, double lat, 00201 double zero_lon, double zero_lat, UInt32 scale) 00202 { 00203 double dif; 00204 double x; 00205 double y; 00206 00207 x = (Ra[(UInt32) fabs(lat)] * M_PI / 180.0) * cos (M_PI * lat / 180.0) * 00208 (lon - zero_lon); 00209 00210 *posx = (Int32)(x * PIXELFACT / scale); 00211 00212 y = (Ra[(UInt32) fabs(lat)] * M_PI / 180.0) * (lat - zero_lat); 00213 00214 dif = 00215 Ra[(UInt32) fabs(lat)] * (1 - (cos ((M_PI * (lon - zero_lon)) / 180.0))); 00216 00217 y = y + dif / 1.85; 00218 *posy = (Int32)(y * PIXELFACT / scale); 00219 00220 } 00221 00222 00223 /****************************************************************************/ 00224 /** 00225 * Calculate UTM Belt from latitude. 00226 * 00227 * \param lat latitude in degrees 00228 * 00229 * \return UTM Belt Designator 00230 ****************************************************************************/ 00231 char Lat_To_UTM_Belt(double lat) 00232 { 00233 Int16 belt; 00234 const char utm_belt[] = "CDEFGHJKLMNPQRSTUVWX"; 00235 00236 /* 00237 * range check actually done by Convert_Geodetic_To_UTM(), 00238 * so we just make sure to keep in the valid range of UTM belts 00239 */ 00240 lat = (lat > 84.0) ? 84.0 : lat; 00241 lat = (lat < -79.0) ? -79.0 : lat; 00242 00243 /* correction for zone X which extends from 72 to 84° */ 00244 if (lat >= 80.0 && lat < 84.5) { 00245 lat = 72.0; 00246 } 00247 00248 belt = (Int16) floor(lat / 8.0); 00249 belt += 10; 00250 return utm_belt[belt]; 00251 } 00252 00253 00254 /****************************************************************************/ 00255 /** 00256 * Convert semicircles to degrees. 00257 * degrees = semicircles * (180 / 2^31) 00258 * 00259 * \param semi angle in semicircles 00260 * 00261 * \return angle in degrees 00262 ****************************************************************************/ 00263 double semi2deg(Int32 semi) { 00264 return ((double)(semi * (180.0/(1UL<<31)))); 00265 } 00266 00267 00268 /****************************************************************************/ 00269 /** 00270 * Convert degrees to semicircles. 00271 * semicircles = degrees * (2^31 / 180) 00272 * 00273 * \param deg angle in degrees 00274 * 00275 * \return angle in semicircles 00276 ****************************************************************************/ 00277 Int32 deg2semi(double deg) 00278 { 00279 return ((Int32)(deg * ((1UL<<31)/180.0))); 00280 } 00281 00282 00283 /****************************************************************************/ 00284 /** 00285 * Convert radians to degrees. 00286 * 00287 * \param rad angle in radians 00288 * 00289 * \return angle in degrees 00290 ****************************************************************************/ 00291 double rad2deg(double rad) 00292 { 00293 return (rad / M_PI * 180.0); 00294 } 00295 00296 00297 /****************************************************************************/ 00298 /** 00299 * Convert degrees to radians. 00300 * 00301 * \param deg angle in degrees 00302 * 00303 * \return angle in radians 00304 ****************************************************************************/ 00305 double deg2rad(double deg) 00306 { 00307 return (deg / 180.0 * M_PI); 00308 } 00309 00310 00311 /****************************************************************************/ 00312 /** 00313 * Convert degrees to degrees string 00314 * for latitude and longitude. 00315 * 00316 * \param lat latitude in degrees 00317 * \param lon longitude in degrees 00318 * \param lat_d_str string buffer for returning latitude 00319 * in degrees 00320 * \param lon_d_str string buffer for returning longitude 00321 * in degrees 00322 * 00323 ****************************************************************************/ 00324 void deg_to_d_str(double lat, double lon, char *lat_d_str, char *lon_d_str) 00325 { 00326 double tmp; /* temp */ 00327 UInt16 ideg; /* integer degrees */ 00328 00329 /* Latitude */ 00330 if (lat < 0) { 00331 StrCopy(lat_d_str, "-"); 00332 lat = -lat; 00333 } else { 00334 StrCopy(lat_d_str, ""); 00335 } 00336 00337 tmp = lat; 00338 ideg = (UInt16)tmp; 00339 tmp = (tmp-ideg) * 10000000.0 + 0.5; /* 1/10000000 degrees of arc */ 00340 00341 if (tmp >= 9999999.9) { 00342 tmp = 0; 00343 ideg++; 00344 } 00345 00346 StrPrintF(&lat_d_str[StrLen(lat_d_str)], "%02u.%07lu°", 00347 ideg, (UInt32)tmp); 00348 00349 /* Longitude */ 00350 if (lon < 0) { 00351 StrCopy(lon_d_str, "-"); 00352 lon = -lon; 00353 } else { 00354 StrCopy(lon_d_str, ""); 00355 } 00356 00357 tmp = lon; 00358 ideg = (UInt16)tmp; 00359 tmp = (tmp-ideg) * 10000000.0 + 0.5; /* 1/10000000 minutes of arc */ 00360 00361 if (tmp >= 9999999.9) { 00362 tmp = 0; 00363 ideg++; 00364 } 00365 00366 StrPrintF(&lon_d_str[StrLen(lon_d_str)], "%03u.%07lu°", 00367 ideg, (UInt32)tmp); 00368 00369 } 00370 00371 00372 /****************************************************************************/ 00373 /** 00374 * Convert degrees to degrees and minutes string 00375 * for latitude and longitude. 00376 * 00377 * \param lat latitude in degrees 00378 * \param lon longitude in degrees 00379 * \param lat_dm_str string buffer for returning latitude 00380 * in degrees and minutes 00381 * \param lon_dm_str string buffer for returning longitude 00382 * in degrees and minutes 00383 * 00384 ****************************************************************************/ 00385 void deg_to_dm_str(double lat, double lon, char *lat_dm_str, char *lon_dm_str) 00386 { 00387 double tmp; /* temp */ 00388 UInt16 ideg; /* integer degrees */ 00389 UInt16 imin; /* integer minutes */ 00390 00391 /* Latitude */ 00392 if (lat < 0) { 00393 StrCopy(lat_dm_str, "-"); 00394 lat = -lat; 00395 } else { 00396 StrCopy(lat_dm_str, ""); 00397 } 00398 00399 tmp = lat; 00400 ideg = (UInt16)tmp; 00401 tmp = (tmp-ideg) * 60.0; /* minutes of arc */ 00402 imin = (UInt16)tmp; 00403 tmp = (tmp-imin) * 10000.0 + 0.5; /* 1/10000 minutes of arc */ 00404 00405 if (tmp >= 9999.9) { 00406 tmp = 0; 00407 imin++; 00408 } 00409 if (imin > 59) { 00410 imin = 0; 00411 ideg++; 00412 } 00413 00414 StrPrintF(&lat_dm_str[StrLen(lat_dm_str)], "%02u°%02u.%04u'", 00415 ideg, imin, (UInt16)tmp); 00416 00417 /* Longitude */ 00418 if (lon < 0) { 00419 StrCopy(lon_dm_str, "-"); 00420 lon = -lon; 00421 } else { 00422 StrCopy(lon_dm_str, ""); 00423 } 00424 00425 tmp = lon; 00426 ideg = (UInt16)tmp; 00427 tmp = (tmp-ideg) * 60.0; /* minutes of arc */ 00428 imin = (UInt16)tmp; 00429 tmp = (tmp-imin) * 10000.0 + 0.5; /* 1/10000 minutes of arc */ 00430 00431 if (tmp >= 9999.9) { 00432 tmp = 0; 00433 imin++; 00434 } 00435 if (imin > 59) { 00436 imin = 0; 00437 ideg++; 00438 } 00439 00440 StrPrintF(&lon_dm_str[StrLen(lon_dm_str)], "%03u°%02u.%04u'", 00441 ideg, imin, (UInt16)tmp); 00442 00443 } 00444 00445 00446 /****************************************************************************/ 00447 /** 00448 * Convert degrees to degrees, minutes and seconds string 00449 * for latitude and longitude. 00450 * 00451 * \param lat latitude in degrees 00452 * \param lon longitude in degrees 00453 * \param lat_dms_str string buffer for returning latitude in 00454 * degrees, minutes and seconds 00455 * \param lon_dms_str string buffer for returning longitude in 00456 * degrees, minutes and seconds 00457 * 00458 ****************************************************************************/ 00459 void deg_to_dms_str(double lat, double lon, char *lat_dms_str, char *lon_dms_str) 00460 { 00461 double tmp; /* temp */ 00462 UInt16 ideg; /* integer degrees */ 00463 UInt16 imin; /* integer minutes */ 00464 UInt16 isec; /* integer seconds */ 00465 00466 /* Latitude */ 00467 if (lat < 0) { 00468 StrCopy(lat_dms_str, "-"); 00469 lat = -lat; 00470 } else { 00471 StrCopy(lat_dms_str, ""); 00472 } 00473 00474 tmp = lat; 00475 ideg = (UInt16)tmp; 00476 tmp = (tmp-ideg) * 60.0; /* minutes of arc */ 00477 imin = (UInt16)tmp; 00478 tmp = (tmp-imin) * 60.0; /* seconds of arc */ 00479 isec = (UInt16)tmp; 00480 tmp = (tmp-isec)*1000.0 + 0.5; /* 1/1000 seconds of arc */ 00481 00482 if (tmp >= 999.9) { 00483 tmp = 0; 00484 isec++; 00485 } 00486 if (isec > 59) { 00487 isec = 0; 00488 imin++; 00489 } 00490 if (imin > 59) { 00491 imin = 0; 00492 ideg++; 00493 } 00494 00495 StrPrintF(&lat_dms_str[StrLen(lat_dms_str)], "%02u°%02u'%02u.%03u\"", 00496 ideg, imin, isec, (UInt16)tmp); 00497 00498 /* Longitude */ 00499 if (lon < 0) { 00500 StrCopy(lon_dms_str, "-"); 00501 lon = -lon; 00502 } else { 00503 StrCopy(lon_dms_str, ""); 00504 } 00505 00506 tmp = lon; 00507 ideg = (UInt16)tmp; 00508 tmp = (tmp-ideg) * 60.0; /* minutes of arc */ 00509 imin = (UInt16)tmp; 00510 tmp = (tmp-imin) * 60.0; /* seconds of arc */ 00511 isec = (UInt16)tmp; 00512 tmp = (tmp-isec)*1000.0 + 0.5; /* 1/1000 seconds of arc */ 00513 00514 if (tmp >= 999.9) { 00515 tmp = 0; 00516 isec++; 00517 } 00518 if (isec > 59) { 00519 isec = 0; 00520 imin++; 00521 } 00522 if (imin > 59) { 00523 imin = 0; 00524 ideg++; 00525 } 00526 00527 StrPrintF(&lon_dms_str[StrLen(lon_dms_str)], "%03u°%02u'%02u.%03u\"", 00528 ideg, imin, isec, (UInt16)tmp); 00529 00530 } 00531 00532 00533 /****************************************************************************/ 00534 /** 00535 * Convert degrees to degrees and minutes string 00536 * for latitude and longitude as required by GeoDB. 00537 * 00538 * \param lat latitude in degrees 00539 * \param lon longitude in degrees 00540 * \param lat_str string buffer for returning latitude 00541 * in degrees, minutes and hemisphere 00542 * \param lon_str string buffer for returning longitude 00543 * in degrees, minutes and hemisphere 00544 * 00545 * \note Mostly identical to deg_to_dm_str() 00546 ****************************************************************************/ 00547 void deg_to_geodb_str(double lat, double lon, char *lat_str, char *lon_str) 00548 { 00549 double tmp; /* temp */ 00550 UInt16 ideg; /* integer degrees */ 00551 UInt16 imin; /* integer minutes */ 00552 Char ns; /* N/S */ 00553 Char ew; /* E/W */ 00554 00555 /* Latitude */ 00556 if (lat < 0) { 00557 ns = 'S'; 00558 lat = -lat; 00559 } else { 00560 ns = 'N'; 00561 } 00562 00563 tmp = lat; 00564 ideg = (UInt16)tmp; 00565 tmp = (tmp-ideg) * 60.0; /* minutes of arc */ 00566 imin = (UInt16)tmp; 00567 tmp = (tmp-imin) * 10000.0 + 0.5; /* 1/10000 minutes of arc */ 00568 00569 if (tmp >= 9999.9) { 00570 tmp = 0; 00571 imin++; 00572 } 00573 if (imin > 59) { 00574 imin = 0; 00575 ideg++; 00576 } 00577 00578 StrPrintF(lat_str, "%02u:%02u.%04u,%c", ideg, imin, (UInt16)tmp, ns); 00579 00580 /* Longitude */ 00581 if (lon < 0) { 00582 ew = 'W'; 00583 lon = -lon; 00584 } else { 00585 ew = 'E'; 00586 } 00587 00588 tmp = lon; 00589 ideg = (UInt16)tmp; 00590 tmp = (tmp-ideg) * 60.0; /* minutes of arc */ 00591 imin = (UInt16)tmp; 00592 tmp = (tmp-imin) * 10000.0 + 0.5; /* 1/10000 minutes of arc */ 00593 00594 if (tmp >= 9999.9) { 00595 tmp = 0; 00596 imin++; 00597 } 00598 if (imin > 59) { 00599 imin = 0; 00600 ideg++; 00601 } 00602 00603 StrPrintF(lon_str, "%03u:%02u.%04u,%c", ideg, imin, (UInt16)tmp, ew); 00604 } 00605 00606 00607 /****************************************************************************/ 00608 /** 00609 * \brief Convert position to string according to selected position unit 00610 * 00611 * \param lat Latitude 00612 * \param lon Longitude 00613 * \param pos_unit position unit <POS_D|POS_DM|POS_DMS|POS_UTM> 00614 * \param lat_strP ptr to Latitude/Easting String buffer 00615 * (>=20 bytes) 00616 * \param lon_strP ptr to Longitude/Northing String buffer 00617 * (>=20 bytes) 00618 * \param field_strP ptr to UTM field String buffer 00619 * If pos_unit != POS_UTM, an empty string is 00620 * returned. If conversion to UTM failed, "n.a." 00621 * is returned. 00622 * (>=5 bytes) 00623 * \param eastP UTM Easting returned by reference 00624 * (pass NULL if not needed) 00625 * \param northP UTM Northing returned by reference 00626 * (pass NULL if not needed) 00627 * 00628 * \return Conversion to UTM successful. Always true, if any 00629 * position format other than UTM is set in the preferences. 00630 * 00631 * \note eastP and northP are updated only if pos_unit == POS_UTM and 00632 * conversion to UTM was successful 00633 ****************************************************************************/ 00634 Boolean deg_to_str(double lat, double lon, UInt8 pos_unit, 00635 char *lat_strP, char *lon_strP, char *field_strP, 00636 double *eastP, double *northP) 00637 { 00638 Int32 err; /* error code */ 00639 Int32 zone; /* UTM Zone */ 00640 char hemisphere; /* 'N' / 'S' */ 00641 double easting; /* Easting in m */ 00642 double northing; /* Northing in m */ 00643 00644 field_strP[0] = '\0'; 00645 00646 switch (pos_unit) { 00647 case POS_D: 00648 /* Degrees */ 00649 deg_to_d_str(lat, lon, lat_strP, lon_strP); 00650 break; 00651 00652 case POS_DM: 00653 deg_to_dm_str(lat, lon, lat_strP, lon_strP); 00654 break; 00655 00656 case POS_DMS: 00657 deg_to_dms_str(lat, lon, lat_strP, lon_strP); 00658 break; 00659 00660 case POS_UTM: 00661 /* Universal Transverse Mercator - convert from lat/lon double values */ 00662 err = Convert_Geodetic_To_UTM ( 00663 deg2rad(lat) /* Latitude (rad) */, 00664 deg2rad(lon) /* Longitude (rad) */, 00665 &zone /* long *Zone */, 00666 &hemisphere /* Hemisphere */, 00667 &easting /* *Easting */, 00668 &northing /* *Northing */); 00669 00670 if (err == UTM_NO_ERROR) { 00671 /* print zone and belt to string */ 00672 StrPrintF(field_strP, "%ld%c", zone, Lat_To_UTM_Belt(lat)); 00673 00674 /* print easting to string */ 00675 format_number(easting, 1, lat_strP); 00676 00677 /* print northing to string */ 00678 format_number(northing, 1, lon_strP); 00679 00680 /* return easting and northing if pointers are not NULL */ 00681 if (eastP && northP) { 00682 *eastP = easting; 00683 *northP = northing; 00684 } 00685 } else { 00686 StrCopy(field_strP, "n.a."); 00687 lat_strP[0] = '\0'; 00688 lon_strP[0] = '\0'; 00689 return false; 00690 } 00691 break; 00692 } /* switch (pos_unit) */ 00693 00694 return true; 00695 } 00696 00697 00698 /****************************************************************************/ 00699 /** 00700 * Calculate Great Circle distance on spheroid of two points 00701 * specified by latitude and longitude in degrees. 00702 * 00703 * Source: Aviation Formulary V1.41 00704 * By Ed Williams 00705 * http://williams.best.vwh.net/avform.htm 00706 * 00707 * \param lat1 latitude of point 1 00708 * \param lon1 longitude of point 1 00709 * \param lat2 latitude of point 2 00710 * \param lon2 longitude of point 2 00711 * 00712 * \return distance in radians 00713 ****************************************************************************/ 00714 double gc_dist_sphere(double lat1, double lon1, double lat2, double lon2) 00715 { 00716 double dist; /* distance in radians */ 00717 00718 lat1 = deg2rad(lat1); 00719 lon1 = deg2rad(lon1); 00720 lat2 = deg2rad(lat2); 00721 lon2 = deg2rad(lon2); 00722 00723 00724 dist = 2*asin(sqrt(SQR(sin((lat1-lat2)/2)) + 00725 cos(lat1)*cos(lat2)*SQR(sin((lon1-lon2)/2)))); 00726 00727 return dist; 00728 } 00729 00730 00731 /****************************************************************************/ 00732 /** 00733 * Calculate initial Great Circle course 00734 * between two points on spheroid 00735 * specified by latitude and longitude in degrees. 00736 * 00737 * As opposed to the source (see below), the following 00738 * convention is followed: 00739 * - East longitude: positive 00740 * - West longitude: negative 00741 * 00742 * Source: Aviation Formulary V1.41 00743 * By Ed Williams 00744 * http://williams.best.vwh.net/avform.htm 00745 * 00746 * \param lat1 latitude of point 1 00747 * \param lon1 longitude of point 1 00748 * \param lat2 latitude of point 2 00749 * \param lon2 longitude of point 2 00750 * 00751 * \return initial course in radians 00752 ****************************************************************************/ 00753 double gc_course_sphere(double lat1, double lon1, double lat2, double lon2) 00754 { 00755 double tc1; 00756 00757 lat1 = deg2rad(lat1); 00758 lon1 = deg2rad(lon1); 00759 lat2 = deg2rad(lat2); 00760 lon2 = deg2rad(lon2); 00761 00762 /* 00763 * We obtain the initial course, tc1, (at point 1) 00764 * from point 1 to point 2 by the following. 00765 */ 00766 00767 if (cos(lat1) < EPS) { /* EPS a small number ~ machine precision */ 00768 /* 00769 * The formula fails if the initial point is a pole. 00770 * We can special case this with: 00771 */ 00772 if (lat1 > 0) { 00773 tc1 = M_PI; /* starting from N pole */ 00774 } else { 00775 tc1 = 2*M_PI; /* starting from S pole */ 00776 } 00777 } else { 00778 /* For starting points other than the poles: */ 00779 tc1 = fmod(atan2(sin(lon2-lon1)*cos(lat2), 00780 cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)), 2*M_PI); 00781 00782 tc1 = (tc1 < 0) ? tc1+2*M_PI : tc1; 00783 } 00784 return tc1; 00785 }