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>
 *
 **********************************************************************/

#include <float.h>

#include "liblwgeom_internal.h"
#include "lwgeom_log.h"

static void
calc_distances_3d(const POINT3D* curr, const POINT3D* points, uint32_t npoints, double* distances)
{
        uint32_t i;
        for (i = 0; i < npoints; i++)
        {
                distances[i] = distance3d_pt_pt(curr, &points[i]);
        }
}

static double
iterate_3d(POINT3D* curr, const POINT3D* points, uint32_t npoints, double* distances)
{
        uint32_t i;
        POINT3D next = { 0, 0, 0 };
        double delta;
        double denom = 0;
        char hit = LW_FALSE;

        calc_distances_3d(curr, points, npoints, distances);

        for (i = 0; i < npoints; i++)
        {
                if (distances[i] == 0)
                        hit = LW_TRUE;
                else
                        denom += 1.0 / distances[i];
        }

        for (i = 0; i < npoints; i++)
        {
                if (distances[i] > 0)
                {
                        next.x += (points[i].x / distances[i]) / denom;
                        next.y += (points[i].y / distances[i]) / denom;
                        next.z += (points[i].z / distances[i]) / 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;
                double dy = 0;
                double dz = 0;
                double d_sqr;
                double r_inv;

                for (i = 0; i < npoints; i++)
                {
                        if (distances[i] > 0)
                        {
                                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);
                r_inv = d_sqr > DBL_EPSILON ? 1.0 / d_sqr : 1.0;

                next.x = FP_MAX(0, 1.0 - r_inv)*next.x + FP_MIN(1.0, r_inv)*curr->x;
                next.y = FP_MAX(0, 1.0 - r_inv)*next.y + FP_MIN(1.0, r_inv)*curr->y;
                next.z = FP_MAX(0, 1.0 - r_inv)*next.z + FP_MIN(1.0, r_inv)*curr->z;
        }

        delta = distance3d_pt_pt(curr, &next);

        curr->x = next.x;
        curr->y = next.y;
        curr->z = next.z;

        return delta;
}

static POINT3D
init_guess(const POINT3D* points, uint32_t npoints)
{
        POINT3D guess = { 0, 0, 0 };
        uint32_t i;
        for (i = 0; i < npoints; i++)
        {
                guess.x += points[i].x / npoints;
                guess.y += points[i].y / npoints;
                guess.z += points[i].z / npoints;
        }

        return guess;
}

static POINT3D*
lwmpoint_extract_points_3d(const LWMPOINT* g, uint32_t* ngeoms)
{
        uint32_t i;
        uint32_t n = 0;
        int is_3d = lwgeom_has_z((LWGEOM*) g);

        POINT3D* points = lwalloc(g->ngeoms * sizeof(POINT3D));
        for (i = 0; i < g->ngeoms; i++)
        {
                LWGEOM* subg = lwcollection_getsubgeom((LWCOLLECTION*) g, i);
                if (!lwgeom_is_empty(subg))
                {
                        getPoint3dz_p(((LWPOINT*) subg)->point, 0, (POINT3DZ*) &points[n++]);
                        if (!is_3d)
                                points[n-1].z = 0.0; /* in case the getPoint functions return NaN in the future for 2d */
                }
        }

        if (ngeoms != NULL)
                *ngeoms = n;

        return points;
}

LWPOINT*
lwmpoint_median(const LWMPOINT* g, double tol, uint32_t max_iter, char fail_if_not_converged)
{
        uint32_t npoints; /* we need to count this ourselves so we can exclude empties */
        uint32_t i;
        double delta = DBL_MAX;
        double* distances;
        POINT3D* points = lwmpoint_extract_points_3d(g, &npoints);
        POINT3D median;

        if (npoints == 0)
        {
                lwfree(points);
                return lwpoint_construct_empty(g->srid, 0, 0);
        }

        median = init_guess(points, npoints);

        distances = lwalloc(npoints * sizeof(double));

        for (i = 0; i < max_iter && delta > tol; i++)
        {
                delta = iterate_3d(&median, points, npoints, distances);
        }

        lwfree(points);
        lwfree(distances);

        if (fail_if_not_converged && delta > tol)
        {
                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( lwgeom_get_type(g) )
        {
                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("Unsupported geometry type in lwgeom_median");
                        return NULL;
        }
}