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 1301cb7e3affebbd368044b16d3790d07b0c57f2..addeed08840e2572792beda19bc8aae7344652a1 100644 (file)
--- 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 77d7e8742f2cb04ff3810814b7439901dcc0d01c..1c78ed17738016e040a31d2884f1d27297f593c3 100644 (file)
--- 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 c2a6a0ee8aaf706cb0eeea1fad6033abad8bcdec..7f055774c440346a1e86b0190499e8c7c16d4a07 100644 (file)
--- a/liblwgeom/cunit/cu_geodetic.h
+++ b/liblwgeom/cunit/cu_geodetic.h
@@ -15,7 +15,7 @@
 #include <string.h>
 #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 678394354000b688428ba47b8fa9a23a1296dc5f..0e5b2ec2928ef63a1e6e75aaf3b6ea8fe27f9bc3 100644 (file)
--- 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 83fee0daa74e3451d51f729629289fbd8efc9739..8facea5d0c641835a9141b954f4195624f93fd67 100644 (file)
--- 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 (file)
index 0000000..b7472f2
--- /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 (file)
index 0000000..023fabd
--- /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);
+
+