More robust geography distance

References #4290

git-svn-id: http://svn.osgeo.org/postgis/branches/2.5@17182 b70326c6-7e19-0410-871a-916f4a2858ee

diff --git a/liblwgeom/lwgeodetic.c b/liblwgeom/lwgeodetic.c
index 5af1b53ce88f6568bde9e21e5f926b8a856b2f59..249270a34da0689a5e3fa07b51c923813015a30c 100644 (file)
--- a/liblwgeom/lwgeodetic.c
+++ b/liblwgeom/lwgeodetic.c
@@ -35,6 +35,15 @@
 */
 int gbox_geocentric_slow = LW_FALSE;
 
+/**
+* Utility function for ptarray_contains_point_sphere()
+*/
+static int
+point3d_equals(const POINT3D *p1, const POINT3D *p2)
+{
+       return FP_EQUALS(p1->x, p2->x) && FP_EQUALS(p1->y, p2->y) && FP_EQUALS(p1->z, p2->z);
+}
+
 /**
 * Convert a longitude to the range of -PI,PI
 */
@@ -3373,6 +3382,10 @@ point_in_cone(const POINT3D *A1, const POINT3D *A2, const POINT3D *P)
        POINT3D AC; /* Center point of A1/A2 */
        double min_similarity, similarity;
 
+       /* Boundary case */
+       if (point3d_equals(A1, P) || point3d_equals(A2, P))
+               return LW_TRUE;
+
        /* The normalized sum bisects the angle between start and end. */
        vector_sum(A1, A2, &AC);
        normalize(&AC);
@@ -3380,27 +3393,51 @@ point_in_cone(const POINT3D *A1, const POINT3D *A2, const POINT3D *P)
        /* The projection of start onto the center defines the minimum similarity */
        min_similarity = dot_product(A1, &AC);
 
-       /* The projection of candidate p onto the center */
-       similarity = dot_product(P, &AC);
+       /* If the edge is sufficiently curved, use the dot product test */
+       if (fabs(1.0 - min_similarity) > 1e-10)
+       {
+               /* The projection of candidate p onto the center */
+               similarity = dot_product(P, &AC);
 
-       /* If the point is more similar than the end, the point is in the cone */
-       if ( similarity > min_similarity || fabs(similarity - min_similarity) < 2e-16 )
+               /* If the projection of the candidate is larger than */
+               /* the projection of the start point, the candidate */
+               /* must be closer to the center than the start, so */
+               /* therefor inside the cone */
+               if (similarity > min_similarity)
+               {
+                       return LW_TRUE;
+               }
+               else
+               {
+                       return LW_FALSE;
+               }
+       }
+       else
        {
-               return LW_TRUE;
+               /* Where the edge is very narrow, the dot product test */
+               /* fails, but we can use the almost-planar nature of the */
+               /* problem space then to test if the vector from the */
+               /* candidate to the start point in a different direction */
+               /* to the vector from candidate to end point */
+               /* If so, then candidate is between start and end */
+               POINT3D PA1, PA2;
+               vector_difference(P, A1, &PA1);
+               vector_difference(P, A2, &PA2);
+               normalize(&PA1);
+               normalize(&PA2);
+               if (dot_product(&PA1, &PA2) < 0.0)
+               {
+                       return LW_TRUE;
+               }
+               else
+               {
+                       return LW_FALSE;
+               }
        }
        return LW_FALSE;
 }
 
 
-/**
-* Utility function for ptarray_contains_point_sphere()
-*/
-static int
-point3d_equals(const POINT3D *p1, const POINT3D *p2)
-{
-       return FP_EQUALS(p1->x, p2->x) && FP_EQUALS(p1->y, p2->y) && FP_EQUALS(p1->z, p2->z);
-}
-
 /**
 * Utility function for edge_intersects(), signum with a tolerance
 * in determining if the value is zero.