/* EarthDist - calculate distance on earth surface * Copyright (C) 2004 Tim Janik * * This software is provided "as is"; redistribution and modification * is permitted, provided that the following disclaimer is retained. * * This software is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. * In no event shall the authors or contributors be liable for any * direct, indirect, incidental, special, exemplary, or consequential * damages (including, but not limited to, procurement of substitute * goods or services; loss of use, data, or profits; or business * interruption) however caused and on any theory of liability, whether * in contract, strict liability, or tort (including negligence or * otherwise) arising in any way out of the use of this software, even * if advised of the possibility of such damage. * * This code was transcribed from fortran into C by Tim Janik. the original * fortran code, which is based on the April, 1975 paper published in Survey * Review by T. Vincenty, can be obtained from * http://www.ngs.noaa.gov/PC_PROD/Inv_Fwd/. */ #include "earthdist.h" #include #include #if 1 /* WGS84 */ #define A (6378137.0) /* semi-major axis of reference ellipsoid (in meters) */ #define F (1.0 / 298.25722356300) /* flattening */ #else /* GRS80 */ #define A (6378137.0) /* semi-major axis of reference ellipsoid (in meters) */ #define F (1.0 / 298.25722210088) /* flattening */ #endif #define ESQ (F * (2.0 - F)) /* eccentricity squared for reference ellipsoid */ #define TWOPI (M_PI + M_PI) #define SQR(v) ((v) * (v)) #ifndef FALSE #define FALSE (0) #endif #ifndef TRUE #define TRUE (!FALSE) #endif /* name: gpnarc * version: 200005.26 * written by: Robert (Sid) Safford * purpose: subroutine to compute the length of a meridional arc * between two latitudes * * comments: *********1*********2*********3*********4*********5*********6*********7* * ::modification history * ::197507.05, rws, ver 00 tencol released for field use * ::198311.20, rws, ver 01 mten released to field * ::198411.26, rws, ver 07 mten2 released to field * ::1985xx.xx, rws, code created * ::198506.10, rws, wrk enhancements released to field * ::198509.01, rws, ver 11 mten3 released to field * ::198512.18, rws, code modified for mten3 * ::198708.10, rws, code modified to use new mten4 gpn record format * ::199112.31, rws, ver 20 mten4 released to field * ::200001.13, rws, ver 21 mten4 released to field * ::200005.26, rws, code restructured & documentation added * ::200012.31, rws, ver 23 mten5 released * ********1*********2*********3*********4*********5*********6*********7* * e::gpnarc * --------------------------- * m t e n (version 3) * m t e n (version 5.23) * --------------------------- */ static int gpnarc (double amax, /* semi-major axis of reference ellipsoid */ double p1, /* lat station 1 (in radians) */ double p2, /* lat station 2 (in radians) */ double *arc) /* geodetic distance */ { static const double tt = 5e-15; double a, b, c, d, e, f, e2, e4, e6, e8, s1, s2, t1, t2, t3, t4, t5, da, db, dc, dd, de, df, ex; int flag = FALSE; /* check for a 90 degree lookup */ s1 = fabs (p1); s2 = fabs (p2); if (M_PI_2 - tt < s2 && s2 < M_PI_2 + tt) flag = TRUE; if (s1 > tt) flag = FALSE; da = p2 - p1; s1 = 0.0; s2 = 0.0; /* compute the length of a meridional arc between two latitudes */ e2 = ESQ; e4 = e2 * e2; e6 = e4 * e2; e8 = e6 * e2; ex = e8 * e2; t1 = e2 * 0.75; t2 = e4 * 0.234375; t3 = e6 * 0.068359375; t4 = e8 * 0.01922607421875; t5 = ex * 0.00528717041015625; a = t1 + 1.0 + t2 * 3.0 + t3 * 10.0 + t4 * 35.0 + t5 * 126.0; if (flag) goto L1; b = t1 + t2 * 4.0 + t3 * 15.0 + t4 * 56.0 + t5 * 210.0; c = t2 + t3 * 6.0 + t4 * 28.0 + t5 * 120.0; d = t3 + t4 * 8.0 + t5 * 45.0; e = t4 + t5 * 10.0; f = t5; db = sin (p2 * 2.0) - sin (p1 * 2.0); dc = sin (p2 * 4.0) - sin (p1 * 4.0); dd = sin (p2 * 6.0) - sin (p1 * 6.0); de = sin (p2 * 8.0) - sin (p1 * 8.0); df = sin (p2 * 10.0) - sin (p1 * 10.0); /* compute the s2 part of the series expansion */ s2 = -db * b / 2.0 + dc * c / 4.0 - dd * d / 6.0 + de * e / 8.0 - df * f / 10.0; L1: /* compute the s1 part of the series expansion */ s1 = da * a; /* compute the arc length */ *arc = amax * (1.0 - ESQ) * (s1 + s2); return 0; } /* gpnarc() */ /* name: gpnloa * version: 200005.26 * written by: Robert (Sid) Safford * purpose: subroutine to compute the liff-off-azimuth constants * * comments: *********1*********2*********3*********4*********5*********6*********7* * ::modification history * ::1985xx.xx, rws, code created * ::198506.10, rws, wrk enhancements released to field * ::198509.01, rws, ver 11 mten3 released to field * ::198512.18, rws, code modified for mten3 * ::198708.10, rws, code modified to use new mten4 gpn record format * ::199112.31, rws, ver 20 mten4 released to field * ::200001.13, rws, ver 21 mten4 released to field * ::200005.26, rws, code restructured & documentation added * ::200012.31, rws, ver 23 mten5 released *********1*********2*********3*********4*********5*********6*********7* * e::gpnloa * --------------------------- * m t e n (version 3) * (version 4.22) * (version 5.23) * --------------------------- */ static int gpnloa (double amax, /* semi-major axis of reference ellipsoid */ double *dl, /* lon difference */ double *az1, /* azi at sta 1 -> sta 2 */ double *az2, /* az2 at sta 2 -> sta 1 */ double *ao, /* const */ double *bo, /* const */ double *sms) /* distance ... equatorial - geodesic (S - s) "SMS" */ { static const double tt = 5e-13; double f, s, c2, t1, t2, u2, t4, u4, t6, u6, t8, u8, cs, az, dlon, cons; int iter; double esqp; dlon = fabs (*dl); cons = (M_PI - dlon) / (M_PI * F); f = F; /* compute an approximate az */ az = asin (cons); t1 = 1.0; t2 = f * -0.25 * (f + 1.0 + f * f); t4 = f * 0.1875 * f * (f * 2.25 + 1.0); t6 = f * -0.1953125 * f * f; iter = 0; L1: ++iter; s = cos (az); c2 = s * s; /* compute new ao */ *ao = t1 + t2 * c2 + t4 * c2 * c2 + t6 * c2 * c2 * c2; cs = cons / *ao; s = asin (cs); if (fabs (s - az) < tt) goto L2; az = s; if (iter <= 6) goto L1; L2: *az1 = s; if (*dl < 0.0) *az1 = TWOPI - *az1; *az2 = TWOPI - *az1; /* equatorial - geodesic (S - s) "SMS" */ esqp = ESQ / (1.0 - ESQ); s = cos (*az1); u2 = esqp * s * s; u4 = u2 * u2; u6 = u4 * u2; u8 = u6 * u2; t1 = 1.0; t2 = u2 * 0.25; t4 = u4 * -0.046875; t6 = u6 * 0.01953125; t8 = u8 * -0.01068115234375; *bo = t1 + t2 + t4 + t6 + t8; s = sin (*az1); *sms = amax * M_PI * (1. - F * fabs (s) * *ao - *bo * (1.0 - F)); return 0; } /* gpnloa() */ /* name: gpnhri * version: 200208.09 * written by: robert (sid) safford * purpose: subroutine to compute helmert rainsford inverse problem * * solution of the geodetic inverse problem after t. vincenty * modified rainsford's method with helmert's elliptical terms * effective in any azimuth and at any distance short of antipocal * from/to stations must not be the geographic pole. * parameter a is the semi-major axis of the reference ellipsoid * finv=1/f is the inverse flattening of the reference ellipsoid * latitudes and longitudes in radians positive north and west * forward and back azimuths returned in radians clockwise from south * geodesic distance s returned in units of semi-major axis a * programmed for ibm 360-195 09/23/75 * * note - note - note - * 1. do not use for meridional arcs and be careful on the equator. * 2. azimuths are from north(+) clockwise and * 3. longitudes are positive east(+) * * comments: *********1*********2*********3*********4*********5*********6*********7* * ::modification history * ::197507.05, rws, ver 00 tencol released for field use * ::198311.20, rws, ver 01 mten released to field * ::198411.26, rws, ver 07 mten2 released to field * ::198506.10, rws, wrk enhancements released to field * ::198507.22, rws, code modified for mten3 * ::198509.01, rws, ver 11 mten3 released to field * ::198708.10, rws, code modified to use new mten4 gpn record format * ::199112.31, rws, ver 20 mten4 released to field * ::200001.13, rws, ver 21 mten4 released to field * ::200005.26, rws, code restructured & documentation added * ::200012.31, rws, ver 23 mten5 released * ::200104.09, rws, code added to calblin program * ::200208.09, rws, code added subroutines gpnarc & gpnloa *********1*********2*********3*********4*********5*********6*********7* * e::gpnhri * ------------------------------- * m t e n (version 3) * (version 4.22) * (version 5.23) * ------------------------------- */ static int gpnhri (double p1, /* lat station 1 (in radians) */ double e1, /* lon station 1 (in radians) */ double p2, /* lat station 2 (in radians) */ double e2, /* lon station 2 (in radians) */ double *az1, /* azimuth forward at station 1 -> station 2 (in radians) */ double *az2, /* azimuth back at station 2 -> station 1 (in radians) */ double *s) /* geodetic distance between stations 1 & 2 (in meters) */ { static const double tol0 = 5e-15; /* tolerance for checking computation value */ static const double tol1 = 5e-14; /* tolerance for checking a real zero value */ static const double tol2 = 0.007; /* tolerance for close to zero value */ double aa; /* constant from subroutine gpnloa */ double bb; /* constant from subroutine gpnloa */ double alimit; /* equatorial arc distance along the equator (radians) */ double arc; /* meridional arc distance latitude p1 to p2 (in meters) */ double dlon; /* temporary value for difference in longitude (radians) */ double equ; /* equatorial distance (in meters) */ double r1, r2, ss; /* temporary variables */ double sms; /* equatorial - geodesic distance (S - s) "Sms" */ /* Local variables */ double b, w, z, a2, b2, a4, f0, f2, f3, f4, a6, b4, b6, q2; double u1, u2, t4, q4, t6, q6, r3, ab, ao, bo, qo, xy, xz; double cu1, cu2, su1, su2, sig, csig, clon, ssig; double epsq, slon; double sinalf; int kount; /* test the longitude difference with tol1 * tol1 is approximately 0.000000001 arc seconds */ ss = e2 - e1; if (fabs (ss) < tol1) { fprintf (stderr, "longitudal difference is near zero\n"); e2 += tol1; r2 = p2; r1 = p1; gpnarc (M_PI, r1, r2, &arc); *s = fabs (arc); if (p2 > p1) { *az1 = 0.0; *az2 = M_PI; } else { *az1 = M_PI; *az2 = 0.0; } return 0; } /* test for longitude over 180 degrees */ dlon = e2 - e1; if (dlon >= 0.) { if (M_PI <= dlon && dlon < TWOPI) dlon -= TWOPI; } else { ss = fabs (dlon); if (M_PI <= ss && ss < TWOPI) dlon += TWOPI; } ss = fabs (dlon); if (ss > M_PI) { /* fprintf (stderr, "Longitude difference over 180 degrees, Turn it around\n"); */ ss = TWOPI - ss; } /* compute the limit in longitude (alimit), it is equal * to twice the distance from the equator to the pole, * as measured along the equator (east/ewst) */ alimit = M_PI * (1.0 - F); /* test for anti-nodal difference */ if (ss >= alimit) { r1 = fabs (p1); r2 = fabs (p2); /* latitudes r1 & r2 are not near the equator */ if (r1 > tol2 && r2 > tol2) goto L60; /* longitude difference is greater than lift-off point * now check to see if "both" r1 & r2 are on equator */ if (r1 < tol1 && r2 > tol2) goto L60; if (r2 < tol1 && r1 > tol2) goto L60; /* check for either r1 or r2 just off the equator but < tol2 */ if (r1 > tol1 || r2 > tol1) { *az1 = 0.; *az2 = 0.; *s = 0.; return 0; } /* compute the azimuth to anti-nodal point */ /* fprintf (stderr, "Longitude difference beyond lift-off point\n"); */ gpnloa (M_PI, &dlon, az1, az2, &aa, &bb, &sms); /* compute the equatorial distance & geodetic */ equ = A * fabs (dlon); *s = equ - sms; return 0; } L60: f0 = 1.0 - F; b = A * f0; epsq = ESQ / (1.0 - ESQ); f2 = F * F; f3 = F * f2; f4 = F * f3; /* the longitude difference */ dlon = e2 - e1; ab = dlon; kount = 0; /* the reduced latitudes */ u1 = f0 * sin (p1) / cos (p1); u2 = f0 * sin (p2) / cos (p2); u1 = atan (u1); u2 = atan (u2); su1 = sin (u1); cu1 = cos (u1); su2 = sin (u2); cu2 = cos (u2); /* counter for the iteration operation */ L1: ++kount; clon = cos (ab); slon = sin (ab); csig = su1 * su2 + cu1 * cu2 * clon; ssig = sqrt (SQR (slon * cu2) + SQR (su2 * cu1 - su1 * cu2 * clon)); sig = atan2 (ssig, csig); sinalf = cu1 * cu2 * slon / ssig; w = 1.0 - sinalf * sinalf; t4 = w * w; t6 = w * t4; /* the coefficients of type a */ ao = F - f2 * (F + 1.0 + f2) * w / 4.0 + f3 * 3.0 * (F * 9.0 / 4.0 + 1.0) * t4 / 16.0 - f4 * 25.0 * t6 / 128.0; a2 = f2 * (F + 1.0 + f2) * w / 4.0 - f3 * (F * 9.0 / 4.0 + 1.0) * t4 / 4.0 + f4 * 75.0 * t6 / 256.0; a4 = f3 * (F * 9.0 / 4.0 + 1.0) * t4 / 32.0 - f4 * 15.0 * t6 / 256.0; a6 = f4 * 5.0 * t6 / 768.0; /* the multiple angle functions */ qo = 0.0; if (w > tol0) qo = su1 * -2.0 * su2 / w; q2 = csig + qo; q4 = q2 * 2.0 * q2 - 1.0; q6 = q2 * (q2 * 4.0 * q2 - 3.0); r2 = ssig * 2.0 * csig; r3 = ssig * (3.0 - ssig * 4.0 * ssig); /* the longitude difference */ *s = sinalf * (ao * sig + a2 * ssig * q2 + a4 * r2 * q4 + a6 * r3 * q6); xz = dlon + *s; // xy = dabs(xz-ab) xy = fabs (xz - ab); ab = dlon + *s; if (xy < 5e-14) goto L4; if (kount <= 7) goto L1; /* the coefficients of type b */ L4: z = epsq * w; bo = z * (z * (z * (0.01953125 - z * 175.0 / 16384.0) - 0.046875) + 0.25) + 1.0; b2 = z * (z * (z * (z * 35.0 / 2048.0 - 0.029296875) + 0.0625) - 0.25); b4 = z * z * (z * (0.005859375 - z * 35.0 / 8192.0) - 0.0078125); b6 = z * z * z * (z * 5.0 / 6144.0 - 6.5104166666666663e-4); /* the distance in meters */ *s = b * (bo * sig + b2 * ssig * q2 + b4 * r2 * q4 + b6 * r3 * q6); /* first compute the az1 & az2 for along the equator */ if (dlon > M_PI) dlon -= TWOPI; if (fabs (dlon) > M_PI) dlon += TWOPI; *az1 = M_PI_2; if (dlon < 0.0) *az1 *= 3.0; *az2 = *az1 + M_PI; if (*az2 > TWOPI) *az2 -= TWOPI; /* now compute the az1 & az2 for latitudes not on the equator */ if (!(fabs (su1) < tol0 && fabs (su2) < tol0)) { double tana1 = slon * cu2 / (su2 * cu1 - clon * su1 * cu2); double tana2 = slon * cu1 / (su1 * cu2 - clon * su2 * cu1); double sina1 = sinalf / cu1; double sina2 = -sinalf / cu2; /* azimuths from north, longitudes positive east */ *az1 = atan2 (sina1, sina1 / tana1); *az2 = M_PI - atan2 (sina2, sina2 / tana2); } if (*az1 < 0.0) *az1 += TWOPI; if (*az2 < 0.0) *az2 += TWOPI; return 0; } /* gpnhri() */ double gps_earth_distance (double latitude1, double longitude1, double latitude2, double longitude2, double *azimuth_forw, double *azimuth_back) { double dummy1, dummy2, geodetic_distance = 0; gpnhri (latitude1 * M_PI / 180.0, longitude1 * M_PI / 180.0, latitude2 * M_PI / 180.0, longitude2 * M_PI / 180.0, azimuth_forw ? azimuth_forw : &dummy1, azimuth_back ? azimuth_back : &dummy2, &geodetic_distance); return geodetic_distance; } double gps_earth_latitude (double degree, double minute, double seconds, int is_south) { double latlong = degree + minute * (1. / 60.) + seconds * (1. / 3600.); return is_south ? -latlong : latlong; } double gps_earth_longitude (double degree, double minute, double seconds, int is_west) { double latlong = degree + minute * (1. / 60.) + seconds * (1. / 3600.); return is_west ? -latlong : latlong; }