lwgeom_median.c

Go to the documentation of this file.

/**********************************************************************
 *
 * PostGIS - Spatial Types for PostgreSQL
 * http://postgis.net
 *
 * PostGIS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 2 of the License, or
 * (at your option) any later version.
 *
 * PostGIS is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with PostGIS.  If not, see <http://www.gnu.org/licenses/>.
 *
 **********************************************************************
 *
 * Copyright 2015 Daniel Baston <dbaston@gmail.com>
 * Copyright 2017 Darafei Praliaskouski <me@komzpa.net>
 *
 **********************************************************************/
 
#include <assert.h>
#include <float.h>
 
#include "liblwgeom_internal.h"
#include "lwgeom_log.h"
 
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++)
        {
                double dist = distance3d_pt_pt(curr, (POINT3D*)&points[i]);
                distances[i] =  dist / points[i].m;
                weight += dist * points[i].m;
        }
 
        return weight;
}
 
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, iter;
        double delta;
        double sum_curr = 0, sum_next = 0;
        int hit = LW_FALSE;
        double *distances = lwalloc(npoints * sizeof(double));
 
        sum_curr = calc_weighted_distances_3d(curr, points, npoints, distances);
 
        for (iter = 0; iter < max_iter; iter++)
        {
                POINT3D next = { 0, 0, 0 };
                double denom = 0;
 
                for (i = 0; i < npoints; 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;
                        }
                }
 
                if (denom < DBL_EPSILON)
                {
                        /* No movement - Final point */
                        break;
                }
 
                /* Calculate the new point */
                next.x /= denom;
                next.y /= denom;
                next.z /= denom;
 
                /* If any of the intermediate points in the calculation is found in the
                * set of input points, the standard Weiszfeld method gets stuck with a
                * divide-by-zero.
                *
                * To get ourselves out of the hole, we follow an alternate procedure to
                * get the next iteration, as described in:
                *
                * Vardi, Y. and Zhang, C. (2011) "A modified Weiszfeld algorithm for the
                * Fermat-Weber location problem."  Math. Program., Ser. A 90: 559-566.
                * DOI 10.1007/s101070100222
                *
                * Available online at the time of this writing at
                * http://www.stat.rutgers.edu/home/cunhui/papers/43.pdf
                */
                if (hit)
                {
                        double dx = 0, dy = 0, dz = 0;
                        double d_sqr;
                        hit = LW_FALSE;
 
                        for (i = 0; i < npoints; i++)
                        {
                                if (distances[i] > DBL_EPSILON)
                                {
                                        dx += (points[i].x - curr->x) / distances[i];
                                        dy += (points[i].y - curr->y) / distances[i];
                                        dz += (points[i].z - curr->z) / distances[i];
                                }
                        }
 
                        d_sqr = sqrt(dx*dx + dy*dy + dz*dz);
                        if (d_sqr > DBL_EPSILON)
                        {
                                double r_inv = FP_MAX(0, 1.0 / d_sqr);
                                next.x = (1.0 - r_inv)*next.x + r_inv*curr->x;
                                next.y = (1.0 - r_inv)*next.y + r_inv*curr->y;
                                next.z = (1.0 - r_inv)*next.z + r_inv*curr->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;
                }
        }
 
        lwfree(distances);
        return iter;
}
 
static POINT3D
init_guess(const POINT4D* points, uint32_t npoints)
{
        assert(npoints > 0);
        POINT3D guess = { 0, 0, 0 };
        double mass = 0;
        uint32_t i;
        for (i = 0; i < npoints; i++)
        {
                guess.x += points[i].x * points[i].m;
                guess.y += points[i].y * points[i].m;
                guess.z += points[i].z * points[i].m;
                mass += points[i].m;
        }
        guess.x /= mass;
        guess.y /= mass;
        guess.z /= mass;
        return guess;
}
 
POINT4D*
lwmpoint_extract_points_4d(const LWMPOINT* g, uint32_t* npoints, int* input_empty)
{
        uint32_t n = 0;
        POINT4D* points = lwalloc(g->ngeoms * sizeof(POINT4D));
        int has_m = lwgeom_has_m((LWGEOM*) g);
 
        for (uint32_t i = 0; i < g->ngeoms; i++)
        {
                const LWGEOM* subg = lwcollection_getsubgeom((LWCOLLECTION*) g, i);
                if (!lwgeom_is_empty(subg))
                {
                        *input_empty = LW_FALSE;
                        if (!getPoint4d_p(((LWPOINT*) subg)->point, 0, &points[n]))
                        {
                                lwerror("Geometric median: getPoint4d_p reported failure on point (POINT(%g %g %g %g), number %d of %d in input).", points[n].x, points[n].y, points[n].z, points[n].m, i, g->ngeoms);
                                lwfree(points);
                                return NULL;
                        }
                        if (has_m)
                        {
                                /* This limitation on non-negativity can be lifted
                                 * by replacing Weiszfeld algorithm with different one.
                                 * Possible option described in:
                                 *
                                 * Drezner, Zvi & O. Wesolowsky, George. (1991).
                                 * The Weber Problem On The Plane With Some Negative Weights.
                                 * INFOR. Information Systems and Operational Research.
                                 * 29. 10.1080/03155986.1991.11732158.
                                 */
                                if (points[n].m < 0)
                                {
                                        lwerror("Geometric median input contains points with negative weights (POINT(%g %g %g %g), number %d of %d in input). Implementation can't guarantee global minimum convergence.", points[n].x, points[n].y, points[n].z, points[n].m, i, g->ngeoms);
                                        lwfree(points);
                                        return NULL;
                                }
 
                                /* points with zero weight are not going to affect calculation, drop them early */
                                if (points[n].m > DBL_EPSILON) n++;
                        }
                        else
                        {
                                points[n].m = 1.0;
                                n++;
                        }
                }
        }
 
#ifndef NDEBUG
        /* check Z=0 for 2D inputs */
        if (!lwgeom_has_z((LWGEOM*) g))
                for (uint32_t i = 0; i < n; i++)
                        assert(points[i].z == 0);
#endif
 
        *npoints = n;
        return points;
}
 
 
LWPOINT*
lwmpoint_median(const LWMPOINT* g, double tol, uint32_t max_iter, char fail_if_not_converged)
{
        /* m ordinate is considered weight, if defined */
        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;
        POINT3D median;
        POINT4D* points = lwmpoint_extract_points_4d(g, &npoints, &input_empty);
 
        /* input validation failed, error reported already */
        if (points == NULL) return NULL;
 
        if (npoints == 0)
        {
                lwfree(points);
                if (input_empty)
                {
                        return lwpoint_construct_empty(g->srid, 0, 0);
                }
                else
                {
                        lwerror("Median failed to find non-empty input points with positive weight.");
                        return NULL;
                }
        }
 
        median = init_guess(points, npoints);
 
        i = iterate_4d(&median, points, npoints, max_iter, tol);
 
        lwfree(points);
 
        if (fail_if_not_converged && i >= max_iter)
        {
                lwerror("Median failed to converge within %g after %d iterations.", tol, max_iter);
                return NULL;
        }
 
        if (lwgeom_has_z((LWGEOM*) g))
        {
                return lwpoint_make3dz(g->srid, median.x, median.y, median.z);
        }
        else
        {
                return lwpoint_make2d(g->srid, median.x, median.y);
        }
}
 
LWPOINT*
lwgeom_median(const LWGEOM* g, double tol, uint32_t max_iter, char fail_if_not_converged)
{
        switch (g->type)
        {
                case POINTTYPE:
                        return lwpoint_clone(lwgeom_as_lwpoint(g));
                case MULTIPOINTTYPE:
                        return lwmpoint_median(lwgeom_as_lwmpoint(g), tol, max_iter, fail_if_not_converged);
                default:
                        lwerror("%s: Unsupported geometry type: %s", __func__, lwtype_name(g->type));
                        return NULL;
        }
}