Add lwgeom_cpa_within function

includes unit test

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

diff --git a/liblwgeom/cunit/cu_measures.c b/liblwgeom/cunit/cu_measures.c
index f71b2be8945eee06b7883a6fe8fba4079cb15477..975d8332bb0a46b187d542e5f61598652b2d1f60 100644 (file)
--- a/liblwgeom/cunit/cu_measures.c
+++ b/liblwgeom/cunit/cu_measures.c
@@ -946,10 +946,11 @@ test_lwgeom_tcpa(void)
        g2 = lwgeom_from_wkt("LINESTRING M(0 0 1, 0 0 5)", LW_PARSER_CHECK_NONE);
   dist  = -1;
        m = lwgeom_tcpa(g1, g2, &dist);
-       lwgeom_free(g1);
-       lwgeom_free(g2);
        ASSERT_DOUBLE_EQUAL(m, 1.0);
        ASSERT_DOUBLE_EQUAL(dist, 0.0);
+       CU_ASSERT( lwgeom_cpa_within(g1, g2, 0.0) == LW_TRUE );
+       lwgeom_free(g1);
+       lwgeom_free(g2);
 
   /* One of the tracks is still, the other passes at 10 meters from point */
 
@@ -957,10 +958,13 @@ test_lwgeom_tcpa(void)
        g2 = lwgeom_from_wkt("LINESTRING M(-10 10 1, 10 10 5)", LW_PARSER_CHECK_NONE);
   dist = -1;
        m = lwgeom_tcpa(g1, g2, &dist);
-       lwgeom_free(g1);
-       lwgeom_free(g2);
        ASSERT_DOUBLE_EQUAL(m, 3.0);
        ASSERT_DOUBLE_EQUAL(dist, 10.0);
+       CU_ASSERT( lwgeom_cpa_within(g1, g2, 11.0) == LW_TRUE );
+       CU_ASSERT( lwgeom_cpa_within(g1, g2, 10.0) == LW_TRUE );
+       CU_ASSERT( lwgeom_cpa_within(g1, g2, 9.0) == LW_FALSE );
+       lwgeom_free(g1);
+       lwgeom_free(g2);
 
   /* Equal tracks, 2d */
 

diff --git a/liblwgeom/liblwgeom.h.in b/liblwgeom/liblwgeom.h.in
index 7c7641f6364ed360681dc1ca90b39d0b460b4ed6..1e381e4b2f4bf0bb0033866a1a68c7d5aca9bd5f 100644 (file)
--- a/liblwgeom/liblwgeom.h.in
+++ b/liblwgeom/liblwgeom.h.in
@@ -1383,6 +1383,13 @@ extern double lwgeom_interpolate_point(const LWGEOM *lwin, const LWPOINT *lwpt);
 */
 extern double lwgeom_tcpa(const LWGEOM *g1, const LWGEOM *g2, double *mindist);
 
+/**
+* Is the closest point of approach within a distance ?
+*
+* @return LW_TRUE or LW_FALSE
+*/
+extern int lwgeom_cpa_within(const LWGEOM *g1, const LWGEOM *g2, double maxdist);
+
 /**
 * Return LW_TRUE or LW_FALSE depending on whether or not a geometry is
 * a linestring with measure value growing from start to end vertex

diff --git a/liblwgeom/lwlinearreferencing.c b/liblwgeom/lwlinearreferencing.c
index 27fa2174953df528a572ee290b9f5af068d05f3b..a3dbe068d4471fc463e40c374b6b89df3f47e15f 100644 (file)
--- a/liblwgeom/lwlinearreferencing.c
+++ b/liblwgeom/lwlinearreferencing.c
@@ -1196,7 +1196,6 @@ lwgeom_tcpa(const LWGEOM *g1, const LWGEOM *g2, double *mindist)
       mintime = t;
        // lwnotice("MINTIME: %g", mintime);
     }
-
   }
 
   /*
@@ -1212,3 +1211,153 @@ lwgeom_tcpa(const LWGEOM *g1, const LWGEOM *g2, double *mindist)
 
   return mintime;
 }
+
+int
+lwgeom_cpa_within(const LWGEOM *g1, const LWGEOM *g2, double maxdist)
+{
+  LWLINE *l1, *l2;
+  int i;
+  const GBOX *gbox1, *gbox2;
+  double tmin, tmax;
+  double *mvals;
+  int nmvals = 0;
+  double maxdist2 = maxdist * maxdist;
+  int within = LW_FALSE;
+
+       if ( ! lwgeom_has_m(g1) || ! lwgeom_has_m(g2) ) {
+               lwerror("Both input geometries must have a measure dimension");
+    return LW_FALSE;
+  }
+
+  l1 = lwgeom_as_lwline(g1);
+  l2 = lwgeom_as_lwline(g2);
+
+       if ( ! l1 || ! l2 ) {
+               lwerror("Both input geometries must be linestrings");
+    return LW_FALSE;
+  }
+
+  if ( l1->points->npoints < 2 || l2->points->npoints < 2 ) {
+    /* TODO: return distance between these two points */
+               lwerror("Both input lines must have at least 2 points");
+    return LW_FALSE;
+  }
+
+  /* WARNING: these ranges may be wider than real ones */
+  gbox1 = lwgeom_get_bbox(g1);
+  gbox2 = lwgeom_get_bbox(g2);
+
+  assert(gbox1); /* or the npoints check above would have failed */
+  assert(gbox2); /* or the npoints check above would have failed */
+
+  /*
+   * Find overlapping M range
+   * WARNING: may be larger than the real one
+   */
+
+  tmin = FP_MAX(gbox1->mmin, gbox2->mmin);
+  tmax = FP_MIN(gbox1->mmax, gbox2->mmax);
+
+  if ( tmax < tmin ) {
+         LWDEBUG(1, "Inputs never exist at the same time");
+    return LW_FALSE;
+  }
+
+   // lwnotice("Min:%g, Max:%g", tmin, tmax);
+
+  /*
+   * Collect M values in common time range from inputs
+   */
+
+  mvals = lwalloc( sizeof(double) *
+                   ( l1->points->npoints + l2->points->npoints ) );
+
+  /* TODO: also clip the lines ? */
+  nmvals  = ptarray_collect_mvals(l1->points, tmin, tmax, mvals);
+  nmvals += ptarray_collect_mvals(l2->points, tmin, tmax, mvals + nmvals);
+
+  /* Sort values in ascending order */
+  qsort(mvals, nmvals, sizeof(double), compare_double);
+
+  /* Remove duplicated values */
+  nmvals = uniq(mvals, nmvals);
+
+  if ( nmvals < 2 )
+  {{
+    /* there's a single time, must be that one... */
+    double t0 = mvals[0];
+    POINT4D p0, p1;
+    LWDEBUGF(1, "Inputs only exist both at a single time (%g)", t0);
+    if ( -1 == ptarray_locate_along_linear(l1->points, t0, &p0, 0) )
+    {
+      lwerror("Could not find point with M=%g on first geom", t0);
+      return -1;
+    }
+    if ( -1 == ptarray_locate_along_linear(l2->points, t0, &p1, 0) )
+    {
+      lwerror("Could not find point with M=%g on second geom", t0);
+      return -1;
+    }
+    if ( distance3d_pt_pt((POINT3D*)&p0, (POINT3D*)&p1) <= maxdist )
+      within = LW_TRUE;
+    lwfree(mvals);
+    return within;
+  }}
+
+  /*
+   * For each consecutive pair of measures, compute time of closest point
+   * approach and actual distance between points at that time
+   */
+  for (i=1; i<nmvals; ++i)
+  {
+    double t0 = mvals[i-1];
+    double t1 = mvals[i];
+    POINT4D p0, p1, q0, q1;
+    int seg;
+    double dist2;
+
+    // lwnotice("T %g-%g", t0, t1);
+
+    seg = ptarray_locate_along_linear(l1->points, t0, &p0, 0);
+    if ( -1 == seg ) continue; /* possible, if GBOX is approximated */
+    // lwnotice("Measure %g on segment %d of line 1: %g, %g, %g", t0, seg, p0.x, p0.y, p0.z);
+
+    seg = ptarray_locate_along_linear(l1->points, t1, &p1, seg);
+    if ( -1 == seg ) continue; /* possible, if GBOX is approximated */
+    // lwnotice("Measure %g on segment %d of line 1: %g, %g, %g", t1, seg, p1.x, p1.y, p1.z);
+
+    seg = ptarray_locate_along_linear(l2->points, t0, &q0, 0);
+    if ( -1 == seg ) continue; /* possible, if GBOX is approximated */
+    // lwnotice("Measure %g on segment %d of line 2: %g, %g, %g", t0, seg, q0.x, q0.y, q0.z);
+
+    seg = ptarray_locate_along_linear(l2->points, t1, &q1, seg);
+    if ( -1 == seg ) continue; /* possible, if GBOX is approximated */
+    // lwnotice("Measure %g on segment %d of line 2: %g, %g, %g", t1, seg, q1.x, q1.y, q1.z);
+
+    segments_tcpa(&p0, &p1, &q0, &q1, t0, t1);
+
+               /*
+    lwnotice("Closest points: %g,%g,%g and %g,%g,%g at time %g",
+      p0.x, p0.y, p0.z,
+      q0.x, q0.y, q0.z, t);
+               */
+
+    dist2 = ( q0.x - p0.x ) * ( q0.x - p0.x ) +
+           ( q0.y - p0.y ) * ( q0.y - p0.y ) +
+           ( q0.z - p0.z ) * ( q0.z - p0.z );
+    if ( dist2 <= maxdist2 )
+    {
+      LWDEBUGF(1, "Within distance %g at time %g, breaking", *mindist, mintime);
+      within = LW_TRUE;
+      break;
+    }
+  }
+
+  /*
+   * Release memory
+   */
+
+  lwfree(mvals);
+
+  return within;
+}