1 /**********************************************************************
2 *
3 * PostGIS - Spatial Types for PostgreSQL
4 * http://postgis.net
5 *
6 * PostGIS is free software: you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation, either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * PostGIS is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with PostGIS. If not, see <http://www.gnu.org/licenses/>.
18 *
19 **********************************************************************
20 *
21 * Copyright 2015 Daniel Baston <dbaston@gmail.com>
22 * Copyright 2017 Darafei Praliaskouski <me@komzpa.net>
23 *
24 **********************************************************************/
25
26 #include <assert.h>
27 #include <float.h>
28
29 #include "liblwgeom_internal.h"
30 #include "lwgeom_log.h"
31
32 static double
calc_weighted_distances_3d(const POINT3D * curr,const POINT4D * points,uint32_t npoints,double * distances)33 calc_weighted_distances_3d(const POINT3D* curr, const POINT4D* points, uint32_t npoints, double* distances)
34 {
35 uint32_t i;
36 double weight = 0.0;
37 for (i = 0; i < npoints; i++)
38 {
39 double dist = distance3d_pt_pt(curr, (POINT3D*)&points[i]);
40 distances[i] = dist / points[i].m;
41 weight += dist * points[i].m;
42 }
43
44 return weight;
45 }
46
47 static uint32_t
iterate_4d(POINT3D * curr,const POINT4D * points,const uint32_t npoints,const uint32_t max_iter,const double tol)48 iterate_4d(POINT3D* curr, const POINT4D* points, const uint32_t npoints, const uint32_t max_iter, const double tol)
49 {
50 uint32_t i, iter;
51 double delta;
52 double sum_curr = 0, sum_next = 0;
53 int hit = LW_FALSE;
54 double *distances = lwalloc(npoints * sizeof(double));
55
56 sum_curr = calc_weighted_distances_3d(curr, points, npoints, distances);
57
58 for (iter = 0; iter < max_iter; iter++)
59 {
60 POINT3D next = { 0, 0, 0 };
61 double denom = 0;
62
63 /** Calculate denom to get the next point */
64 for (i = 0; i < npoints; i++)
65 {
66 /* we need to use lower epsilon than in FP_IS_ZERO in the loop for calculation to converge */
67 if (distances[i] > DBL_EPSILON)
68 {
69 next.x += points[i].x / distances[i];
70 next.y += points[i].y / distances[i];
71 next.z += points[i].z / distances[i];
72 denom += 1.0 / distances[i];
73 }
74 else
75 {
76 hit = LW_TRUE;
77 }
78 }
79
80 if (denom < DBL_EPSILON)
81 {
82 /* No movement - Final point */
83 break;
84 }
85
86 /* Calculate the new point */
87 next.x /= denom;
88 next.y /= denom;
89 next.z /= denom;
90
91 /* If any of the intermediate points in the calculation is found in the
92 * set of input points, the standard Weiszfeld method gets stuck with a
93 * divide-by-zero.
94 *
95 * To get ourselves out of the hole, we follow an alternate procedure to
96 * get the next iteration, as described in:
97 *
98 * Vardi, Y. and Zhang, C. (2011) "A modified Weiszfeld algorithm for the
99 * Fermat-Weber location problem." Math. Program., Ser. A 90: 559-566.
100 * DOI 10.1007/s101070100222
101 *
102 * Available online at the time of this writing at
103 * http://www.stat.rutgers.edu/home/cunhui/papers/43.pdf
104 */
105 if (hit)
106 {
107 double dx = 0, dy = 0, dz = 0;
108 double d_sqr;
109 hit = LW_FALSE;
110
111 for (i = 0; i < npoints; i++)
112 {
113 if (distances[i] > DBL_EPSILON)
114 {
115 dx += (points[i].x - curr->x) / distances[i];
116 dy += (points[i].y - curr->y) / distances[i];
117 dz += (points[i].z - curr->z) / distances[i];
118 }
119 }
120
121 d_sqr = sqrt(dx*dx + dy*dy + dz*dz);
122 if (d_sqr > DBL_EPSILON)
123 {
124 double r_inv = FP_MAX(0, 1.0 / d_sqr);
125 next.x = (1.0 - r_inv)*next.x + r_inv*curr->x;
126 next.y = (1.0 - r_inv)*next.y + r_inv*curr->y;
127 next.z = (1.0 - r_inv)*next.z + r_inv*curr->z;
128 }
129 }
130
131 /* Check movement with next point */
132 sum_next = calc_weighted_distances_3d(&next, points, npoints, distances);
133 delta = sum_curr - sum_next;
134 if (delta < tol)
135 {
136 break;
137 }
138 else
139 {
140 curr->x = next.x;
141 curr->y = next.y;
142 curr->z = next.z;
143 sum_curr = sum_next;
144 }
145 }
146
147 lwfree(distances);
148 return iter;
149 }
150
151 static POINT3D
init_guess(const POINT4D * points,uint32_t npoints)152 init_guess(const POINT4D* points, uint32_t npoints)
153 {
154 assert(npoints > 0);
155 POINT3D guess = { 0, 0, 0 };
156 double mass = 0;
157 uint32_t i;
158 for (i = 0; i < npoints; i++)
159 {
160 guess.x += points[i].x * points[i].m;
161 guess.y += points[i].y * points[i].m;
162 guess.z += points[i].z * points[i].m;
163 mass += points[i].m;
164 }
165 guess.x /= mass;
166 guess.y /= mass;
167 guess.z /= mass;
168 return guess;
169 }
170
171 POINT4D*
lwmpoint_extract_points_4d(const LWMPOINT * g,uint32_t * npoints,int * input_empty)172 lwmpoint_extract_points_4d(const LWMPOINT* g, uint32_t* npoints, int* input_empty)
173 {
174 uint32_t i;
175 uint32_t n = 0;
176 POINT4D* points = lwalloc(g->ngeoms * sizeof(POINT4D));
177 int has_m = lwgeom_has_m((LWGEOM*) g);
178
179 for (i = 0; i < g->ngeoms; i++)
180 {
181 LWGEOM* subg = lwcollection_getsubgeom((LWCOLLECTION*) g, i);
182 if (!lwgeom_is_empty(subg))
183 {
184 *input_empty = LW_FALSE;
185 if (!getPoint4d_p(((LWPOINT*) subg)->point, 0, &points[n]))
186 {
187 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);
188 lwfree(points);
189 return NULL;
190 }
191 if (has_m)
192 {
193 /* This limitation on non-negativity can be lifted
194 * by replacing Weiszfeld algorithm with different one.
195 * Possible option described in:
196 *
197 * Drezner, Zvi & O. Wesolowsky, George. (1991).
198 * The Weber Problem On The Plane With Some Negative Weights.
199 * INFOR. Information Systems and Operational Research.
200 * 29. 10.1080/03155986.1991.11732158.
201 */
202 if (points[n].m < 0)
203 {
204 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);
205 lwfree(points);
206 return NULL;
207 }
208
209 /* points with zero weight are not going to affect calculation, drop them early */
210 if (points[n].m > DBL_EPSILON) n++;
211 }
212 else
213 {
214 points[n].m = 1.0;
215 n++;
216 }
217 }
218 }
219
220 #if PARANOIA_LEVEL > 0
221 /* check Z=0 for 2D inputs*/
222 if (!lwgeom_has_z((LWGEOM*) g)) for (i = 0; i < n; i++) assert(points[i].z == 0);
223 #endif
224
225 *npoints = n;
226 return points;
227 }
228
229
230 LWPOINT*
lwmpoint_median(const LWMPOINT * g,double tol,uint32_t max_iter,char fail_if_not_converged)231 lwmpoint_median(const LWMPOINT* g, double tol, uint32_t max_iter, char fail_if_not_converged)
232 {
233 /* m ordinate is considered weight, if defined */
234 uint32_t npoints = 0; /* we need to count this ourselves so we can exclude empties and weightless points */
235 uint32_t i;
236 int input_empty = LW_TRUE;
237 POINT3D median;
238 POINT4D* points = lwmpoint_extract_points_4d(g, &npoints, &input_empty);
239
240 /* input validation failed, error reported already */
241 if (points == NULL) return NULL;
242
243 if (npoints == 0)
244 {
245 lwfree(points);
246 if (input_empty)
247 {
248 return lwpoint_construct_empty(g->srid, 0, 0);
249 }
250 else
251 {
252 lwerror("Median failed to find non-empty input points with positive weight.");
253 return NULL;
254 }
255 }
256
257 median = init_guess(points, npoints);
258
259 i = iterate_4d(&median, points, npoints, max_iter, tol);
260
261 lwfree(points);
262
263 if (fail_if_not_converged && i >= max_iter)
264 {
265 lwerror("Median failed to converge within %g after %d iterations.", tol, max_iter);
266 return NULL;
267 }
268
269 if (lwgeom_has_z((LWGEOM*) g))
270 {
271 return lwpoint_make3dz(g->srid, median.x, median.y, median.z);
272 }
273 else
274 {
275 return lwpoint_make2d(g->srid, median.x, median.y);
276 }
277 }
278
279 LWPOINT*
lwgeom_median(const LWGEOM * g,double tol,uint32_t max_iter,char fail_if_not_converged)280 lwgeom_median(const LWGEOM* g, double tol, uint32_t max_iter, char fail_if_not_converged)
281 {
282 switch (g->type)
283 {
284 case POINTTYPE:
285 return lwpoint_clone(lwgeom_as_lwpoint(g));
286 case MULTIPOINTTYPE:
287 return lwmpoint_median(lwgeom_as_lwmpoint(g), tol, max_iter, fail_if_not_converged);
288 default:
289 lwerror("%s: Unsupported geometry type: %s", __func__, lwtype_name(g->type));
290 return NULL;
291 }
292 }
293
294