From: Paul Ramsey Date: Wed, 30 Sep 2009 23:59:01 +0000 (+0000) Subject: Roughing in lwgeom distance machinery now. X-Git-Tag: 1.5.0b1~441 X-Git-Url: https://granicus.if.org/sourcecode?a=commitdiff_plain;h=a9848bdc11af72168f78ec19a893b53fffe0d30c;p=postgis Roughing in lwgeom distance machinery now. git-svn-id: http://svn.osgeo.org/postgis/trunk@4569 b70326c6-7e19-0410-871a-916f4a2858ee --- diff --git a/liblwgeom/libgeom.h b/liblwgeom/libgeom.h index 12d0f48ff..9fb7848ec 100644 --- a/liblwgeom/libgeom.h +++ b/liblwgeom/libgeom.h @@ -370,6 +370,12 @@ extern int lwgeom_calculate_gbox_geodetic(const LWGEOM *geom, GBOX *gbox); */ extern int lwgeom_calculate_gbox(const LWGEOM *lwgeom, GBOX *gbox); +/** +* Calculate the geodetic distance from lwgeom1 to lwgeom2 on the unit sphere. +* Pass in a tolerance in radians. +*/ +extern int lwgeom_distance_sphere(LWGEOM *lwgeom1, LWGEOM *lwgeom2, double tolerance, double *result); + /** * New function to read doubles directly from the double* coordinate array * of an aligned lwgeom #POINTARRAY (built by de-serializing a #GSERIALIZED). diff --git a/liblwgeom/liblwgeom.h b/liblwgeom/liblwgeom.h index b421cca99..801e8e746 100644 --- a/liblwgeom/liblwgeom.h +++ b/liblwgeom/liblwgeom.h @@ -1266,6 +1266,11 @@ extern int32 lwgeom_npoints(uchar *serialized); */ extern int lwgeom_is_empty(LWGEOM *geom); +/** +* Return the dimensionality (relating to point/line/poly) of an lwgeom +*/ +extern int lwgeom_dimensionality(LWGEOM *geom); + /* Is lwgeom1 geometrically equal to lwgeom2 ? */ char lwgeom_same(const LWGEOM *lwgeom1, const LWGEOM *lwgeom2); char ptarray_same(const POINTARRAY *pa1, const POINTARRAY *pa2); diff --git a/liblwgeom/lwgeodetic.c b/liblwgeom/lwgeodetic.c index 218adf879..5ca9dbf86 100644 --- a/liblwgeom/lwgeodetic.c +++ b/liblwgeom/lwgeodetic.c @@ -167,6 +167,12 @@ static inline int point_equal(GEOGRAPHIC_POINT g1, GEOGRAPHIC_POINT g2) return FP_EQUALS(g1.lat, g2.lat) && FP_EQUALS(g1.lon, g2.lon); } +void geographic_point_init(double lon, double lat, GEOGRAPHIC_POINT *g) +{ + g->lat = latitude_radians_normalize(deg2rad(lat)); + g->lon = longitude_radians_normalize(deg2rad(lon)); +} + /** * Check to see if this geocentric gbox is wrapped around a pole. * Only makes sense if this gbox originated from a polygon, as it's assuming @@ -279,11 +285,11 @@ static void inline vector_difference(POINT3D a, POINT3D b, POINT3D *n) /** * Scale a vector out by a factor */ -static void inline vector_scale(POINT3D a, double scale, POINT3D *n) +static void inline vector_scale(POINT3D *n, double scale) { - n->x = a.x * scale; - n->y = a.y * scale; - n->z = a.z * scale; + n->x *= scale; + n->y *= scale; + n->z *= scale; return; } @@ -433,16 +439,29 @@ int edge_contains_longitude(GEOGRAPHIC_EDGE e, GEOGRAPHIC_POINT p) */ double sphere_distance(GEOGRAPHIC_POINT s, GEOGRAPHIC_POINT e) { - double latS = s.lat; - double latE = e.lat; - double dlng = e.lon - s.lon; - double a1 = pow(cos(latE) * sin(dlng), 2.0); - double a2 = pow(cos(latS) * sin(latE) - sin(latS) * cos(latE) * cos(dlng), 2.0); + double d_lon = e.lon - s.lon; + double cos_d_lon = cos(d_lon); + double cos_lat_e = cos(e.lat); + double sin_lat_e = sin(e.lat); + double cos_lat_s = cos(s.lat); + double sin_lat_s = sin(s.lat); + + double a1 = pow(cos_lat_e * sin(d_lon), 2.0); + double a2 = pow(cos_lat_s * sin_lat_e - sin_lat_s * cos_lat_e * cos_d_lon, 2.0); double a = sqrt(a1 + a2); - double b = sin(latS) * sin(latE) + cos(latS) * cos(latE) * cos(dlng); + double b = sin_lat_s * sin_lat_e + cos_lat_s * cos_lat_e * cos_d_lon; return atan2(a, b); } +/** +* Given two unit vectors, calculate their distance apart in radians. +*/ +double sphere_distance_cartesian(POINT3D s, POINT3D e) +{ + return acos(dot_product(s, e)); +} + + /** * Given two points on a unit sphere, calculate the direction from s to e. */ @@ -615,7 +634,7 @@ int edge_intersection(GEOGRAPHIC_EDGE e1, GEOGRAPHIC_EDGE e2, GEOGRAPHIC_POINT * double edge_distance_to_point(GEOGRAPHIC_EDGE e, GEOGRAPHIC_POINT gp, GEOGRAPHIC_POINT *closest) { double d1 = 1000000000.0, d2, d3, d_nearest; - POINT3D n, p, k, v1; + POINT3D n, p, k; GEOGRAPHIC_POINT gk, g_nearest; /* Zero length edge, */ @@ -625,8 +644,8 @@ double edge_distance_to_point(GEOGRAPHIC_EDGE e, GEOGRAPHIC_POINT gp, GEOGRAPHIC robust_cross_product(e.start, e.end, &n); normalize(&n); geog2cart(gp, &p); - vector_scale(n, dot_product(p, n), &v1); - vector_difference(p, v1, &k); + vector_scale(&n, dot_product(p, n)); + vector_difference(p, n, &k); normalize(&k); cart2geog(k, &gk); if( edge_contains_point(e, gk) ) @@ -655,6 +674,46 @@ double edge_distance_to_point(GEOGRAPHIC_EDGE e, GEOGRAPHIC_POINT gp, GEOGRAPHIC return d_nearest; } +double edge_distance_to_edge(GEOGRAPHIC_EDGE e1, GEOGRAPHIC_EDGE e2, GEOGRAPHIC_POINT *closest1, GEOGRAPHIC_POINT *closest2) +{ + double d; + GEOGRAPHIC_POINT gcp1s, gcp1e, gcp2s, gcp2e, c1, c2; + double d1s = edge_distance_to_point(e1, e2.start, &gcp1s); + double d1e = edge_distance_to_point(e1, e2.end, &gcp1e); + double d2s = edge_distance_to_point(e2, e1.start, &gcp2s); + double d2e = edge_distance_to_point(e2, e1.end, &gcp2e); + + d = d1s; + c1 = gcp1s; + c2 = e2.start; + + if( d1e < d ) + { + d = d1e; + c1 = gcp1e; + c2 = e2.end; + } + + if( d2s < d ) + { + d = d2s; + c1 = e1.start; + c2 = gcp2s; + } + + if( d2e < d ) + { + d = d2e; + c1 = e1.end; + c2 = gcp2e; + } + + if( closest1 ) *closest1 = c1; + if( closest2 ) *closest2 = c2; + + return d; +} + /** * Given a starting location r, a distance and an azimuth * to the new point, compute the location of the projected point on the unit sphere. @@ -1018,6 +1077,100 @@ int edge_calculate_gbox(GEOGRAPHIC_EDGE e, GBOX *gbox) return G_SUCCESS; } +static int lw_line_point_distance_sphere(LWLINE *lwline, LWPOINT *lwpt, double tolerance, double *result) +{ + GEOGRAPHIC_POINT g; + GEOGRAPHIC_EDGE e; + POINT2D p; + int i; + + /* Initialize our point */ + getPoint2d_p(lwpt->point, 0, &p); + geographic_point_init(p.x, p.y, &g); + + /* Degenerate one-point line! We handle gracefully. */ + if ( lwline->points->npoints == 1 ) + { + GEOGRAPHIC_POINT f; + getPoint2d_p(lwline->points, 0, &p); + geographic_point_init(p.x, p.y, &f); + *result = sphere_distance(g, f); + return LW_TRUE; + } + + /* Iterate through the edges in our line */ + for( i = 1; i < lwline->points->npoints; i++ ) + { + getPoint2d_p(lwline->points, i - 1, &p); + geographic_point_init(p.x, p.y, &(e.start)); + getPoint2d_p(lwline->points, i, &p); + geographic_point_init(p.x, p.y, &(e.end)); + *result = edge_distance_to_point(e, g, 0); + if( *result < tolerance ) + break; + } + return LW_TRUE; +} + +static int lw_point_point_distance_sphere(LWPOINT *lwpt1, LWPOINT *lwpt2, double *result) +{ + GEOGRAPHIC_POINT g1, g2; + POINT2D p; + getPoint2d_p(lwpt1->point, 0, &p); + geographic_point_init(p.x, p.y, &g1); + getPoint2d_p(lwpt2->point, 0, &p); + geographic_point_init(p.x, p.y, &g2); + *result = sphere_distance(g1, g2); + return LW_TRUE; +} + +/** +* Calculate the distance between two LWGEOMs, using the coordinates are +* longitude and latitude. Return immediately when the calulated distance drops +* below the tolerance (useful for dwithin calculations). +*/ +int lwgeom_distance_sphere(LWGEOM *lwgeom1, LWGEOM *lwgeom2, double tolerance, double *result) +{ + int type1, type2; + + assert(lwgeom1); + assert(lwgeom2); + + if( lwgeom_is_empty(lwgeom1) || lwgeom_is_empty(lwgeom2) ) + { + *result = -1.0; + return LW_FALSE; + } + + /* Ensure that for LINE/POINT, POLY/POINT, POLY/LINE cases + the lower dimensioned object is always type2 */ + if ( lwgeom_dimensionality(lwgeom1) < lwgeom_dimensionality(lwgeom2) ) + { + LWGEOM *tmp = lwgeom2; + lwgeom2 = lwgeom1; + lwgeom1 = tmp; + } + + type1 = TYPE_GETTYPE(lwgeom1->type); + type2 = TYPE_GETTYPE(lwgeom2->type); + + if( type1 == POINTTYPE && type2 == POINTTYPE ) + { + return lw_point_point_distance_sphere( + lwgeom_as_lwpoint(lwgeom1), + lwgeom_as_lwpoint(lwgeom2), result); + } + if( type1 == LINETYPE && type2 == POINTTYPE ) + { + return lw_line_point_distance_sphere( + lwgeom_as_lwline(lwgeom1), + lwgeom_as_lwpoint(lwgeom2), + tolerance, result); + + } + return LW_FALSE; + +} /** * This function can only be used on LWGEOM that is built on top of diff --git a/liblwgeom/lwgeodetic.h b/liblwgeom/lwgeodetic.h index af5a29403..2a72dc5c9 100644 --- a/liblwgeom/lwgeodetic.h +++ b/liblwgeom/lwgeodetic.h @@ -20,7 +20,7 @@ extern int gbox_geocentric_slow; typedef struct { double lon; - double lat; + double lat; } GEOGRAPHIC_POINT; /** @@ -59,13 +59,16 @@ double z_to_latitude(double z, int top); int clairaut_cartesian(POINT3D start, POINT3D end, GEOGRAPHIC_POINT *g_top, GEOGRAPHIC_POINT *g_bottom); int clairaut_geographic(GEOGRAPHIC_POINT start, GEOGRAPHIC_POINT end, GEOGRAPHIC_POINT *g_top, GEOGRAPHIC_POINT *g_bottom); double sphere_distance(GEOGRAPHIC_POINT s, GEOGRAPHIC_POINT e); +double sphere_distance_cartesian(POINT3D s, POINT3D e); double sphere_direction(GEOGRAPHIC_POINT s, GEOGRAPHIC_POINT e); int sphere_project(GEOGRAPHIC_POINT r, double distance, double azimuth, GEOGRAPHIC_POINT *n); int edge_calculate_gbox(GEOGRAPHIC_EDGE e, GBOX *gbox); int edge_calculate_gbox_slow(GEOGRAPHIC_EDGE e, GBOX *gbox); int edge_intersection(GEOGRAPHIC_EDGE e1, GEOGRAPHIC_EDGE e2, GEOGRAPHIC_POINT *g); double edge_distance_to_point(GEOGRAPHIC_EDGE e, GEOGRAPHIC_POINT gp, GEOGRAPHIC_POINT *closest); +double edge_distance_to_edge(GEOGRAPHIC_EDGE e1, GEOGRAPHIC_EDGE e2, GEOGRAPHIC_POINT *closest1, GEOGRAPHIC_POINT *closest2); void edge_deg2rad(GEOGRAPHIC_EDGE *e); void edge_rad2deg(GEOGRAPHIC_EDGE *e); void point_deg2rad(GEOGRAPHIC_POINT *p); void point_rad2deg(GEOGRAPHIC_POINT *p); +void geographic_point_init(double lon, double lat, GEOGRAPHIC_POINT *g); diff --git a/liblwgeom/lwgeom.c b/liblwgeom/lwgeom.c index 879be127e..f5d7aed0c 100644 --- a/liblwgeom/lwgeom.c +++ b/liblwgeom/lwgeom.c @@ -1131,4 +1131,49 @@ int lwgeom_is_empty(LWGEOM *geom) break; } return result; -} \ No newline at end of file +} + +static int lwcollection_dimensionality(LWCOLLECTION *col) +{ + int i; + int dimensionality = 0; + for( i = 0; i < col->ngeoms; i++ ) + { + int d = lwgeom_dimensionality(col->geoms[i]); + if( d > dimensionality ) + dimensionality = d; + } + return dimensionality; +} + +extern int lwgeom_dimensionality(LWGEOM *geom) +{ + LWDEBUGF(4, "got type %d", TYPE_GETTYPE(geom->type)); + switch (TYPE_GETTYPE(geom->type)) + { + case POINTTYPE: + case MULTIPOINTTYPE: + return 0; + break; + case LINETYPE: + case CIRCSTRINGTYPE: + case MULTILINETYPE: + case COMPOUNDTYPE: + case MULTICURVETYPE: + return 1; + break; + case POLYGONTYPE: + case CURVEPOLYTYPE: + case MULTIPOLYGONTYPE: + case MULTISURFACETYPE: + return 2; + break; + case COLLECTIONTYPE: + return lwcollection_dimensionality((LWCOLLECTION *)geom); + break; + default: + lwerror("unsupported input geometry type: %d", TYPE_GETTYPE(geom->type)); + break; + } + return 0; +}