213 lines
4.6 KiB
C
213 lines
4.6 KiB
C
![]() |
/*
|
|||
|
* geo-dist
|
|||
|
*
|
|||
|
* Copyright (C) 2006 Nils Faerber <nils.faerber@kernelconcepts.de>
|
|||
|
*
|
|||
|
*
|
|||
|
* This program is free software; you can redistribute it and/or modify
|
|||
|
* it under the terms of the GNU General Public License as published by
|
|||
|
* the Free Software Foundation; either version 2 of the License, or
|
|||
|
* (at your option) any later version.
|
|||
|
*
|
|||
|
* This program 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. See the
|
|||
|
* GNU General Public License for more details.
|
|||
|
*
|
|||
|
* You should have received a copy of the GNU General Public License
|
|||
|
* along with this program; if not, write to the Free Software
|
|||
|
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
|||
|
*
|
|||
|
*/
|
|||
|
|
|||
|
|
|||
|
/*
|
|||
|
* Calculate distances on Earth surface given geogrphic locations
|
|||
|
*/
|
|||
|
|
|||
|
#include <stdio.h>
|
|||
|
#include <math.h>
|
|||
|
|
|||
|
#include "geo-dist.h"
|
|||
|
|
|||
|
// #define DEBUG
|
|||
|
|
|||
|
|
|||
|
/*
|
|||
|
* Earth models
|
|||
|
* Remember, Earth is not a simple ball ;)
|
|||
|
*/
|
|||
|
struct ellipse_s {
|
|||
|
char *name;
|
|||
|
double a;
|
|||
|
double invf;
|
|||
|
};
|
|||
|
|
|||
|
static struct ellipse_s ellipse[] = {
|
|||
|
{
|
|||
|
"WGS84",
|
|||
|
6378.137 / 1.852,
|
|||
|
298.257223563
|
|||
|
}, {
|
|||
|
"NAD27",
|
|||
|
6378.2064 / 1.852,
|
|||
|
294.9786982138
|
|||
|
}, {
|
|||
|
"International",
|
|||
|
6378.388 / 1.852,
|
|||
|
297.0
|
|||
|
}, {
|
|||
|
"Krasovsky",
|
|||
|
6378.245 / 1.852,
|
|||
|
298.3
|
|||
|
}, {
|
|||
|
"Bessel",
|
|||
|
6377.397155 / 1.852,
|
|||
|
299.1528
|
|||
|
}, {
|
|||
|
"WGS72",
|
|||
|
6378.135 / 1.852,
|
|||
|
298.26
|
|||
|
}, {
|
|||
|
"WGS66",
|
|||
|
6378.145 / 1.852,
|
|||
|
298.25
|
|||
|
}, {
|
|||
|
"FAI sphere",
|
|||
|
6371.0 / 1.852,
|
|||
|
1000000000.
|
|||
|
}
|
|||
|
};
|
|||
|
|
|||
|
|
|||
|
/*
|
|||
|
* math helper
|
|||
|
*/
|
|||
|
#if 0
|
|||
|
double atan2(double y, double x)
|
|||
|
{
|
|||
|
double out=0.;
|
|||
|
|
|||
|
if (x < 0) {
|
|||
|
out = atan(y/x) + M_PI;
|
|||
|
} else
|
|||
|
if ((x > 0) && (y >= 0)) {
|
|||
|
out = atan(y/x);
|
|||
|
} else
|
|||
|
if ((x > 0) && (y < 0)) {
|
|||
|
out = atan(y/x) + 2. * M_PI;
|
|||
|
} else
|
|||
|
if ((x == 0) && (y > 0)) {
|
|||
|
out = M_PI / 2.;
|
|||
|
} else
|
|||
|
if ((x == 0) && (y < 0)) {
|
|||
|
out = 3. * M_PI / 2.;
|
|||
|
} else
|
|||
|
if ((x == 0) && (y == 0)) {
|
|||
|
out= 0.;
|
|||
|
}
|
|||
|
|
|||
|
return out;
|
|||
|
}
|
|||
|
#endif
|
|||
|
|
|||
|
/*
|
|||
|
* glat1, glat2: geodetic latitude in radians, N positive
|
|||
|
* glon1, glon2: geodetic longitude in radians E positive
|
|||
|
* Coordinates in radians means
|
|||
|
* 50<EFBFBD>50.32N ^= 50 degree, 50 minutes and 32 seconds
|
|||
|
* ^= 50 + (50 / 60) + (32 / 3600)
|
|||
|
* ^= 50.84222
|
|||
|
*
|
|||
|
* returns: distance in nm (nautical miles), factor of 1.852 gives km
|
|||
|
*/
|
|||
|
double dist_ell(double glat1, double glon1, double glat2, double glon2, EllipsoidType e_model)
|
|||
|
{
|
|||
|
double a = ellipse[e_model].a; // 6378.137/1.852
|
|||
|
double invf = ellipse[e_model].invf; // 298.257223563
|
|||
|
double f = 1. / invf;
|
|||
|
double r, tu1, tu2, cu1, su1, cu2, s1, b1, f1;
|
|||
|
double x, sx, cx, sy=0, cy=0, y=0, sa, c2a=0, cz=0, e=0, c, d;
|
|||
|
double EPS = 0.00000000005;
|
|||
|
double s;
|
|||
|
double l_iter=1;
|
|||
|
double MAXITER=100;
|
|||
|
|
|||
|
#ifdef DEBUG
|
|||
|
printf(" calc dist from %.04lf %.04lf to %.04lf %.04lf\n", glat1, glon1, glat2, glon2);
|
|||
|
#endif
|
|||
|
glat1 = (M_PI / 180.) * glat1;
|
|||
|
glon1 = (M_PI / 180.) * -1. * glon1;
|
|||
|
glat2 = (M_PI / 180.) * glat2;
|
|||
|
glon2 = (M_PI / 180.) * -1. * glon2;
|
|||
|
|
|||
|
#ifdef DEBUG
|
|||
|
printf("Using ellipse model %s (%.04lf %.04lf)\n", ellipse[e_model].name, a, invf);
|
|||
|
printf(" calc dist from %.04lf %.04lf to %.04lf %.04lf\n", glat1, glon1, glat2, glon2);
|
|||
|
#endif
|
|||
|
|
|||
|
if ((glat1+glat2==0.) && (fabs(glon1-glon2)==M_PI)) {
|
|||
|
#ifdef DEBUG
|
|||
|
printf("Warning: Course and distance between antipodal points is undefined\n");
|
|||
|
#endif
|
|||
|
glat1=glat1+0.00001; // allow algorithm to complete
|
|||
|
}
|
|||
|
|
|||
|
if (glat1==glat2 && (glon1==glon2 || fabs(fabs(glon1-glon2)-2*M_PI) < EPS)) {
|
|||
|
#ifdef DEBUG
|
|||
|
printf("Points 1 and 2 are identical- course undefined\n");
|
|||
|
#endif
|
|||
|
return 0;
|
|||
|
}
|
|||
|
|
|||
|
r = 1. - f;
|
|||
|
tu1 = r * tan (glat1);
|
|||
|
tu2 = r * tan (glat2);
|
|||
|
cu1 = 1. / sqrt (1. + tu1 * tu1);
|
|||
|
su1 = cu1 * tu1;
|
|||
|
cu2 = 1. / sqrt (1. + tu2 * tu2);
|
|||
|
s1 = cu1 * cu2;
|
|||
|
b1 = s1 * tu2;
|
|||
|
f1 = b1 * tu1;
|
|||
|
x = glon2 - glon1;
|
|||
|
d = x + 1.; // force one pass
|
|||
|
|
|||
|
while ((fabs(d - x) > EPS) && (l_iter < MAXITER)) {
|
|||
|
l_iter = l_iter + 1;
|
|||
|
sx = sin (x);
|
|||
|
cx = cos (x);
|
|||
|
tu1 = cu2 * sx;
|
|||
|
tu2 = b1 - su1 * cu2 * cx;
|
|||
|
sy = sqrt (tu1 * tu1 + tu2 * tu2);
|
|||
|
cy = s1 * cx + f1;
|
|||
|
y = atan2 (sy, cy);
|
|||
|
sa = s1 * sx / sy;
|
|||
|
c2a = 1. - sa * sa;
|
|||
|
cz = f1 + f1;
|
|||
|
if (c2a > 0.)
|
|||
|
cz = cy - cz / c2a;
|
|||
|
e = cz * cz * 2. - 1.;
|
|||
|
c = ((-3. * c2a + 4.) * f + 4.) * c2a * f / 16.;
|
|||
|
d = x;
|
|||
|
x = ((e * cy * c + cz) * sy * c + y) * sa;
|
|||
|
x = (1. - c) * x * f + glon2 - glon1;
|
|||
|
}
|
|||
|
|
|||
|
x = sqrt ((1. / (r * r) - 1) * c2a + 1.);
|
|||
|
x += 1.;
|
|||
|
x = (x - 2.) / x;
|
|||
|
c = 1. - x;
|
|||
|
c = (x * x / 4. + 1.) / c;
|
|||
|
d = (0.375 * x * x - 1.) * x;
|
|||
|
x = e * cy;
|
|||
|
s = ((((sy*sy*4.-3.)*(1.-e-e)*cz*d/6.-x)*d/4.+cz)*sy*d+y)*c*a*r;
|
|||
|
#ifdef DEBUG
|
|||
|
if (fabs (l_iter - MAXITER) < EPS) {
|
|||
|
printf ("Algorithm did not converge\n");
|
|||
|
} else
|
|||
|
fprintf(stderr,"%s = %.4f\n", __FUNCTION__, s);
|
|||
|
#endif
|
|||
|
|
|||
|
return s;
|
|||
|
}
|