From eeb3f999080b084f1289609722f403d367425c4b Mon Sep 17 00:00:00 2001 From: Sandro Santilli Date: Tue, 23 Jun 2015 17:32:59 +0000 Subject: [PATCH] Add lwgeom_cpa_within function includes unit test git-svn-id: http://svn.osgeo.org/postgis/trunk@13693 b70326c6-7e19-0410-871a-916f4a2858ee --- liblwgeom/cunit/cu_measures.c | 12 ++- liblwgeom/liblwgeom.h.in | 7 ++ liblwgeom/lwlinearreferencing.c | 151 +++++++++++++++++++++++++++++++- 3 files changed, 165 insertions(+), 5 deletions(-) diff --git a/liblwgeom/cunit/cu_measures.c b/liblwgeom/cunit/cu_measures.c index f71b2be89..975d8332b 100644 --- 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 7c7641f63..1e381e4b2 100644 --- 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 27fa21749..a3dbe068d 100644 --- 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; ipoints, 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; +} -- 2.40.0