From: Paul Ramsey Date: Thu, 29 Oct 2009 19:24:06 +0000 (+0000) Subject: Spheroid distance calculation between points added. X-Git-Tag: 1.5.0b1~327 X-Git-Url: https://granicus.if.org/sourcecode?a=commitdiff_plain;h=ac8f490ddb0ba017f7057ab77d86d63b074eb550;p=postgis Spheroid distance calculation between points added. git-svn-id: http://svn.osgeo.org/postgis/trunk@4700 b70326c6-7e19-0410-871a-916f4a2858ee --- diff --git a/liblwgeom/Makefile.in b/liblwgeom/Makefile.in index 1301cb7e3..addeed088 100644 --- a/liblwgeom/Makefile.in +++ b/liblwgeom/Makefile.in @@ -49,7 +49,8 @@ SA_OBJS = \ g_ptarray.o \ g_serialized.o \ g_util.o \ - lwgeodetic.o + lwgeodetic.o \ + lwspheroid.o SA_HEADERS = \ liblwgeom.h \ diff --git a/liblwgeom/cunit/cu_geodetic.c b/liblwgeom/cunit/cu_geodetic.c index 77d7e8742..1c78ed177 100644 --- a/liblwgeom/cunit/cu_geodetic.c +++ b/liblwgeom/cunit/cu_geodetic.c @@ -36,7 +36,8 @@ CU_pSuite register_geodetic_suite(void) (NULL == CU_add_test(pSuite, "test_edge_distance_to_point()", test_edge_distance_to_point)) || (NULL == CU_add_test(pSuite, "test_edge_distance_to_edge()", test_edge_distance_to_edge)) || (NULL == CU_add_test(pSuite, "test_lwgeom_distance_sphere()", test_lwgeom_distance_sphere)) || - (NULL == CU_add_test(pSuite, "test_ptarray_point_in_ring()", test_ptarray_point_in_ring)) + (NULL == CU_add_test(pSuite, "test_ptarray_point_in_ring()", test_ptarray_point_in_ring)) || + (NULL == CU_add_test(pSuite, "test_spheroid_distance()", test_spheroid_distance)) ) { CU_cleanup_registry(); @@ -563,3 +564,43 @@ void test_lwgeom_distance_sphere(void) lwgeom_free(lwg2); } + +void test_spheroid_distance(void) +{ + /* WGS 84 values */ + static double a = 6378137.0; + static double b = 6356752.314245179498; + GEOGRAPHIC_POINT g1, g2; + double d; + + /* One vertical degree */ + point_set(0.0, 0.0, &g1); + point_set(0.0, 1.0, &g2); + d = spheroid_distance(g1, g2, a, b); + CU_ASSERT_DOUBLE_EQUAL(d, 110574.388615329, 0.001); + + /* Ten horizontal degrees */ + point_set(-10.0, 0.0, &g1); + point_set(0.0, 0.0, &g2); + d = spheroid_distance(g1, g2, a, b); + CU_ASSERT_DOUBLE_EQUAL(d, 1113194.90793274, 0.001); + + /* One horizonal degree */ + point_set(-1.0, 0.0, &g1); + point_set(0.0, 0.0, &g2); + d = spheroid_distance(g1, g2, a, b); + CU_ASSERT_DOUBLE_EQUAL(d, 111319.490779, 0.001); + + /* Around world w/ slight bend */ + point_set(-180.0, 0.0, &g1); + point_set(0.0, 1.0, &g2); + d = spheroid_distance(g1, g2, a, b); + CU_ASSERT_DOUBLE_EQUAL(d, 19893357.0704483, 0.001); + + /* Up to pole */ + point_set(-180.0, 0.0, &g1); + point_set(0.0, 90.0, &g2); + d = spheroid_distance(g1, g2, a, b); + CU_ASSERT_DOUBLE_EQUAL(d, 10001965.7295318, 0.001); + +} \ No newline at end of file diff --git a/liblwgeom/cunit/cu_geodetic.h b/liblwgeom/cunit/cu_geodetic.h index c2a6a0ee8..7f055774c 100644 --- a/liblwgeom/cunit/cu_geodetic.h +++ b/liblwgeom/cunit/cu_geodetic.h @@ -15,7 +15,7 @@ #include #include "CUnit/Basic.h" -#include "lwgeodetic.h" +#include "lwspheroid.h" #include "cu_tester.h" /*********************************************************************** @@ -35,3 +35,4 @@ void test_edge_distance_to_edge(void); void test_ptarray_point_in_ring_winding(void); void test_lwgeom_distance_sphere(void); void test_ptarray_point_in_ring(void); +void test_spheroid_distance(void); diff --git a/liblwgeom/lwgeodetic.c b/liblwgeom/lwgeodetic.c index 678394354..0e5b2ec29 100644 --- a/liblwgeom/lwgeodetic.c +++ b/liblwgeom/lwgeodetic.c @@ -174,7 +174,7 @@ static void point_shift(GEOGRAPHIC_POINT *p, double shift) } -static int geographic_point_equals(GEOGRAPHIC_POINT g1, GEOGRAPHIC_POINT g2) +int geographic_point_equals(GEOGRAPHIC_POINT g1, GEOGRAPHIC_POINT g2) { return FP_EQUALS(g1.lat, g2.lat) && FP_EQUALS(g1.lon, g2.lon); } diff --git a/liblwgeom/lwgeodetic.h b/liblwgeom/lwgeodetic.h index 83fee0daa..8facea5d0 100644 --- a/liblwgeom/lwgeodetic.h +++ b/liblwgeom/lwgeodetic.h @@ -78,3 +78,4 @@ double ptarray_area_sphere(POINTARRAY *pa, POINT2D pt_outside); double latitude_degrees_normalize(double lat); double longitude_degrees_normalize(double lon); double ptarray_length_sphere(POINTARRAY *pa); +int geographic_point_equals(GEOGRAPHIC_POINT g1, GEOGRAPHIC_POINT g2); diff --git a/liblwgeom/lwspheroid.c b/liblwgeom/lwspheroid.c new file mode 100644 index 000000000..b7472f264 --- /dev/null +++ b/liblwgeom/lwspheroid.c @@ -0,0 +1,105 @@ +#include "lwspheroid.h" + +#define POW2(x) ((x)*(x)) + +static double spheroid_mu2(double alpha, double a, double b) +{ + double b2 = POW2(b); + return POW2(cos(alpha)) * (POW2(a) - b2) / b2; +} + +static double spheroid_big_a(double u2) +{ + return 1.0 + (u2 / 16384.0) * (4096.0 + u2 * (-768.0 + u2 * (320.0 - 175.0 * u2))); +} + +static double spheroid_big_b(double u2) +{ + return (u2 / 1024.0) * (256.0 + u2 * (-128.0 + u2 * (74.0 - 47.0 * u2))); +} + +double spheroid_distance(GEOGRAPHIC_POINT r, GEOGRAPHIC_POINT s, double a, double b) +{ + double lambda = (s.lon - r.lon); + double f = (a - b)/a; + double omf = 1 - f; + double u1, u2; + double cos_u1, cos_u2; + double sin_u1, sin_u2; + double big_a, big_b, delta_sigma; + double alpha, sin_alpha, cos_alphasq, c; + double sigma, sin_sigma, cos_sigma, cos2_sigma_m, sqrsin_sigma, last_lambda, omega; + double distance; + int i = 0; + + /* Same point => zero distance */ + if( geographic_point_equals(r, s) ) + { + return 0.0; + } + + u1 = atan(omf * tan(r.lat)); + cos_u1 = cos(u1); + sin_u1 = sin(u1); + u2 = atan(omf * tan(s.lat)); + cos_u2 = cos(u2); + sin_u2 = sin(u2); + + omega = lambda; + do + { + sqrsin_sigma = POW2(cos_u2 * sin(lambda)) + + POW2((cos_u1 * sin_u2 - sin_u1 * cos_u2 * cos(lambda))); +// printf("\ni %d\n", i); +// printf("sqrsin_sigma %.12g\n", sqrsin_sigma); + sin_sigma = sqrt(sqrsin_sigma); +// printf("sin_sigma %.12g\n", sin_sigma); + cos_sigma = sin_u1 * sin_u2 + cos_u1 * cos_u2 * cos(lambda); +// printf("cos_sigma %.12g\n", cos_sigma); + sigma = atan2(sin_sigma, cos_sigma); +// printf("sigma %.12g\n", sigma); + sin_alpha = cos_u1 * cos_u2 * sin(lambda) / sin(sigma); +// printf("sin_alpha %.12g\n", sin_alpha); + if( FP_EQUALS(sin_alpha, 1.0) ) + alpha = M_PI_2; + else + alpha = asin(sin_alpha); +// printf("alpha %.12g\n", alpha); + cos_alphasq = POW2(cos(alpha)); +// printf("cos_alphasq %.12g\n", cos_alphasq); + cos2_sigma_m = cos(sigma) - (2.0 * sin_u1 * sin_u2 / cos_alphasq); +// printf("cos2_sigma_m %.12g\n", cos2_sigma_m); + if( cos2_sigma_m > 1.0 ) cos2_sigma_m = 1.0; +// printf("cos2_sigma_m %.12g\n", cos2_sigma_m); + if( cos2_sigma_m < -1.0 ) cos2_sigma_m = -1.0; +// printf("cos2_sigma_m %.12g\n", cos2_sigma_m); + c = (f / 16.0) * cos_alphasq * (4.0 + f * (4.0 - 3.0 * cos_alphasq)); +// printf("c %.12g\n", c); + last_lambda = lambda; +// printf("last_lambda %.12g\n", last_lambda); + lambda = omega + (1.0 - c) * f * sin(alpha) * (sigma + c * sin(sigma) * + (cos2_sigma_m + c * cos(sigma) * (-1.0 + 2.0 * POW2(cos2_sigma_m)))); +// printf("lambda %.12g\n", lambda); +// printf("i %d\n\n", i); + i++; + } + while ( i < 999 && lambda != 0.0 && fabs((last_lambda - lambda)/lambda) > 1.0e-9 ); + + u2 = spheroid_mu2(alpha, a, b); + big_a = spheroid_big_a(u2); + big_b = spheroid_big_b(u2); + delta_sigma = big_b * sin_sigma * (cos2_sigma_m + (big_b / 4.0) * (cos_sigma * (-1.0 + 2.0 * POW2(cos2_sigma_m)) - + (big_b / 6.0) * cos2_sigma_m * (-3.0 + 4.0 * sqrsin_sigma) * (-3.0 + 4.0 * POW2(cos2_sigma_m)))); + + distance = b * big_a * (sigma - delta_sigma); + + /* Algorithm failure, distance == NaN, fallback to sphere */ + if( distance != distance ) + { +// return (2.0 * a + b) * sphere_distance(r, s) / 3.0; + lwerror("spheroid_distance returned NaN: (%.20g %.20g) (%.20g %.20g) a = %.20g b = %.20g",r.lat, r.lon, s.lat, s.lon, a, b); + return -1.0; + } + + return distance; +} \ No newline at end of file diff --git a/liblwgeom/lwspheroid.h b/liblwgeom/lwspheroid.h new file mode 100644 index 000000000..023fabd66 --- /dev/null +++ b/liblwgeom/lwspheroid.h @@ -0,0 +1,16 @@ +#include "lwgeodetic.h" + +/* +** Useful information about spheroids: +** +** a = semi-major axis +** b = semi-minor axis = a - af +** f = flattening = (a - b)/a +** e = eccentricity (first) +** e_sq = eccentricity (first) squared = (a*a - b*b)/(a*a) +** omf = 1 - f +*/ + +double spheroid_distance(GEOGRAPHIC_POINT r, GEOGRAPHIC_POINT s, double a, double b); + +