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