Line data Source code
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 108 : bool VizGeorefSpline2D::grow_points()
42 :
43 : {
44 108 : const int new_max = _max_nof_points * 2 + 2 + 3;
45 :
46 : double *new_x =
47 108 : static_cast<double *>(VSI_REALLOC_VERBOSE(x, sizeof(double) * new_max));
48 108 : if (!new_x)
49 0 : return false;
50 108 : x = new_x;
51 : double *new_y =
52 108 : static_cast<double *>(VSI_REALLOC_VERBOSE(y, sizeof(double) * new_max));
53 108 : if (!new_y)
54 0 : return false;
55 108 : y = new_y;
56 : double *new_u =
57 108 : static_cast<double *>(VSI_REALLOC_VERBOSE(u, sizeof(double) * new_max));
58 108 : if (!new_u)
59 0 : return false;
60 108 : u = new_u;
61 : int *new_unused =
62 108 : static_cast<int *>(VSI_REALLOC_VERBOSE(unused, sizeof(int) * new_max));
63 108 : if (!new_unused)
64 0 : return false;
65 108 : unused = new_unused;
66 : int *new_index =
67 108 : static_cast<int *>(VSI_REALLOC_VERBOSE(index, sizeof(int) * new_max));
68 108 : if (!new_index)
69 0 : return false;
70 108 : index = new_index;
71 324 : for (int i = 0; i < _nof_vars; i++)
72 : {
73 : double *rhs_i_new = static_cast<double *>(
74 216 : VSI_REALLOC_VERBOSE(rhs[i], sizeof(double) * new_max));
75 216 : if (!rhs_i_new)
76 0 : return false;
77 216 : rhs[i] = rhs_i_new;
78 : double *coef_i_new = static_cast<double *>(
79 216 : VSI_REALLOC_VERBOSE(coef[i], sizeof(double) * new_max));
80 216 : if (!coef_i_new)
81 0 : return false;
82 216 : coef[i] = coef_i_new;
83 216 : if (_max_nof_points == 0)
84 : {
85 76 : memset(rhs[i], 0, 3 * sizeof(double));
86 76 : memset(coef[i], 0, 3 * sizeof(double));
87 : }
88 : }
89 :
90 108 : _max_nof_points = new_max - 3;
91 108 : return true;
92 : }
93 :
94 4812 : bool VizGeorefSpline2D::add_point(const double Px, const double Py,
95 : const double *Pvars)
96 : {
97 4812 : type = VIZ_GEOREF_SPLINE_POINT_WAS_ADDED;
98 : int i;
99 :
100 4812 : if (_nof_points == _max_nof_points)
101 : {
102 70 : if (!grow_points())
103 0 : return false;
104 : }
105 :
106 4812 : i = _nof_points;
107 : // A new point is added.
108 4812 : x[i] = Px;
109 4812 : y[i] = Py;
110 14436 : for (int j = 0; j < _nof_vars; j++)
111 9624 : rhs[j][i + 3] = Pvars[j];
112 4812 : _nof_points++;
113 4812 : return true;
114 : }
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 64118230 : template <typename T> static inline T SQ(const T &x)
171 : {
172 64118230 : return x * x;
173 : }
174 :
175 4606470 : static inline double VizGeorefSpline2DBase_func(const double x1,
176 : const double y1,
177 : const double x2,
178 : const double y2)
179 : {
180 4606470 : const double dist = SQ(x2 - x1) + SQ(y2 - y1);
181 4606470 : return dist != 0.0 ? dist * log(dist) : 0.0;
182 : }
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 : #define GET_HIGH_WORD(hx, x) memcpy(&hx, reinterpret_cast<char *>(&x) + 4, 4)
296 : #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 13726300 : static void FastApproxLog4Val(v2dfunion *x)
307 : {
308 13726300 : i64union hx[2] = {};
309 13726300 : i64union k[2] = {};
310 13726300 : i64union i[2] = {};
311 13726300 : GET_HIGH_WORD(hx[0].i[0], x[0].d[0]);
312 13726300 : GET_HIGH_WORD(hx[0].i[1], x[0].d[1]);
313 :
314 : // coverity[uninit_use]
315 13726300 : k[0].li = hx[0].li & cst_expmask;
316 13726300 : hx[0].li &= ~cst_expmask;
317 13726300 : i[0].li = (hx[0].li + cst_0x95f64) & cst_0x100000;
318 13726300 : hx[0].li |= i[0].li ^ cst_0x3ff00000;
319 13726300 : SET_HIGH_WORD(x[0].d[0], hx[0].i[0]); // Normalize x or x/2.
320 13726300 : SET_HIGH_WORD(x[0].d[1], hx[0].i[1]); // Normalize x or x/2.
321 13726300 : k[0].li += i[0].li;
322 :
323 13726300 : v2dfunion dk[2] = {};
324 13726300 : dk[0].d[0] = static_cast<double>(k[0].i[0]);
325 13726300 : dk[0].d[1] = static_cast<double>(k[0].i[1]);
326 :
327 13726300 : GET_HIGH_WORD(hx[1].i[0], x[1].d[0]);
328 13726300 : GET_HIGH_WORD(hx[1].i[1], x[1].d[1]);
329 13726300 : k[1].li = hx[1].li & cst_expmask;
330 13726300 : hx[1].li &= ~cst_expmask;
331 13726300 : i[1].li = (hx[1].li + cst_0x95f64) & cst_0x100000;
332 13726300 : hx[1].li |= i[1].li ^ cst_0x3ff00000;
333 13726300 : SET_HIGH_WORD(x[1].d[0], hx[1].i[0]); // Normalize x or x/2.
334 13726300 : SET_HIGH_WORD(x[1].d[1], hx[1].i[1]); // Normalize x or x/2.
335 13726300 : k[1].li += i[1].li;
336 13726300 : dk[1].d[0] = static_cast<double>(k[1].i[0]);
337 13726300 : dk[1].d[1] = static_cast<double>(k[1].i[1]);
338 :
339 13726300 : V2DF f[2] = {};
340 13726300 : f[0] = x[0].v2 - v2_one;
341 13726300 : V2DF s[2] = {};
342 13726300 : s[0] = f[0] / (x[0].v2 + v2_one);
343 13726300 : V2DF z[2] = {};
344 13726300 : z[0] = s[0] * s[0];
345 13726300 : V2DF w[2] = {};
346 13726300 : w[0] = z[0] * z[0];
347 :
348 13726300 : V2DF t1[2] = {};
349 : // coverity[ptr_arith]
350 13726300 : t1[0] = w[0] * (v2_Lg2 + w[0] * (v2_Lg4 + w[0] * v2_Lg6));
351 :
352 13726300 : V2DF t2[2] = {};
353 : // coverity[ptr_arith]
354 13726300 : t2[0] =
355 13726300 : z[0] * (v2_Lg1 + w[0] * (v2_Lg3 + w[0] * (v2_Lg5 /*+w[0]*v2_Lg7*/)));
356 :
357 13726300 : V2DF R[2] = {};
358 13726300 : R[0] = t2[0] + t1[0];
359 13726300 : x[0].v2 = (dk[0].v2 - v2_const1023_mul_2pow20) * v2_ln2_div_2pow20 -
360 13726300 : (s[0] * (f[0] - R[0]) - f[0]);
361 :
362 13726300 : f[1] = x[1].v2 - v2_one;
363 13726300 : s[1] = f[1] / (x[1].v2 + v2_one);
364 13726300 : z[1] = s[1] * s[1];
365 13726300 : w[1] = z[1] * z[1];
366 : // coverity[ptr_arith]
367 13726300 : t1[1] = w[1] * (v2_Lg2 + w[1] * (v2_Lg4 + w[1] * v2_Lg6));
368 : // coverity[ptr_arith]
369 13726300 : t2[1] =
370 13726300 : z[1] * (v2_Lg1 + w[1] * (v2_Lg3 + w[1] * (v2_Lg5 /*+w[1]*v2_Lg7*/)));
371 13726300 : R[1] = t2[1] + t1[1];
372 13726300 : x[1].v2 = (dk[1].v2 - v2_const1023_mul_2pow20) * v2_ln2_div_2pow20 -
373 13726300 : (s[1] * (f[1] - R[1]) - f[1]);
374 13726300 : }
375 :
376 13726300 : static CPL_INLINE void VizGeorefSpline2DBase_func4(double *res,
377 : const double *pxy,
378 : const double *xr,
379 : const double *yr)
380 : {
381 13726300 : v2dfunion xv[2] = {};
382 13726300 : xv[0].d[0] = xr[0];
383 13726300 : xv[0].d[1] = xr[1];
384 13726300 : xv[1].d[0] = xr[2];
385 13726300 : xv[1].d[1] = xr[3];
386 13726300 : v2dfunion yv[2] = {};
387 13726300 : yv[0].d[0] = yr[0];
388 13726300 : yv[0].d[1] = yr[1];
389 13726300 : yv[1].d[0] = yr[2];
390 13726300 : yv[1].d[1] = yr[3];
391 : v2dfunion x1v;
392 13726300 : x1v.d[0] = pxy[0];
393 13726300 : x1v.d[1] = pxy[0];
394 : v2dfunion y1v;
395 13726300 : y1v.d[0] = pxy[1];
396 13726300 : y1v.d[1] = pxy[1];
397 13726300 : v2dfunion dist[2] = {};
398 13726300 : dist[0].v2 = SQ(xv[0].v2 - x1v.v2) + SQ(yv[0].v2 - y1v.v2);
399 13726300 : dist[1].v2 = SQ(xv[1].v2 - x1v.v2) + SQ(yv[1].v2 - y1v.v2);
400 13726300 : v2dfunion resv[2] = {dist[0], dist[1]};
401 13726300 : FastApproxLog4Val(dist);
402 13726300 : resv[0].v2 *= dist[0].v2;
403 13726300 : resv[1].v2 *= dist[1].v2;
404 13726300 : res[0] = resv[0].d[0];
405 13726300 : res[1] = resv[0].d[1];
406 13726300 : res[2] = resv[1].d[0];
407 13726300 : res[3] = resv[1].d[1];
408 13726300 : }
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 37 : int VizGeorefSpline2D::solve(bool bForceBuiltinMethod)
425 : {
426 : // No points at all.
427 37 : if (_nof_points < 1)
428 : {
429 0 : type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
430 0 : return 0;
431 : }
432 :
433 : // Only one point.
434 37 : if (_nof_points == 1)
435 : {
436 0 : type = VIZ_GEOREF_SPLINE_ONE_POINT;
437 0 : return 1;
438 : }
439 : // Just 2 points - it is necessarily 1D case.
440 37 : if (_nof_points == 2)
441 : {
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 : }
454 :
455 : // More than 2 points - first we have to check if it is 1D or 2D case
456 :
457 37 : double xmax = x[0];
458 37 : double xmin = x[0];
459 37 : double ymax = y[0];
460 37 : double ymin = y[0];
461 37 : double sumx = 0.0;
462 37 : double sumy = 0.0;
463 37 : double sumx2 = 0.0;
464 37 : double sumy2 = 0.0;
465 37 : double sumxy = 0.0;
466 :
467 4844 : for (int p = 0; p < _nof_points; p++)
468 : {
469 4807 : const double xx = x[p];
470 4807 : const double yy = y[p];
471 :
472 4807 : xmax = std::max(xmax, xx);
473 4807 : xmin = std::min(xmin, xx);
474 4807 : ymax = std::max(ymax, yy);
475 4807 : ymin = std::min(ymin, yy);
476 :
477 4807 : sumx += xx;
478 4807 : sumx2 += xx * xx;
479 4807 : sumy += yy;
480 4807 : sumy2 += yy * yy;
481 4807 : sumxy += xx * yy;
482 : }
483 37 : const double delx = xmax - xmin;
484 37 : const double dely = ymax - ymin;
485 :
486 37 : const double SSxx = sumx2 - sumx * sumx / _nof_points;
487 37 : const double SSyy = sumy2 - sumy * sumy / _nof_points;
488 37 : const double SSxy = sumxy - sumx * sumy / _nof_points;
489 :
490 37 : if (SSxx * SSyy == 0.0)
491 : {
492 0 : CPLError(CE_Failure, CPLE_AppDefined,
493 : "Degenerate system. Computation aborted.");
494 0 : return 0;
495 : }
496 37 : if (delx < 0.001 * dely || dely < 0.001 * delx ||
497 37 : fabs(SSxy * SSxy / (SSxx * SSyy)) > 0.99)
498 : {
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 : {
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 : }
514 :
515 0 : for (int p = 0; p < _nof_points; p++)
516 : {
517 0 : int min_index = -1;
518 0 : double min_u = 0.0;
519 0 : for (int p1 = 0; p1 < _nof_points; p1++)
520 : {
521 0 : if (unused[p1])
522 : {
523 0 : if (min_index < 0 || u[p1] < min_u)
524 : {
525 0 : min_index = p1;
526 0 : min_u = u[p1];
527 : }
528 : }
529 : }
530 0 : index[p] = min_index;
531 0 : unused[min_index] = 0;
532 : }
533 :
534 0 : return 3;
535 : }
536 :
537 37 : type = VIZ_GEOREF_SPLINE_FULL;
538 : // Make the necessary memory allocations.
539 :
540 37 : _nof_eqs = _nof_points + 3;
541 :
542 37 : if (_nof_eqs > std::numeric_limits<int>::max() / _nof_eqs)
543 : {
544 0 : CPLError(CE_Failure, CPLE_AppDefined,
545 : "Too many coefficients. Computation aborted.");
546 0 : return 0;
547 : }
548 :
549 : try
550 : {
551 74 : GDALMatrix A(_nof_eqs, _nof_eqs);
552 37 : x_mean = 0;
553 37 : y_mean = 0;
554 4844 : for (int c = 0; c < _nof_points; c++)
555 : {
556 4807 : x_mean += x[c];
557 4807 : y_mean += y[c];
558 : }
559 37 : x_mean /= _nof_points;
560 37 : y_mean /= _nof_points;
561 :
562 4844 : for (int c = 0; c < _nof_points; c++)
563 : {
564 4807 : x[c] -= x_mean;
565 4807 : y[c] -= y_mean;
566 4807 : A(0, c + 3) = 1.0;
567 4807 : A(1, c + 3) = x[c];
568 4807 : A(2, c + 3) = y[c];
569 :
570 4807 : A(c + 3, 0) = 1.0;
571 4807 : A(c + 3, 1) = x[c];
572 4807 : A(c + 3, 2) = y[c];
573 : }
574 :
575 4844 : for (int r = 0; r < _nof_points; r++)
576 4525030 : for (int c = r; c < _nof_points; c++)
577 : {
578 9040450 : A(r + 3, c + 3) =
579 4520230 : VizGeorefSpline2DBase_func(x[r], y[r], x[c], y[c]);
580 4520230 : if (r != c)
581 4515420 : A(c + 3, r + 3) = A(r + 3, c + 3);
582 : }
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 74 : GDALMatrix RHS(_nof_eqs, _nof_vars);
596 111 : for (int iRHS = 0; iRHS < _nof_vars; iRHS++)
597 9910 : for (int iRow = 0; iRow < _nof_eqs; iRow++)
598 9836 : RHS(iRow, iRHS) = rhs[iRHS][iRow];
599 :
600 74 : GDALMatrix Coef(_nof_eqs, _nof_vars);
601 :
602 37 : if (!GDALLinearSystemSolve(A, RHS, Coef, bForceBuiltinMethod))
603 : {
604 2 : return 0;
605 : }
606 :
607 105 : for (int iRHS = 0; iRHS < _nof_vars; iRHS++)
608 9874 : for (int iRow = 0; iRow < _nof_eqs; iRow++)
609 9804 : coef[iRHS][iRow] = Coef(iRow, iRHS);
610 :
611 35 : return 4;
612 : }
613 0 : catch (const std::bad_alloc &)
614 : {
615 0 : CPLError(CE_Failure, CPLE_OutOfMemory,
616 : "thinplatespline: out of memory allocating matrices");
617 0 : return 0;
618 : }
619 : }
620 :
621 567854 : int VizGeorefSpline2D::get_point(const double Px, const double Py, double *vars)
622 : {
623 567854 : switch (type)
624 : {
625 0 : case VIZ_GEOREF_SPLINE_ZERO_POINTS:
626 : {
627 0 : for (int v = 0; v < _nof_vars; v++)
628 0 : vars[v] = 0.0;
629 0 : break;
630 : }
631 0 : case VIZ_GEOREF_SPLINE_ONE_POINT:
632 : {
633 0 : for (int v = 0; v < _nof_vars; v++)
634 0 : vars[v] = rhs[v][3];
635 0 : break;
636 : }
637 0 : case VIZ_GEOREF_SPLINE_TWO_POINTS:
638 : {
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 : }
644 0 : case VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL:
645 : {
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 : {
651 0 : leftP = index[0];
652 0 : rightP = index[1];
653 : }
654 0 : else if (Pu >= u[index[_nof_points - 1]])
655 : {
656 0 : leftP = index[_nof_points - 2];
657 0 : rightP = index[_nof_points - 1];
658 : }
659 : else
660 : {
661 0 : for (int r = 1; r < _nof_points; r++)
662 : {
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 : }
668 : }
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 : }
676 567854 : case VIZ_GEOREF_SPLINE_FULL:
677 : {
678 567854 : const double Pxy[2] = {Px - x_mean, Py - y_mean};
679 1703560 : for (int v = 0; v < _nof_vars; v++)
680 1135710 : vars[v] =
681 1135710 : coef[v][0] + coef[v][1] * Pxy[0] + coef[v][2] * Pxy[1];
682 :
683 567854 : int r = 0; // Used after for.
684 14294200 : for (; r < (_nof_points & (~3)); r += 4)
685 : {
686 13726300 : double dfTmp[4] = {};
687 13726300 : VizGeorefSpline2DBase_func4(dfTmp, Pxy, &x[r], &y[r]);
688 41179000 : for (int v = 0; v < _nof_vars; v++)
689 27452700 : vars[v] += coef[v][r + 3] * dfTmp[0] +
690 27452700 : coef[v][r + 3 + 1] * dfTmp[1] +
691 27452700 : coef[v][r + 3 + 2] * dfTmp[2] +
692 27452700 : coef[v][r + 3 + 3] * dfTmp[3];
693 : }
694 654093 : for (; r < _nof_points; r++)
695 : {
696 : const double tmp =
697 86239 : VizGeorefSpline2DBase_func(Pxy[0], Pxy[1], x[r], y[r]);
698 258717 : for (int v = 0; v < _nof_vars; v++)
699 172478 : vars[v] += coef[v][r + 3] * tmp;
700 : }
701 567854 : break;
702 : }
703 0 : case VIZ_GEOREF_SPLINE_POINT_WAS_ADDED:
704 : {
705 0 : CPLError(CE_Failure, CPLE_AppDefined,
706 : "A point was added after the last solve."
707 : " 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 : }
712 0 : case VIZ_GEOREF_SPLINE_POINT_WAS_DELETED:
713 : {
714 0 : CPLError(CE_Failure, CPLE_AppDefined,
715 : "A point was deleted after the last solve."
716 : " 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 : }
721 0 : default:
722 : {
723 0 : return 0;
724 : }
725 : }
726 567854 : return 1;
727 : }
728 :
729 : /*! @endcond */
|
