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