/src/gdal/alg/thinplatespline.cpp
Line | Count | Source |
1 | | /****************************************************************************** |
2 | | * |
3 | | * Project: GDAL Warp API |
4 | | * Purpose: Implemenentation of 2D Thin Plate Spline transformer. |
5 | | * Author: VIZRT Development Team. |
6 | | * |
7 | | * This code was provided by Gilad Ronnen (gro at visrt dot com) with |
8 | | * permission to reuse under the following license. |
9 | | * |
10 | | ****************************************************************************** |
11 | | * Copyright (c) 2004, VIZRT Inc. |
12 | | * Copyright (c) 2008-2014, Even Rouault <even dot rouault at spatialys.com> |
13 | | * |
14 | | * SPDX-License-Identifier: MIT |
15 | | ****************************************************************************/ |
16 | | |
17 | | /*! @cond Doxygen_Suppress */ |
18 | | |
19 | | #include "cpl_port.h" |
20 | | #include "thinplatespline.h" |
21 | | #include "gdallinearsystem.h" |
22 | | |
23 | | #include <climits> |
24 | | #include <cstdio> |
25 | | #include <cstring> |
26 | | |
27 | | #include <algorithm> |
28 | | #include <limits> |
29 | | #include <new> // bad_alloc |
30 | | #include <utility> |
31 | | |
32 | | #include "cpl_error.h" |
33 | | #include "cpl_vsi.h" |
34 | | |
35 | | ////////////////////////////////////////////////////////////////////////////// |
36 | | //// vizGeorefSpline2D |
37 | | ////////////////////////////////////////////////////////////////////////////// |
38 | | |
39 | | // #define VIZ_GEOREF_SPLINE_DEBUG 0 |
40 | | |
41 | | bool VizGeorefSpline2D::grow_points() |
42 | | |
43 | 0 | { |
44 | 0 | const int new_max = _max_nof_points * 2 + 2 + 3; |
45 | |
|
46 | 0 | double *new_x = |
47 | 0 | static_cast<double *>(VSI_REALLOC_VERBOSE(x, sizeof(double) * new_max)); |
48 | 0 | if (!new_x) |
49 | 0 | return false; |
50 | 0 | x = new_x; |
51 | 0 | double *new_y = |
52 | 0 | static_cast<double *>(VSI_REALLOC_VERBOSE(y, sizeof(double) * new_max)); |
53 | 0 | if (!new_y) |
54 | 0 | return false; |
55 | 0 | y = new_y; |
56 | 0 | double *new_u = |
57 | 0 | static_cast<double *>(VSI_REALLOC_VERBOSE(u, sizeof(double) * new_max)); |
58 | 0 | if (!new_u) |
59 | 0 | return false; |
60 | 0 | u = new_u; |
61 | 0 | int *new_unused = |
62 | 0 | static_cast<int *>(VSI_REALLOC_VERBOSE(unused, sizeof(int) * new_max)); |
63 | 0 | if (!new_unused) |
64 | 0 | return false; |
65 | 0 | unused = new_unused; |
66 | 0 | int *new_index = |
67 | 0 | static_cast<int *>(VSI_REALLOC_VERBOSE(index, sizeof(int) * new_max)); |
68 | 0 | if (!new_index) |
69 | 0 | return false; |
70 | 0 | index = new_index; |
71 | 0 | for (int i = 0; i < _nof_vars; i++) |
72 | 0 | { |
73 | 0 | double *rhs_i_new = static_cast<double *>( |
74 | 0 | VSI_REALLOC_VERBOSE(rhs[i], sizeof(double) * new_max)); |
75 | 0 | if (!rhs_i_new) |
76 | 0 | return false; |
77 | 0 | rhs[i] = rhs_i_new; |
78 | 0 | double *coef_i_new = static_cast<double *>( |
79 | 0 | VSI_REALLOC_VERBOSE(coef[i], sizeof(double) * new_max)); |
80 | 0 | if (!coef_i_new) |
81 | 0 | return false; |
82 | 0 | coef[i] = coef_i_new; |
83 | 0 | if (_max_nof_points == 0) |
84 | 0 | { |
85 | 0 | memset(rhs[i], 0, 3 * sizeof(double)); |
86 | 0 | memset(coef[i], 0, 3 * sizeof(double)); |
87 | 0 | } |
88 | 0 | } |
89 | | |
90 | 0 | _max_nof_points = new_max - 3; |
91 | 0 | return true; |
92 | 0 | } |
93 | | |
94 | | bool VizGeorefSpline2D::add_point(const double Px, const double Py, |
95 | | const double *Pvars) |
96 | 0 | { |
97 | 0 | type = VIZ_GEOREF_SPLINE_POINT_WAS_ADDED; |
98 | 0 | int i; |
99 | |
|
100 | 0 | if (_nof_points == _max_nof_points) |
101 | 0 | { |
102 | 0 | if (!grow_points()) |
103 | 0 | return false; |
104 | 0 | } |
105 | | |
106 | 0 | i = _nof_points; |
107 | | // A new point is added. |
108 | 0 | x[i] = Px; |
109 | 0 | y[i] = Py; |
110 | 0 | for (int j = 0; j < _nof_vars; j++) |
111 | 0 | rhs[j][i + 3] = Pvars[j]; |
112 | 0 | _nof_points++; |
113 | 0 | return true; |
114 | 0 | } |
115 | | |
116 | | #if 0 |
117 | | bool VizGeorefSpline2D::change_point( int index, double Px, double Py, |
118 | | double* Pvars ) |
119 | | { |
120 | | if( index < _nof_points ) |
121 | | { |
122 | | int i = index; |
123 | | x[i] = Px; |
124 | | y[i] = Py; |
125 | | for( int j = 0; j < _nof_vars; j++ ) |
126 | | rhs[j][i+3] = Pvars[j]; |
127 | | } |
128 | | |
129 | | return true; |
130 | | } |
131 | | |
132 | | bool VizGeorefSpline2D::get_xy( int index, double& outX, double& outY ) |
133 | | { |
134 | | if( index < _nof_points ) |
135 | | { |
136 | | ok = true; |
137 | | outX = x[index]; |
138 | | outY = y[index]; |
139 | | return true; |
140 | | } |
141 | | |
142 | | outX = 0.0; |
143 | | outY = 0.0; |
144 | | |
145 | | return false; |
146 | | } |
147 | | |
148 | | int VizGeorefSpline2D::delete_point( const double Px, const double Py ) |
149 | | { |
150 | | for( int i = 0; i < _nof_points; i++ ) |
151 | | { |
152 | | if( ( fabs(Px - x[i]) <= _tx ) && ( fabs(Py - y[i]) <= _ty ) ) |
153 | | { |
154 | | for( int j = i; j < _nof_points - 1; j++ ) |
155 | | { |
156 | | x[j] = x[j+1]; |
157 | | y[j] = y[j+1]; |
158 | | for( int k = 0; k < _nof_vars; k++ ) |
159 | | rhs[k][j+3] = rhs[k][j+3+1]; |
160 | | } |
161 | | _nof_points--; |
162 | | type = VIZ_GEOREF_SPLINE_POINT_WAS_DELETED; |
163 | | return 1; |
164 | | } |
165 | | } |
166 | | return 0; |
167 | | } |
168 | | #endif |
169 | | |
170 | | template <typename T> static inline T SQ(const T &x) |
171 | 0 | { |
172 | 0 | return x * x; |
173 | 0 | } Unexecuted instantiation: thinplatespline.cpp:double SQ<double>(double const&) Unexecuted instantiation: thinplatespline.cpp:double __vector(2) SQ<double __vector(2)>(double __vector(2) const&) |
174 | | |
175 | | static inline double VizGeorefSpline2DBase_func(const double x1, |
176 | | const double y1, |
177 | | const double x2, |
178 | | const double y2) |
179 | 0 | { |
180 | 0 | const double dist = SQ(x2 - x1) + SQ(y2 - y1); |
181 | 0 | return dist != 0.0 ? dist * log(dist) : 0.0; |
182 | 0 | } |
183 | | |
184 | | #if defined(__GNUC__) && defined(__x86_64__) |
185 | | /* Some versions of ICC fail to compile VizGeorefSpline2DBase_func4 (#6350) */ |
186 | | #if defined(__INTEL_COMPILER) |
187 | | #if __INTEL_COMPILER >= 1500 |
188 | | #define USE_OPTIMIZED_VizGeorefSpline2DBase_func4 |
189 | | #else |
190 | | #if (__INTEL_COMPILER == 1200) || (__INTEL_COMPILER == 1210) |
191 | | #define USE_OPTIMIZED_VizGeorefSpline2DBase_func4 |
192 | | #else |
193 | | #undef USE_OPTIMIZED_VizGeorefSpline2DBase_func4 |
194 | | #endif |
195 | | #endif |
196 | | #else // defined(__INTEL_COMPILER) |
197 | | #define USE_OPTIMIZED_VizGeorefSpline2DBase_func4 |
198 | | #endif // defined(__INTEL_COMPILER) |
199 | | #endif |
200 | | |
201 | | #if defined(USE_OPTIMIZED_VizGeorefSpline2DBase_func4) && !defined(CPPCHECK) |
202 | | |
203 | | /* Derived and adapted from code originating from: */ |
204 | | |
205 | | /* @(#)e_log.c 1.3 95/01/18 */ |
206 | | /* |
207 | | * ==================================================== |
208 | | * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. |
209 | | * |
210 | | * Developed at SunSoft, a Sun Microsystems, Inc. business. |
211 | | * Permission to use, copy, modify, and distribute this |
212 | | * software is freely granted, provided that this notice |
213 | | * is preserved. |
214 | | * ==================================================== |
215 | | */ |
216 | | |
217 | | /* __ieee754_log(x) |
218 | | * Return the logarithm of x |
219 | | * |
220 | | * Method: |
221 | | * 1. Argument Reduction: find k and f such that |
222 | | * x = 2^k * (1+f), |
223 | | * where sqrt(2)/2 < 1+f < sqrt(2) . |
224 | | * |
225 | | * 2. Approximation of log(1+f). |
226 | | * Let s = f/(2+f) ; based on log(1+f) = log(1+s) - log(1-s) |
227 | | * = 2s + 2/3 s**3 + 2/5 s**5 + ....., |
228 | | * = 2s + s*R |
229 | | * We use a special Reme algorithm on [0,0.1716] to generate |
230 | | * a polynomial of degree 14 to approximate R The maximum error |
231 | | * of this polynomial approximation is bounded by 2**-58.45. In |
232 | | * other words, |
233 | | * 2 4 6 8 10 12 14 |
234 | | * R(z) ~ Lg1*s +Lg2*s +Lg3*s +Lg4*s +Lg5*s +Lg6*s +Lg7*s |
235 | | * (the values of Lg1 to Lg7 are listed in the program) |
236 | | * and |
237 | | * | 2 14 | -58.45 |
238 | | * | Lg1*s +...+Lg7*s - R(z) | <= 2 |
239 | | * | | |
240 | | * Note that 2s = f - s*f = f - hfsq + s*hfsq, where hfsq = f*f/2. |
241 | | * In order to guarantee error in log below 1ulp, we compute log |
242 | | * by |
243 | | * log(1+f) = f - s*(f - R) (if f is not too large) |
244 | | * log(1+f) = f - (hfsq - s*(hfsq+R)). (better accuracy) |
245 | | * |
246 | | * 3. Finally, log(x) = k*ln2 + log(1+f). |
247 | | * = k*ln2_hi+(f-(hfsq-(s*(hfsq+R)+k*ln2_lo))) |
248 | | * Here ln2 is split into two floating point number: |
249 | | * ln2_hi + ln2_lo, |
250 | | * where n*ln2_hi is always exact for |n| < 2000. |
251 | | * |
252 | | * Special cases: |
253 | | * log(x) is NaN with signal if x < 0 (including -INF) ; |
254 | | * log(+INF) is +INF; log(0) is -INF with signal; |
255 | | * log(NaN) is that NaN with no signal. |
256 | | * |
257 | | * Accuracy: |
258 | | * according to an error analysis, the error is always less than |
259 | | * 1 ulp (unit in the last place). |
260 | | * |
261 | | * Constants: |
262 | | * The hexadecimal values are the intended ones for the following |
263 | | * constants. The decimal values may be used, provided that the |
264 | | * compiler will convert from decimal to binary accurately enough |
265 | | * to produce the hexadecimal values shown. |
266 | | */ |
267 | | |
268 | | typedef double V2DF __attribute__((__vector_size__(16))); |
269 | | |
270 | | typedef union |
271 | | { |
272 | | V2DF v2; |
273 | | double d[2]; |
274 | | } v2dfunion; |
275 | | |
276 | | typedef union |
277 | | { |
278 | | int i[2]; |
279 | | long long li; |
280 | | } i64union; |
281 | | |
282 | | static const V2DF v2_ln2_div_2pow20 = {6.93147180559945286e-01 / 1048576, |
283 | | 6.93147180559945286e-01 / 1048576}; |
284 | | static const V2DF v2_Lg1 = {6.666666666666735130e-01, 6.666666666666735130e-01}; |
285 | | static const V2DF v2_Lg2 = {3.999999999940941908e-01, 3.999999999940941908e-01}; |
286 | | static const V2DF v2_Lg3 = {2.857142874366239149e-01, 2.857142874366239149e-01}; |
287 | | static const V2DF v2_Lg4 = {2.222219843214978396e-01, 2.222219843214978396e-01}; |
288 | | static const V2DF v2_Lg5 = {1.818357216161805012e-01, 1.818357216161805012e-01}; |
289 | | static const V2DF v2_Lg6 = {1.531383769920937332e-01, 1.531383769920937332e-01}; |
290 | | /*v2_Lg7 = {1.479819860511658591e-01, 1.479819860511658591e-01}, */ |
291 | | static const V2DF v2_one = {1.0, 1.0}; |
292 | | static const V2DF v2_const1023_mul_2pow20 = {1023.0 * 1048576, |
293 | | 1023.0 * 1048576}; |
294 | | |
295 | 0 | #define GET_HIGH_WORD(hx, x) memcpy(&hx, reinterpret_cast<char *>(&x) + 4, 4) |
296 | 0 | #define SET_HIGH_WORD(x, hx) memcpy(reinterpret_cast<char *>(&x) + 4, &hx, 4) |
297 | | |
298 | | #define MAKE_WIDE_CST(x) (((static_cast<long long>(x)) << 32) | (x)) |
299 | | constexpr long long cst_expmask = MAKE_WIDE_CST(0xfff00000); |
300 | | constexpr long long cst_0x95f64 = MAKE_WIDE_CST(0x00095f64); |
301 | | constexpr long long cst_0x100000 = MAKE_WIDE_CST(0x00100000); |
302 | | constexpr long long cst_0x3ff00000 = MAKE_WIDE_CST(0x3ff00000); |
303 | | |
304 | | // Modified version of __ieee754_log(), less precise than log() but a bit |
305 | | // faster, and computing 4 log() at a time. Assumes that the values are > 0. |
306 | | static void FastApproxLog4Val(v2dfunion *x) |
307 | 0 | { |
308 | 0 | i64union hx[2] = {}; |
309 | 0 | i64union k[2] = {}; |
310 | 0 | i64union i[2] = {}; |
311 | 0 | GET_HIGH_WORD(hx[0].i[0], x[0].d[0]); |
312 | 0 | GET_HIGH_WORD(hx[0].i[1], x[0].d[1]); |
313 | | |
314 | | // coverity[uninit_use] |
315 | 0 | k[0].li = hx[0].li & cst_expmask; |
316 | 0 | hx[0].li &= ~cst_expmask; |
317 | 0 | i[0].li = (hx[0].li + cst_0x95f64) & cst_0x100000; |
318 | 0 | hx[0].li |= i[0].li ^ cst_0x3ff00000; |
319 | 0 | SET_HIGH_WORD(x[0].d[0], hx[0].i[0]); // Normalize x or x/2. |
320 | 0 | SET_HIGH_WORD(x[0].d[1], hx[0].i[1]); // Normalize x or x/2. |
321 | 0 | k[0].li += i[0].li; |
322 | |
|
323 | 0 | v2dfunion dk[2] = {}; |
324 | 0 | dk[0].d[0] = static_cast<double>(k[0].i[0]); |
325 | 0 | dk[0].d[1] = static_cast<double>(k[0].i[1]); |
326 | |
|
327 | 0 | GET_HIGH_WORD(hx[1].i[0], x[1].d[0]); |
328 | 0 | GET_HIGH_WORD(hx[1].i[1], x[1].d[1]); |
329 | 0 | k[1].li = hx[1].li & cst_expmask; |
330 | 0 | hx[1].li &= ~cst_expmask; |
331 | 0 | i[1].li = (hx[1].li + cst_0x95f64) & cst_0x100000; |
332 | 0 | hx[1].li |= i[1].li ^ cst_0x3ff00000; |
333 | 0 | SET_HIGH_WORD(x[1].d[0], hx[1].i[0]); // Normalize x or x/2. |
334 | 0 | SET_HIGH_WORD(x[1].d[1], hx[1].i[1]); // Normalize x or x/2. |
335 | 0 | k[1].li += i[1].li; |
336 | 0 | dk[1].d[0] = static_cast<double>(k[1].i[0]); |
337 | 0 | dk[1].d[1] = static_cast<double>(k[1].i[1]); |
338 | |
|
339 | 0 | V2DF f[2] = {}; |
340 | 0 | f[0] = x[0].v2 - v2_one; |
341 | 0 | V2DF s[2] = {}; |
342 | 0 | s[0] = f[0] / (x[0].v2 + v2_one); |
343 | 0 | V2DF z[2] = {}; |
344 | 0 | z[0] = s[0] * s[0]; |
345 | 0 | V2DF w[2] = {}; |
346 | 0 | w[0] = z[0] * z[0]; |
347 | |
|
348 | 0 | V2DF t1[2] = {}; |
349 | | // coverity[ptr_arith] |
350 | 0 | t1[0] = w[0] * (v2_Lg2 + w[0] * (v2_Lg4 + w[0] * v2_Lg6)); |
351 | |
|
352 | 0 | V2DF t2[2] = {}; |
353 | | // coverity[ptr_arith] |
354 | 0 | t2[0] = |
355 | 0 | z[0] * (v2_Lg1 + w[0] * (v2_Lg3 + w[0] * (v2_Lg5 /*+w[0]*v2_Lg7*/))); |
356 | |
|
357 | 0 | V2DF R[2] = {}; |
358 | 0 | R[0] = t2[0] + t1[0]; |
359 | 0 | x[0].v2 = (dk[0].v2 - v2_const1023_mul_2pow20) * v2_ln2_div_2pow20 - |
360 | 0 | (s[0] * (f[0] - R[0]) - f[0]); |
361 | |
|
362 | 0 | f[1] = x[1].v2 - v2_one; |
363 | 0 | s[1] = f[1] / (x[1].v2 + v2_one); |
364 | 0 | z[1] = s[1] * s[1]; |
365 | 0 | w[1] = z[1] * z[1]; |
366 | | // coverity[ptr_arith] |
367 | 0 | t1[1] = w[1] * (v2_Lg2 + w[1] * (v2_Lg4 + w[1] * v2_Lg6)); |
368 | | // coverity[ptr_arith] |
369 | 0 | t2[1] = |
370 | 0 | z[1] * (v2_Lg1 + w[1] * (v2_Lg3 + w[1] * (v2_Lg5 /*+w[1]*v2_Lg7*/))); |
371 | 0 | R[1] = t2[1] + t1[1]; |
372 | 0 | x[1].v2 = (dk[1].v2 - v2_const1023_mul_2pow20) * v2_ln2_div_2pow20 - |
373 | 0 | (s[1] * (f[1] - R[1]) - f[1]); |
374 | 0 | } |
375 | | |
376 | | static CPL_INLINE void VizGeorefSpline2DBase_func4(double *res, |
377 | | const double *pxy, |
378 | | const double *xr, |
379 | | const double *yr) |
380 | 0 | { |
381 | 0 | v2dfunion xv[2] = {}; |
382 | 0 | xv[0].d[0] = xr[0]; |
383 | 0 | xv[0].d[1] = xr[1]; |
384 | 0 | xv[1].d[0] = xr[2]; |
385 | 0 | xv[1].d[1] = xr[3]; |
386 | 0 | v2dfunion yv[2] = {}; |
387 | 0 | yv[0].d[0] = yr[0]; |
388 | 0 | yv[0].d[1] = yr[1]; |
389 | 0 | yv[1].d[0] = yr[2]; |
390 | 0 | yv[1].d[1] = yr[3]; |
391 | 0 | v2dfunion x1v; |
392 | 0 | x1v.d[0] = pxy[0]; |
393 | 0 | x1v.d[1] = pxy[0]; |
394 | 0 | v2dfunion y1v; |
395 | 0 | y1v.d[0] = pxy[1]; |
396 | 0 | y1v.d[1] = pxy[1]; |
397 | 0 | v2dfunion dist[2] = {}; |
398 | 0 | dist[0].v2 = SQ(xv[0].v2 - x1v.v2) + SQ(yv[0].v2 - y1v.v2); |
399 | 0 | dist[1].v2 = SQ(xv[1].v2 - x1v.v2) + SQ(yv[1].v2 - y1v.v2); |
400 | 0 | v2dfunion resv[2] = {dist[0], dist[1]}; |
401 | 0 | FastApproxLog4Val(dist); |
402 | 0 | resv[0].v2 *= dist[0].v2; |
403 | 0 | resv[1].v2 *= dist[1].v2; |
404 | 0 | res[0] = resv[0].d[0]; |
405 | 0 | res[1] = resv[0].d[1]; |
406 | 0 | res[2] = resv[1].d[0]; |
407 | 0 | res[3] = resv[1].d[1]; |
408 | 0 | } |
409 | | #else // defined(USE_OPTIMIZED_VizGeorefSpline2DBase_func4) |
410 | | static void VizGeorefSpline2DBase_func4(double *res, const double *pxy, |
411 | | const double *xr, const double *yr) |
412 | | { |
413 | | double dist0 = SQ(xr[0] - pxy[0]) + SQ(yr[0] - pxy[1]); |
414 | | res[0] = dist0 != 0.0 ? dist0 * log(dist0) : 0.0; |
415 | | double dist1 = SQ(xr[1] - pxy[0]) + SQ(yr[1] - pxy[1]); |
416 | | res[1] = dist1 != 0.0 ? dist1 * log(dist1) : 0.0; |
417 | | double dist2 = SQ(xr[2] - pxy[0]) + SQ(yr[2] - pxy[1]); |
418 | | res[2] = dist2 != 0.0 ? dist2 * log(dist2) : 0.0; |
419 | | double dist3 = SQ(xr[3] - pxy[0]) + SQ(yr[3] - pxy[1]); |
420 | | res[3] = dist3 != 0.0 ? dist3 * log(dist3) : 0.0; |
421 | | } |
422 | | #endif // defined(USE_OPTIMIZED_VizGeorefSpline2DBase_func4) |
423 | | |
424 | | int VizGeorefSpline2D::solve(bool bForceBuiltinMethod) |
425 | 0 | { |
426 | | // No points at all. |
427 | 0 | if (_nof_points < 1) |
428 | 0 | { |
429 | 0 | type = VIZ_GEOREF_SPLINE_ZERO_POINTS; |
430 | 0 | return 0; |
431 | 0 | } |
432 | | |
433 | | // Only one point. |
434 | 0 | if (_nof_points == 1) |
435 | 0 | { |
436 | 0 | type = VIZ_GEOREF_SPLINE_ONE_POINT; |
437 | 0 | return 1; |
438 | 0 | } |
439 | | // Just 2 points - it is necessarily 1D case. |
440 | 0 | if (_nof_points == 2) |
441 | 0 | { |
442 | 0 | _dx = x[1] - x[0]; |
443 | 0 | _dy = y[1] - y[0]; |
444 | 0 | const double denom = _dx * _dx + _dy * _dy; |
445 | 0 | if (denom == 0.0) |
446 | 0 | return 0; |
447 | 0 | const double fact = 1.0 / denom; |
448 | 0 | _dx *= fact; |
449 | 0 | _dy *= fact; |
450 | |
|
451 | 0 | type = VIZ_GEOREF_SPLINE_TWO_POINTS; |
452 | 0 | return 2; |
453 | 0 | } |
454 | | |
455 | | // More than 2 points - first we have to check if it is 1D or 2D case |
456 | | |
457 | 0 | double xmax = x[0]; |
458 | 0 | double xmin = x[0]; |
459 | 0 | double ymax = y[0]; |
460 | 0 | double ymin = y[0]; |
461 | 0 | double sumx = 0.0; |
462 | 0 | double sumy = 0.0; |
463 | 0 | double sumx2 = 0.0; |
464 | 0 | double sumy2 = 0.0; |
465 | 0 | double sumxy = 0.0; |
466 | |
|
467 | 0 | for (int p = 0; p < _nof_points; p++) |
468 | 0 | { |
469 | 0 | const double xx = x[p]; |
470 | 0 | const double yy = y[p]; |
471 | |
|
472 | 0 | xmax = std::max(xmax, xx); |
473 | 0 | xmin = std::min(xmin, xx); |
474 | 0 | ymax = std::max(ymax, yy); |
475 | 0 | ymin = std::min(ymin, yy); |
476 | |
|
477 | 0 | sumx += xx; |
478 | 0 | sumx2 += xx * xx; |
479 | 0 | sumy += yy; |
480 | 0 | sumy2 += yy * yy; |
481 | 0 | sumxy += xx * yy; |
482 | 0 | } |
483 | 0 | const double delx = xmax - xmin; |
484 | 0 | const double dely = ymax - ymin; |
485 | |
|
486 | 0 | const double SSxx = sumx2 - sumx * sumx / _nof_points; |
487 | 0 | const double SSyy = sumy2 - sumy * sumy / _nof_points; |
488 | 0 | const double SSxy = sumxy - sumx * sumy / _nof_points; |
489 | |
|
490 | 0 | if (SSxx * SSyy == 0.0) |
491 | 0 | { |
492 | 0 | CPLError(CE_Failure, CPLE_AppDefined, |
493 | 0 | "Degenerate system. Computation aborted."); |
494 | 0 | return 0; |
495 | 0 | } |
496 | 0 | if (delx < 0.001 * dely || dely < 0.001 * delx || |
497 | 0 | fabs(SSxy * SSxy / (SSxx * SSyy)) > 0.99) |
498 | 0 | { |
499 | 0 | type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL; |
500 | |
|
501 | 0 | _dx = _nof_points * sumx2 - sumx * sumx; |
502 | 0 | _dy = _nof_points * sumy2 - sumy * sumy; |
503 | 0 | const double fact = 1.0 / sqrt(_dx * _dx + _dy * _dy); |
504 | 0 | _dx *= fact; |
505 | 0 | _dy *= fact; |
506 | |
|
507 | 0 | for (int p = 0; p < _nof_points; p++) |
508 | 0 | { |
509 | 0 | const double dxp = x[p] - x[0]; |
510 | 0 | const double dyp = y[p] - y[0]; |
511 | 0 | u[p] = _dx * dxp + _dy * dyp; |
512 | 0 | unused[p] = 1; |
513 | 0 | } |
514 | |
|
515 | 0 | for (int p = 0; p < _nof_points; p++) |
516 | 0 | { |
517 | 0 | int min_index = -1; |
518 | 0 | double min_u = 0.0; |
519 | 0 | for (int p1 = 0; p1 < _nof_points; p1++) |
520 | 0 | { |
521 | 0 | if (unused[p1]) |
522 | 0 | { |
523 | 0 | if (min_index < 0 || u[p1] < min_u) |
524 | 0 | { |
525 | 0 | min_index = p1; |
526 | 0 | min_u = u[p1]; |
527 | 0 | } |
528 | 0 | } |
529 | 0 | } |
530 | 0 | index[p] = min_index; |
531 | 0 | unused[min_index] = 0; |
532 | 0 | } |
533 | |
|
534 | 0 | return 3; |
535 | 0 | } |
536 | | |
537 | 0 | type = VIZ_GEOREF_SPLINE_FULL; |
538 | | // Make the necessary memory allocations. |
539 | |
|
540 | 0 | _nof_eqs = _nof_points + 3; |
541 | |
|
542 | 0 | if (_nof_eqs > std::numeric_limits<int>::max() / _nof_eqs) |
543 | 0 | { |
544 | 0 | CPLError(CE_Failure, CPLE_AppDefined, |
545 | 0 | "Too many coefficients. Computation aborted."); |
546 | 0 | return 0; |
547 | 0 | } |
548 | | |
549 | 0 | try |
550 | 0 | { |
551 | 0 | GDALMatrix A(_nof_eqs, _nof_eqs); |
552 | 0 | x_mean = 0; |
553 | 0 | y_mean = 0; |
554 | 0 | for (int c = 0; c < _nof_points; c++) |
555 | 0 | { |
556 | 0 | x_mean += x[c]; |
557 | 0 | y_mean += y[c]; |
558 | 0 | } |
559 | 0 | x_mean /= _nof_points; |
560 | 0 | y_mean /= _nof_points; |
561 | |
|
562 | 0 | for (int c = 0; c < _nof_points; c++) |
563 | 0 | { |
564 | 0 | x[c] -= x_mean; |
565 | 0 | y[c] -= y_mean; |
566 | 0 | A(0, c + 3) = 1.0; |
567 | 0 | A(1, c + 3) = x[c]; |
568 | 0 | A(2, c + 3) = y[c]; |
569 | |
|
570 | 0 | A(c + 3, 0) = 1.0; |
571 | 0 | A(c + 3, 1) = x[c]; |
572 | 0 | A(c + 3, 2) = y[c]; |
573 | 0 | } |
574 | |
|
575 | 0 | for (int r = 0; r < _nof_points; r++) |
576 | 0 | for (int c = r; c < _nof_points; c++) |
577 | 0 | { |
578 | 0 | A(r + 3, c + 3) = |
579 | 0 | VizGeorefSpline2DBase_func(x[r], y[r], x[c], y[c]); |
580 | 0 | if (r != c) |
581 | 0 | A(c + 3, r + 3) = A(r + 3, c + 3); |
582 | 0 | } |
583 | |
|
584 | | #if VIZ_GEOREF_SPLINE_DEBUG |
585 | | |
586 | | for (r = 0; r < _nof_eqs; r++) |
587 | | { |
588 | | for (c = 0; c < _nof_eqs; c++) |
589 | | fprintf(stderr, "%f", A(r, c)); /*ok*/ |
590 | | fprintf(stderr, "\n"); /*ok*/ |
591 | | } |
592 | | |
593 | | #endif |
594 | |
|
595 | 0 | GDALMatrix RHS(_nof_eqs, _nof_vars); |
596 | 0 | for (int iRHS = 0; iRHS < _nof_vars; iRHS++) |
597 | 0 | for (int iRow = 0; iRow < _nof_eqs; iRow++) |
598 | 0 | RHS(iRow, iRHS) = rhs[iRHS][iRow]; |
599 | |
|
600 | 0 | GDALMatrix Coef(_nof_eqs, _nof_vars); |
601 | |
|
602 | 0 | if (!GDALLinearSystemSolve(A, RHS, Coef, bForceBuiltinMethod)) |
603 | 0 | { |
604 | 0 | return 0; |
605 | 0 | } |
606 | | |
607 | 0 | for (int iRHS = 0; iRHS < _nof_vars; iRHS++) |
608 | 0 | for (int iRow = 0; iRow < _nof_eqs; iRow++) |
609 | 0 | coef[iRHS][iRow] = Coef(iRow, iRHS); |
610 | |
|
611 | 0 | return 4; |
612 | 0 | } |
613 | 0 | catch (const std::bad_alloc &) |
614 | 0 | { |
615 | 0 | CPLError(CE_Failure, CPLE_OutOfMemory, |
616 | 0 | "thinplatespline: out of memory allocating matrices"); |
617 | 0 | return 0; |
618 | 0 | } |
619 | 0 | } |
620 | | |
621 | | int VizGeorefSpline2D::get_point(const double Px, const double Py, double *vars) |
622 | 0 | { |
623 | 0 | switch (type) |
624 | 0 | { |
625 | 0 | case VIZ_GEOREF_SPLINE_ZERO_POINTS: |
626 | 0 | { |
627 | 0 | for (int v = 0; v < _nof_vars; v++) |
628 | 0 | vars[v] = 0.0; |
629 | 0 | break; |
630 | 0 | } |
631 | 0 | case VIZ_GEOREF_SPLINE_ONE_POINT: |
632 | 0 | { |
633 | 0 | for (int v = 0; v < _nof_vars; v++) |
634 | 0 | vars[v] = rhs[v][3]; |
635 | 0 | break; |
636 | 0 | } |
637 | 0 | case VIZ_GEOREF_SPLINE_TWO_POINTS: |
638 | 0 | { |
639 | 0 | const double fact = _dx * (Px - x[0]) + _dy * (Py - y[0]); |
640 | 0 | for (int v = 0; v < _nof_vars; v++) |
641 | 0 | vars[v] = (1 - fact) * rhs[v][3] + fact * rhs[v][4]; |
642 | 0 | break; |
643 | 0 | } |
644 | 0 | case VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL: |
645 | 0 | { |
646 | 0 | int leftP = 0; |
647 | 0 | int rightP = 0; |
648 | 0 | const double Pu = _dx * (Px - x[0]) + _dy * (Py - y[0]); |
649 | 0 | if (Pu <= u[index[0]]) |
650 | 0 | { |
651 | 0 | leftP = index[0]; |
652 | 0 | rightP = index[1]; |
653 | 0 | } |
654 | 0 | else if (Pu >= u[index[_nof_points - 1]]) |
655 | 0 | { |
656 | 0 | leftP = index[_nof_points - 2]; |
657 | 0 | rightP = index[_nof_points - 1]; |
658 | 0 | } |
659 | 0 | else |
660 | 0 | { |
661 | 0 | for (int r = 1; r < _nof_points; r++) |
662 | 0 | { |
663 | 0 | leftP = index[r - 1]; |
664 | 0 | rightP = index[r]; |
665 | 0 | if (Pu >= u[leftP] && Pu <= u[rightP]) |
666 | 0 | break; // Found. |
667 | 0 | } |
668 | 0 | } |
669 | |
|
670 | 0 | const double fact = (Pu - u[leftP]) / (u[rightP] - u[leftP]); |
671 | 0 | for (int v = 0; v < _nof_vars; v++) |
672 | 0 | vars[v] = (1.0 - fact) * rhs[v][leftP + 3] + |
673 | 0 | fact * rhs[v][rightP + 3]; |
674 | 0 | break; |
675 | 0 | } |
676 | 0 | case VIZ_GEOREF_SPLINE_FULL: |
677 | 0 | { |
678 | 0 | const double Pxy[2] = {Px - x_mean, Py - y_mean}; |
679 | 0 | for (int v = 0; v < _nof_vars; v++) |
680 | 0 | vars[v] = |
681 | 0 | coef[v][0] + coef[v][1] * Pxy[0] + coef[v][2] * Pxy[1]; |
682 | |
|
683 | 0 | int r = 0; // Used after for. |
684 | 0 | for (; r < (_nof_points & (~3)); r += 4) |
685 | 0 | { |
686 | 0 | double dfTmp[4] = {}; |
687 | 0 | VizGeorefSpline2DBase_func4(dfTmp, Pxy, &x[r], &y[r]); |
688 | 0 | for (int v = 0; v < _nof_vars; v++) |
689 | 0 | vars[v] += coef[v][r + 3] * dfTmp[0] + |
690 | 0 | coef[v][r + 3 + 1] * dfTmp[1] + |
691 | 0 | coef[v][r + 3 + 2] * dfTmp[2] + |
692 | 0 | coef[v][r + 3 + 3] * dfTmp[3]; |
693 | 0 | } |
694 | 0 | for (; r < _nof_points; r++) |
695 | 0 | { |
696 | 0 | const double tmp = |
697 | 0 | VizGeorefSpline2DBase_func(Pxy[0], Pxy[1], x[r], y[r]); |
698 | 0 | for (int v = 0; v < _nof_vars; v++) |
699 | 0 | vars[v] += coef[v][r + 3] * tmp; |
700 | 0 | } |
701 | 0 | break; |
702 | 0 | } |
703 | 0 | case VIZ_GEOREF_SPLINE_POINT_WAS_ADDED: |
704 | 0 | { |
705 | 0 | CPLError(CE_Failure, CPLE_AppDefined, |
706 | 0 | "A point was added after the last solve." |
707 | 0 | " NO interpolation - return values are zero"); |
708 | 0 | for (int v = 0; v < _nof_vars; v++) |
709 | 0 | vars[v] = 0.0; |
710 | 0 | return 0; |
711 | 0 | } |
712 | 0 | case VIZ_GEOREF_SPLINE_POINT_WAS_DELETED: |
713 | 0 | { |
714 | 0 | CPLError(CE_Failure, CPLE_AppDefined, |
715 | 0 | "A point was deleted after the last solve." |
716 | 0 | " NO interpolation - return values are zero"); |
717 | 0 | for (int v = 0; v < _nof_vars; v++) |
718 | 0 | vars[v] = 0.0; |
719 | 0 | return 0; |
720 | 0 | } |
721 | 0 | default: |
722 | 0 | { |
723 | 0 | return 0; |
724 | 0 | } |
725 | 0 | } |
726 | 0 | return 1; |
727 | 0 | } |
728 | | |
729 | | /*! @endcond */ |