diff --git a/liblwgeom/cunit/cu_algorithm.c b/liblwgeom/cunit/cu_algorithm.c index 47783177b2d..bd9b5dae762 100644 --- a/liblwgeom/cunit/cu_algorithm.c +++ b/liblwgeom/cunit/cu_algorithm.c @@ -1115,6 +1115,19 @@ static void test_median_handles_3d_correctly(void) do_median_dims_check("MULTIPOINT ZM ((1 3 4 5), (4 7 8 6), (2 9 1 7), (0 4 4 8), (2 2 3 9))", 3); } +static double +test_weighted_distance(const POINT4D* curr, const POINT4D* points, uint32_t npoints) +{ + double distance = 0.0; + uint32_t i; + for (i = 0; i < npoints; i++) + { + distance += distance3d_pt_pt((POINT3D*)curr, (POINT3D*)&points[i]) * points[i].m; + } + + return distance; +} + static void do_median_test(char* input, char* expected, int fail_if_not_converged, int iter_count) { cu_error_msg_reset(); @@ -1142,15 +1155,43 @@ static void do_median_test(char* input, char* expected, int fail_if_not_converge passed = LW_TRUE; passed = passed && lwgeom_is_empty((LWGEOM*) expected_result) == lwgeom_is_empty((LWGEOM*) result); passed = passed && (lwgeom_has_z((LWGEOM*) expected_result) == lwgeom_has_z((LWGEOM*) result)); - if (!lwgeom_is_empty((LWGEOM*) result)) + + if (passed && !lwgeom_is_empty((LWGEOM*) result)) { - passed = passed && fabs(actual_pt.x - expected_pt.x) < tolerance; - passed = passed && fabs(actual_pt.y - expected_pt.y) < tolerance; - passed = passed && (!lwgeom_has_z((LWGEOM*) expected_result) || fabs(actual_pt.z - expected_pt.z) < tolerance); - passed = passed && (!lwgeom_has_m((LWGEOM*) expected_result) || fabs(actual_pt.m - expected_pt.m) < tolerance); + if (g->type == POINTTYPE) + { + passed &= fabs(actual_pt.x - expected_pt.x) < tolerance; + passed &= fabs(actual_pt.y - expected_pt.y) < tolerance; + passed &= (!lwgeom_has_z((LWGEOM*) expected_result) || fabs(actual_pt.z - expected_pt.z) < tolerance); + passed &= (!lwgeom_has_m((LWGEOM*) expected_result) || fabs(actual_pt.m - expected_pt.m) < tolerance); + } + else + { + /* Check that the difference between the obtained geometric + median and the expected point is within tolerance */ + uint32_t npoints = 1; + int input_empty = LW_TRUE; + POINT4D* points = lwmpoint_extract_points_4d(lwgeom_as_lwmpoint(g), &npoints, &input_empty); + double distance_expected = test_weighted_distance(&expected_pt, points, npoints); + double distance_result = test_weighted_distance(&actual_pt, points, npoints); + + passed = distance_result <= (1.0 + tolerance) * distance_expected; + if (!passed) + { + printf("Diff: Got %.10f Expected %.10f\n", distance_result, distance_expected); + } + lwfree(points); + } } + if (!passed) - printf("median_test input %s (parsed %s) expected %s got %s\n", input, lwgeom_to_ewkt(g), lwgeom_to_ewkt((LWGEOM*) expected_result), lwgeom_to_ewkt((LWGEOM*) result)); + { + printf("median_test input %s (parsed %s) expected %s got %s\n", + input, lwgeom_to_ewkt(g), + lwgeom_to_ewkt((LWGEOM*) expected_result), + lwgeom_to_ewkt((LWGEOM*) result)); + } + } else if (result == NULL && expected == NULL) /* got nothing, expecting nothing */ { @@ -1231,18 +1272,46 @@ static void test_median_robustness(void) do_median_test("POLYGON((1 0,0 1,1 2,2 1,1 0))", NULL, LW_TRUE, 1000); do_median_test("POLYGON((1 0,0 1,1 2,2 1,1 0))", NULL, LW_FALSE, 1000); - /* Intermediate point included in the set */ + /* Median point is included */ + do_median_test("MULTIPOINT ZM (" + "(1480 0 200 100)," + "(620 0 200 100)," + "(1000 0 -200 100)," + "(1000 0 -590 100)," + "(1025 0 65 100)," + "(1025 0 -65 100)" + ")", + "POINT (1025 0 -65)", LW_TRUE, 10000); + +#if 0 + /* Leads to invalid result (0 0 0) with 80bit (fmulp + faddp) precision. ok with 64 bit float ops */ + do_median_test("MULTIPOINT ZM (" + "(0 0 20000 0.5)," + "(0 0 59000 0.5)," + "(0 -3000 -3472.22222222222262644208967685699462890625 1)," + "(0 3000 3472.22222222222262644208967685699462890625 1)," + "(0 0 -1644.736842105263121993630193173885345458984375 1)," + "(0 0 1644.736842105263121993630193173885345458984375 1)," + "(0 48000 -20000 1.3)," + "(0 -48000 -20000 1.3)" + ")", + "POINT (0 0 0)", LW_TRUE, 10000); +#endif + +#if 0 + /* Leads to invalid result (0 0 0) with 64bit (vfmadd231sd) precision. Ok with 80 bit float ops */ do_median_test("MULTIPOINT ZM (" - "(0 0 20000 0.5)," - "(0 0 59000 0.5)," - "(0 48000 -20000 1.3)," - "(0 -48000 -20000 1.3)," - "(0 -3000 -3472.22222222222262644208967685699462890625 1)," - "(0 3000 3472.22222222222262644208967685699462890625 1)," - "(0 0 -1644.736842105263121993630193173885345458984375 1)," - "(0 0 1644.736842105263121993630193173885345458984375 1)" - ")", - "POINT (0 0 0)", LW_TRUE, 296); + "(0 0 20000 0.5)," + "(0 0 59000 0.5)," + "(0 -3000 -3472.22222222222262644208967685699462890625 1)," + "(0 3000 3472.22222222222262644208967685699462890625 1)," + "(0 -0.00000000000028047739569477638384522295466033823196 -1644.736842105263121993630193173885345458984375 1)," + "(0 0.00000000000028047739569477638384522295466033823196 1644.736842105263121993630193173885345458984375 1)," + "(0 48000 -20000 1.3)," + "(0 -48000 -20000 1.3)" + ")", + "POINT (0 0 0)", LW_TRUE, 10000); +#endif } static void test_point_density(void) diff --git a/liblwgeom/liblwgeom_internal.h b/liblwgeom/liblwgeom_internal.h index e5184d3c787..e09b4f5a99f 100644 --- a/liblwgeom/liblwgeom_internal.h +++ b/liblwgeom/liblwgeom_internal.h @@ -500,5 +500,6 @@ extern int _lwgeom_interrupt_requested; int ptarray_npoints_in_rect(const POINTARRAY *pa, const GBOX *gbox); int gbox_contains_point2d(const GBOX *g, const POINT2D *p); int lwpoly_contains_point(const LWPOLY *poly, const POINT2D *pt); +POINT4D* lwmpoint_extract_points_4d(const LWMPOINT* g, uint32_t* npoints, int* input_empty); #endif /* _LIBLWGEOM_INTERNAL_H */ diff --git a/liblwgeom/lwgeom_median.c b/liblwgeom/lwgeom_median.c index 6aeff3de329..03d37b12c88 100644 --- a/liblwgeom/lwgeom_median.c +++ b/liblwgeom/lwgeom_median.c @@ -29,48 +29,61 @@ #include "liblwgeom_internal.h" #include "lwgeom_log.h" -static void +static double calc_weighted_distances_3d(const POINT3D* curr, const POINT4D* points, uint32_t npoints, double* distances) { uint32_t i; + double weight = 0.0; for (i = 0; i < npoints; i++) { - distances[i] = distance3d_pt_pt(curr, (POINT3D*)&points[i]) / points[i].m; + double dist = distance3d_pt_pt(curr, (POINT3D*)&points[i]); + distances[i] = dist / points[i].m; + weight += dist * points[i].m; } + + return weight; } -static double -iterate_4d(POINT3D* curr, const POINT4D* points, uint32_t npoints, double* distances) +static uint32_t +iterate_4d(POINT3D* curr, const POINT4D* points, const uint32_t npoints, const uint32_t max_iter, const double tol) { - uint32_t i; - POINT3D next = { 0, 0, 0 }; - double delta = 0; - double denom = 0; - char hit = LW_FALSE; + uint32_t i, iter; + double delta; + double sum_curr = 0, sum_next = 0; + int hit = LW_FALSE; + double *distances = lwalloc(npoints * sizeof(double)); - calc_weighted_distances_3d(curr, points, npoints, distances); + sum_curr = calc_weighted_distances_3d(curr, points, npoints, distances); - for (i = 0; i < npoints; i++) + for (iter = 0; iter < max_iter; iter++) { - /* we need to use lower epsilon than in FP_IS_ZERO in the loop for calculation to converge */ - if (distances[i] > DBL_EPSILON) + POINT3D next = { 0, 0, 0 }; + double denom = 0; + + /** Calculate denom to get the next point */ + for (i = 0; i < npoints; i++) { - next.x += points[i].x / distances[i]; - next.y += points[i].y / distances[i]; - next.z += points[i].z / distances[i]; - denom += 1.0 / distances[i]; + /* we need to use lower epsilon than in FP_IS_ZERO in the loop for calculation to converge */ + if (distances[i] > DBL_EPSILON) + { + next.x += points[i].x / distances[i]; + next.y += points[i].y / distances[i]; + next.z += points[i].z / distances[i]; + denom += 1.0 / distances[i]; + } + else + { + hit = LW_TRUE; + } } - else + + if (denom < DBL_EPSILON) { - hit = LW_TRUE; + /* No movement - Final point */ + break; } - } - /* negative weight shouldn't get here */ - assert(denom >= 0); - /* denom is zero in case of multipoint of single point when we've converged perfectly */ - if (denom > DBL_EPSILON) - { + /* Calculate the new point */ next.x /= denom; next.y /= denom; next.z /= denom; @@ -91,10 +104,10 @@ iterate_4d(POINT3D* curr, const POINT4D* points, uint32_t npoints, double* dista */ if (hit) { - double dx = 0; - double dy = 0; - double dz = 0; + double dx = 0, dy = 0, dz = 0; double d_sqr; + hit = LW_FALSE; + for (i = 0; i < npoints; i++) { if (distances[i] > DBL_EPSILON) @@ -106,7 +119,6 @@ iterate_4d(POINT3D* curr, const POINT4D* points, uint32_t npoints, double* dista } d_sqr = sqrt(dx*dx + dy*dy + dz*dz); - /* Avoid division by zero if the intermediate point is the median */ if (d_sqr > DBL_EPSILON) { double r_inv = FP_MAX(0, 1.0 / d_sqr); @@ -116,14 +128,24 @@ iterate_4d(POINT3D* curr, const POINT4D* points, uint32_t npoints, double* dista } } - delta = distance3d_pt_pt(curr, &next); - - curr->x = next.x; - curr->y = next.y; - curr->z = next.z; + /* Check movement with next point */ + sum_next = calc_weighted_distances_3d(&next, points, npoints, distances); + delta = sum_curr - sum_next; + if (delta < tol) + { + break; + } + else + { + curr->x = next.x; + curr->y = next.y; + curr->z = next.z; + sum_curr = sum_next; + } } - return delta; + lwfree(distances); + return iter; } static POINT3D @@ -146,7 +168,7 @@ init_guess(const POINT4D* points, uint32_t npoints) return guess; } -static POINT4D* +POINT4D* lwmpoint_extract_points_4d(const LWMPOINT* g, uint32_t* npoints, int* input_empty) { uint32_t i; @@ -212,9 +234,7 @@ lwmpoint_median(const LWMPOINT* g, double tol, uint32_t max_iter, char fail_if_n uint32_t npoints = 0; /* we need to count this ourselves so we can exclude empties and weightless points */ uint32_t i; int input_empty = LW_TRUE; - double delta = DBL_MAX; POINT3D median; - double* distances = NULL; POINT4D* points = lwmpoint_extract_points_4d(g, &npoints, &input_empty); /* input validation failed, error reported already */ @@ -236,17 +256,11 @@ lwmpoint_median(const LWMPOINT* g, double tol, uint32_t max_iter, char fail_if_n median = init_guess(points, npoints); - distances = lwalloc(npoints * sizeof(double)); - - for (i = 0; i < max_iter && delta > tol; i++) - { - delta = iterate_4d(&median, points, npoints, distances); - } + i = iterate_4d(&median, points, npoints, max_iter, tol); - lwfree(distances); lwfree(points); - if (fail_if_not_converged && delta > tol) + if (fail_if_not_converged && i >= max_iter) { lwerror("Median failed to converge within %g after %d iterations.", tol, max_iter); return NULL;