1 /******************************************************************************
2 * $Id: thinplatespline.cpp 27546 2014-07-22 22:40:02Z rouault $
3 *
4 * Project: GDAL Warp API
5 * Purpose: Implemenentation of 2D Thin Plate Spline transformer.
6 * Author: VIZRT Development Team.
7 *
8 * This code was provided by Gilad Ronnen (gro at visrt dot com) with
9 * permission to reuse under the following license.
10 *
11 ******************************************************************************
12 * Copyright (c) 2004, VIZRT Inc.
13 * Copyright (c) 2008-2014, Even Rouault <even dot rouault at mines-paris dot org>
14 *
15 * Permission is hereby granted, free of charge, to any person obtaining a
16 * copy of this software and associated documentation files (the "Software"),
17 * to deal in the Software without restriction, including without limitation
18 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
19 * and/or sell copies of the Software, and to permit persons to whom the
20 * Software is furnished to do so, subject to the following conditions:
21 *
22 * The above copyright notice and this permission notice shall be included
23 * in all copies or substantial portions of the Software.
24 *
25 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
26 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
27 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
28 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
29 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
30 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
31 * DEALINGS IN THE SOFTWARE.
32 ****************************************************************************/
33
34 #ifdef HAVE_ARMADILLO
35 /* Include before #define A(r,c) because armadillo uses A in its include files */
36 #include "armadillo"
37 #endif
38
39 #include "thinplatespline.h"
40
41 /////////////////////////////////////////////////////////////////////////////////////
42 //// vizGeorefSpline2D
43 /////////////////////////////////////////////////////////////////////////////////////
44
45 #define A(r,c) _AA[ _nof_eqs * (r) + (c) ]
46 #define Ainv(r,c) _Ainv[ _nof_eqs * (r) + (c) ]
47
48
49 #define VIZ_GEOREF_SPLINE_DEBUG 0
50
51 #ifndef HAVE_ARMADILLO
52 static int matrixInvert( int N, double input[], double output[] );
53 #endif
54
grow_points()55 void VizGeorefSpline2D::grow_points()
56
57 {
58 int new_max = _max_nof_points*2 + 2 + 3;
59 int i;
60
61 x = (double *) VSIRealloc( x, sizeof(double) * new_max );
62 y = (double *) VSIRealloc( y, sizeof(double) * new_max );
63 u = (double *) VSIRealloc( u, sizeof(double) * new_max );
64 unused = (int *) VSIRealloc( unused, sizeof(int) * new_max );
65 index = (int *) VSIRealloc( index, sizeof(int) * new_max );
66 for( i = 0; i < VIZGEOREF_MAX_VARS; i++ )
67 {
68 rhs[i] = (double *)
69 VSIRealloc( rhs[i], sizeof(double) * new_max );
70 coef[i] = (double *)
71 VSIRealloc( coef[i], sizeof(double) * new_max );
72 if( _max_nof_points == 0 )
73 {
74 memset(rhs[i], 0, 3 * sizeof(double));
75 memset(coef[i], 0, 3 * sizeof(double));
76 }
77 }
78
79 _max_nof_points = new_max - 3;
80 }
81
add_point(const double Px,const double Py,const double * Pvars)82 int VizGeorefSpline2D::add_point( const double Px, const double Py, const double *Pvars )
83 {
84 type = VIZ_GEOREF_SPLINE_POINT_WAS_ADDED;
85 int i;
86
87 if( _nof_points == _max_nof_points )
88 grow_points();
89
90 i = _nof_points;
91 //A new point is added
92 x[i] = Px;
93 y[i] = Py;
94 for ( int j = 0; j < _nof_vars; j++ )
95 rhs[j][i+3] = Pvars[j];
96 _nof_points++;
97 return 1;
98 }
99
100 #if 0
101 bool VizGeorefSpline2D::change_point(int index, double Px, double Py, double* Pvars)
102 {
103 if ( index < _nof_points )
104 {
105 int i = index;
106 x[i] = Px;
107 y[i] = Py;
108 for ( int j = 0; j < _nof_vars; j++ )
109 rhs[j][i+3] = Pvars[j];
110 }
111
112 return( true );
113 }
114
115 bool VizGeorefSpline2D::get_xy(int index, double& outX, double& outY)
116 {
117 bool ok;
118
119 if ( index < _nof_points )
120 {
121 ok = true;
122 outX = x[index];
123 outY = y[index];
124 }
125 else
126 {
127 ok = false;
128 outX = outY = 0.0f;
129 }
130
131 return(ok);
132 }
133
134 int VizGeorefSpline2D::delete_point(const double Px, const double Py )
135 {
136 for ( int i = 0; i < _nof_points; i++ )
137 {
138 if ( ( fabs(Px - x[i]) <= _tx ) && ( fabs(Py - y[i]) <= _ty ) )
139 {
140 for ( int j = i; j < _nof_points - 1; j++ )
141 {
142 x[j] = x[j+1];
143 y[j] = y[j+1];
144 for ( int k = 0; k < _nof_vars; k++ )
145 rhs[k][j+3] = rhs[k][j+3+1];
146 }
147 _nof_points--;
148 type = VIZ_GEOREF_SPLINE_POINT_WAS_DELETED;
149 return(1);
150 }
151 }
152 return(0);
153 }
154 #endif
155
156 #define SQ(x) ((x)*(x))
157
VizGeorefSpline2DBase_func(const double x1,const double y1,const double x2,const double y2)158 static CPL_INLINE double VizGeorefSpline2DBase_func( const double x1, const double y1,
159 const double x2, const double y2 )
160 {
161 double dist = SQ( x2 - x1 ) + SQ( y2 - y1 );
162 return dist ? dist * log( dist ) : 0.0;
163 }
164
165 #if defined(__GNUC__) && defined(__x86_64__)
166
167 /* Derived and adapted from code originating from: */
168
169 /* @(#)e_log.c 1.3 95/01/18 */
170 /*
171 * ====================================================
172 * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
173 *
174 * Developed at SunSoft, a Sun Microsystems, Inc. business.
175 * Permission to use, copy, modify, and distribute this
176 * software is freely granted, provided that this notice
177 * is preserved.
178 * ====================================================
179 */
180
181 /* __ieee754_log(x)
182 * Return the logrithm of x
183 *
184 * Method :
185 * 1. Argument Reduction: find k and f such that
186 * x = 2^k * (1+f),
187 * where sqrt(2)/2 < 1+f < sqrt(2) .
188 *
189 * 2. Approximation of log(1+f).
190 * Let s = f/(2+f) ; based on log(1+f) = log(1+s) - log(1-s)
191 * = 2s + 2/3 s**3 + 2/5 s**5 + .....,
192 * = 2s + s*R
193 * We use a special Reme algorithm on [0,0.1716] to generate
194 * a polynomial of degree 14 to approximate R The maximum error
195 * of this polynomial approximation is bounded by 2**-58.45. In
196 * other words,
197 * 2 4 6 8 10 12 14
198 * R(z) ~ Lg1*s +Lg2*s +Lg3*s +Lg4*s +Lg5*s +Lg6*s +Lg7*s
199 * (the values of Lg1 to Lg7 are listed in the program)
200 * and
201 * | 2 14 | -58.45
202 * | Lg1*s +...+Lg7*s - R(z) | <= 2
203 * | |
204 * Note that 2s = f - s*f = f - hfsq + s*hfsq, where hfsq = f*f/2.
205 * In order to guarantee error in log below 1ulp, we compute log
206 * by
207 * log(1+f) = f - s*(f - R) (if f is not too large)
208 * log(1+f) = f - (hfsq - s*(hfsq+R)). (better accuracy)
209 *
210 * 3. Finally, log(x) = k*ln2 + log(1+f).
211 * = k*ln2_hi+(f-(hfsq-(s*(hfsq+R)+k*ln2_lo)))
212 * Here ln2 is split into two floating point number:
213 * ln2_hi + ln2_lo,
214 * where n*ln2_hi is always exact for |n| < 2000.
215 *
216 * Special cases:
217 * log(x) is NaN with signal if x < 0 (including -INF) ;
218 * log(+INF) is +INF; log(0) is -INF with signal;
219 * log(NaN) is that NaN with no signal.
220 *
221 * Accuracy:
222 * according to an error analysis, the error is always less than
223 * 1 ulp (unit in the last place).
224 *
225 * Constants:
226 * The hexadecimal values are the intended ones for the following
227 * constants. The decimal values may be used, provided that the
228 * compiler will convert from decimal to binary accurately enough
229 * to produce the hexadecimal values shown.
230 */
231
232 typedef double V2DF __attribute__ ((__vector_size__ (16)));
233 typedef union
234 {
235 V2DF v2;
236 double d[2];
237 } v2dfunion;
238
239 typedef union
240 {
241 int i[2];
242 long long li;
243 } i64union;
244
245 static const V2DF
246 v2_ln2_div_2pow20 = {6.93147180559945286e-01 / 1048576, 6.93147180559945286e-01 / 1048576},
247 v2_Lg1 = {6.666666666666735130e-01, 6.666666666666735130e-01},
248 v2_Lg2 = {3.999999999940941908e-01, 3.999999999940941908e-01},
249 v2_Lg3 = {2.857142874366239149e-01, 2.857142874366239149e-01},
250 v2_Lg4 = {2.222219843214978396e-01, 2.222219843214978396e-01},
251 v2_Lg5 = {1.818357216161805012e-01, 1.818357216161805012e-01},
252 v2_Lg6 = {1.531383769920937332e-01, 1.531383769920937332e-01},
253 /*v2_Lg7 = {1.479819860511658591e-01, 1.479819860511658591e-01}, */
254 v2_one = { 1.0, 1.0 },
255 v2_const1023_mul_2pow20 = { 1023.0 * 1048576, 1023.0 * 1048576};
256
257 #define GET_HIGH_WORD(hx,x) memcpy(&hx,((char*)&x+4),4)
258 #define SET_HIGH_WORD(x,hx) memcpy(((char*)&x+4),&hx,4)
259
260 #define MAKE_WIDE_CST(x) ((((long long)(x)) << 32) | (x))
261 static const long long cst_expmask = MAKE_WIDE_CST(0xfff00000);
262 static const long long cst_0x95f64 = MAKE_WIDE_CST(0x00095f64);
263 static const long long cst_0x100000 = MAKE_WIDE_CST(0x00100000);
264 static const long long cst_0x3ff00000 = MAKE_WIDE_CST(0x3ff00000);
265
266 /* Modified version of __ieee754_log(), less precise than log() but a bit */
267 /* faste, and computing 4 log() at a time. Assumes that the values are > 0 */
FastApproxLog4Val(v2dfunion * x)268 static void FastApproxLog4Val(v2dfunion* x)
269 {
270 V2DF f[2],s[2],z[2],R[2],w[2],t1[2],t2[2];
271 v2dfunion dk[2];
272 i64union k[2], hx[2], i[2];
273
274 GET_HIGH_WORD(hx[0].i[0],x[0].d[0]);
275 GET_HIGH_WORD(hx[0].i[1],x[0].d[1]);
276 k[0].li = hx[0].li & cst_expmask;
277 hx[0].li &= ~cst_expmask;
278 i[0].li = (hx[0].li + cst_0x95f64) & cst_0x100000;
279 hx[0].li |= i[0].li ^ cst_0x3ff00000;
280 SET_HIGH_WORD(x[0].d[0],hx[0].i[0]); /* normalize x or x/2 */
281 SET_HIGH_WORD(x[0].d[1],hx[0].i[1]); /* normalize x or x/2 */
282 k[0].li += i[0].li;
283 dk[0].d[0] = (double)k[0].i[0];
284 dk[0].d[1] = (double)k[0].i[1];
285
286 GET_HIGH_WORD(hx[1].i[0],x[1].d[0]);
287 GET_HIGH_WORD(hx[1].i[1],x[1].d[1]);
288 k[1].li = hx[1].li & cst_expmask;
289 hx[1].li &= ~cst_expmask;
290 i[1].li = (hx[1].li + cst_0x95f64) & cst_0x100000;
291 hx[1].li |= i[1].li ^ cst_0x3ff00000;
292 SET_HIGH_WORD(x[1].d[0],hx[1].i[0]); /* normalize x or x/2 */
293 SET_HIGH_WORD(x[1].d[1],hx[1].i[1]); /* normalize x or x/2 */
294 k[1].li += i[1].li;
295 dk[1].d[0] = (double)k[1].i[0];
296 dk[1].d[1] = (double)k[1].i[1];
297
298 f[0] = x[0].v2-v2_one;
299 s[0] = f[0]/(x[0].v2+v2_one);
300 z[0] = s[0]*s[0];
301 w[0] = z[0]*z[0];
302 t1[0]= w[0]*(v2_Lg2+w[0]*(v2_Lg4+w[0]*v2_Lg6));
303 t2[0]= z[0]*(v2_Lg1+w[0]*(v2_Lg3+w[0]*(v2_Lg5/*+w[0]*v2_Lg7*/)));
304 R[0] = t2[0]+t1[0];
305 x[0].v2 = ((dk[0].v2 - v2_const1023_mul_2pow20)*v2_ln2_div_2pow20-(s[0]*(f[0]-R[0])-f[0]));
306
307 f[1] = x[1].v2-v2_one;
308 s[1] = f[1]/(x[1].v2+v2_one);
309 z[1] = s[1]*s[1];
310 w[1] = z[1]*z[1];
311 t1[1]= w[1]*(v2_Lg2+w[1]*(v2_Lg4+w[1]*v2_Lg6));
312 t2[1]= z[1]*(v2_Lg1+w[1]*(v2_Lg3+w[1]*(v2_Lg5/*+w[1]*v2_Lg7*/)));
313 R[1] = t2[1]+t1[1];
314 x[1].v2 = ((dk[1].v2- v2_const1023_mul_2pow20)*v2_ln2_div_2pow20-(s[1]*(f[1]-R[1])-f[1]));
315 }
316
VizGeorefSpline2DBase_func4(double * res,const double * pxy,const double * xr,const double * yr)317 static CPL_INLINE void VizGeorefSpline2DBase_func4( double* res,
318 const double* pxy,
319 const double* xr, const double* yr )
320 {
321 v2dfunion x1v, y1v, xv[2], yv[2], dist[2], resv[2];
322 xv[0].d[0] = xr[0];
323 xv[0].d[1] = xr[1];
324 xv[1].d[0] = xr[2];
325 xv[1].d[1] = xr[3];
326 yv[0].d[0] = yr[0];
327 yv[0].d[1] = yr[1];
328 yv[1].d[0] = yr[2];
329 yv[1].d[1] = yr[3];
330 x1v.d[0] = pxy[0];
331 x1v.d[1] = pxy[0];
332 y1v.d[0] = pxy[1];
333 y1v.d[1] = pxy[1];
334 dist[0].v2 = SQ( xv[0].v2 - x1v.v2 ) + SQ( yv[0].v2 - y1v.v2 );
335 dist[1].v2 = SQ( xv[1].v2 - x1v.v2 ) + SQ( yv[1].v2 - y1v.v2 );
336 resv[0] = dist[0];
337 resv[1] = dist[1];
338 FastApproxLog4Val(dist);
339 resv[0].v2 *= dist[0].v2;
340 resv[1].v2 *= dist[1].v2;
341 res[0] = resv[0].d[0];
342 res[1] = resv[0].d[1];
343 res[2] = resv[1].d[0];
344 res[3] = resv[1].d[1];
345 }
346 #else
VizGeorefSpline2DBase_func4(double * res,const double * pxy,const double * xr,const double * yr)347 static void VizGeorefSpline2DBase_func4( double* res,
348 const double* pxy,
349 const double* xr, const double* yr )
350 {
351 double dist0 = SQ( xr[0] - pxy[0] ) + SQ( yr[0] - pxy[1] );
352 res[0] = dist0 ? dist0 * log(dist0) : 0.0;
353 double dist1 = SQ( xr[1] - pxy[0] ) + SQ( yr[1] - pxy[1] );
354 res[1] = dist1 ? dist1 * log(dist1) : 0.0;
355 double dist2 = SQ( xr[2] - pxy[0] ) + SQ( yr[2] - pxy[1] );
356 res[2] = dist2 ? dist2 * log(dist2) : 0.0;
357 double dist3 = SQ( xr[3] - pxy[0] ) + SQ( yr[3] - pxy[1] );
358 res[3] = dist3 ? dist3 * log(dist3) : 0.0;
359 }
360 #endif
361
solve(void)362 int VizGeorefSpline2D::solve(void)
363 {
364 int r, c;
365 int p;
366
367 // No points at all
368 if ( _nof_points < 1 )
369 {
370 type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
371 return(0);
372 }
373
374 // Only one point
375 if ( _nof_points == 1 )
376 {
377 type = VIZ_GEOREF_SPLINE_ONE_POINT;
378 return(1);
379 }
380 // Just 2 points - it is necessarily 1D case
381 if ( _nof_points == 2 )
382 {
383 _dx = x[1] - x[0];
384 _dy = y[1] - y[0];
385 double fact = 1.0 / ( _dx * _dx + _dy * _dy );
386 _dx *= fact;
387 _dy *= fact;
388
389 type = VIZ_GEOREF_SPLINE_TWO_POINTS;
390 return(2);
391 }
392
393 // More than 2 points - first we have to check if it is 1D or 2D case
394
395 double xmax = x[0], xmin = x[0], ymax = y[0], ymin = y[0];
396 double delx, dely;
397 double xx, yy;
398 double sumx = 0.0f, sumy= 0.0f, sumx2 = 0.0f, sumy2 = 0.0f, sumxy = 0.0f;
399 double SSxx, SSyy, SSxy;
400
401 for ( p = 0; p < _nof_points; p++ )
402 {
403 xx = x[p];
404 yy = y[p];
405
406 xmax = MAX( xmax, xx );
407 xmin = MIN( xmin, xx );
408 ymax = MAX( ymax, yy );
409 ymin = MIN( ymin, yy );
410
411 sumx += xx;
412 sumx2 += xx * xx;
413 sumy += yy;
414 sumy2 += yy * yy;
415 sumxy += xx * yy;
416 }
417 delx = xmax - xmin;
418 dely = ymax - ymin;
419
420 SSxx = sumx2 - sumx * sumx / _nof_points;
421 SSyy = sumy2 - sumy * sumy / _nof_points;
422 SSxy = sumxy - sumx * sumy / _nof_points;
423
424 if ( delx < 0.001 * dely || dely < 0.001 * delx ||
425 fabs ( SSxy * SSxy / ( SSxx * SSyy ) ) > 0.99 )
426 {
427 int p1;
428
429 type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL;
430
431 _dx = _nof_points * sumx2 - sumx * sumx;
432 _dy = _nof_points * sumy2 - sumy * sumy;
433 double fact = 1.0 / sqrt( _dx * _dx + _dy * _dy );
434 _dx *= fact;
435 _dy *= fact;
436
437 for ( p = 0; p < _nof_points; p++ )
438 {
439 double dxp = x[p] - x[0];
440 double dyp = y[p] - y[0];
441 u[p] = _dx * dxp + _dy * dyp;
442 unused[p] = 1;
443 }
444
445 for ( p = 0; p < _nof_points; p++ )
446 {
447 int min_index = -1;
448 double min_u = 0;
449 for ( p1 = 0; p1 < _nof_points; p1++ )
450 {
451 if ( unused[p1] )
452 {
453 if ( min_index < 0 || u[p1] < min_u )
454 {
455 min_index = p1;
456 min_u = u[p1];
457 }
458 }
459 }
460 index[p] = min_index;
461 unused[min_index] = 0;
462 }
463
464 return(3);
465 }
466
467 type = VIZ_GEOREF_SPLINE_FULL;
468 // Make the necessary memory allocations
469
470 _nof_eqs = _nof_points + 3;
471
472 if( _nof_eqs > INT_MAX / _nof_eqs )
473 {
474 CPLError(CE_Failure, CPLE_AppDefined, "Too many coefficients. Computation aborted.");
475 return 0;
476 }
477
478 double* _AA = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
479 double* _Ainv = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
480
481 if( _AA == NULL || _Ainv == NULL )
482 {
483 CPLError(CE_Failure, CPLE_AppDefined, "Out-of-memory while allocating temporary arrays. Computation aborted.");
484 VSIFree(_AA);
485 VSIFree(_Ainv);
486 return 0;
487 }
488
489 // Calc the values of the matrix A
490 for ( r = 0; r < 3; r++ )
491 for ( c = 0; c < 3; c++ )
492 A(r,c) = 0.0;
493
494 for ( c = 0; c < _nof_points; c++ )
495 {
496 A(0,c+3) = 1.0;
497 A(1,c+3) = x[c];
498 A(2,c+3) = y[c];
499
500 A(c+3,0) = 1.0;
501 A(c+3,1) = x[c];
502 A(c+3,2) = y[c];
503 }
504
505 for ( r = 0; r < _nof_points; r++ )
506 for ( c = r; c < _nof_points; c++ )
507 {
508 A(r+3,c+3) = VizGeorefSpline2DBase_func( x[r], y[r], x[c], y[c] );
509 if ( r != c )
510 A(c+3,r+3 ) = A(r+3,c+3);
511 }
512
513 #if VIZ_GEOREF_SPLINE_DEBUG
514
515 for ( r = 0; r < _nof_eqs; r++ )
516 {
517 for ( c = 0; c < _nof_eqs; c++ )
518 fprintf(stderr, "%f", A(r,c));
519 fprintf(stderr, "\n");
520 }
521
522 #endif
523
524 int ret = 4;
525 #ifdef HAVE_ARMADILLO
526 try
527 {
528 arma::mat matA(_AA,_nof_eqs,_nof_eqs,false);
529 arma::mat matRHS(_nof_eqs, _nof_vars);
530 int row, col;
531 for(row = 0; row < _nof_eqs; row++)
532 for(col = 0; col < _nof_vars; col++)
533 matRHS.at(row, col) = rhs[col][row];
534 arma::mat matCoefs(_nof_vars, _nof_eqs);
535 if( !arma::solve(matCoefs, matA, matRHS) )
536 {
537 CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
538 ret = 0;
539 }
540 else
541 {
542 for(row = 0; row < _nof_eqs; row++)
543 for(col = 0; col < _nof_vars; col++)
544 coef[col][row] = matCoefs.at(row, col);
545 }
546 }
547 catch(...)
548 {
549 CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
550 ret = 0;
551 }
552 #else
553 // Invert the matrix
554 int status = matrixInvert( _nof_eqs, _AA, _Ainv );
555
556 if ( !status )
557 {
558 CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
559 ret = 0;
560 }
561 else
562 {
563 // calc the coefs
564 for ( int v = 0; v < _nof_vars; v++ )
565 for ( r = 0; r < _nof_eqs; r++ )
566 {
567 coef[v][r] = 0.0;
568 for ( c = 0; c < _nof_eqs; c++ )
569 coef[v][r] += Ainv(r,c) * rhs[v][c];
570 }
571 }
572 #endif
573
574 VSIFree(_AA);
575 VSIFree(_Ainv);
576
577 return(ret);
578 }
579
get_point(const double Px,const double Py,double * vars)580 int VizGeorefSpline2D::get_point( const double Px, const double Py, double *vars )
581 {
582 int v, r;
583 double tmp, Pu;
584 double fact;
585 int leftP=0, rightP=0, found = 0;
586
587 switch ( type )
588 {
589 case VIZ_GEOREF_SPLINE_ZERO_POINTS :
590 for ( v = 0; v < _nof_vars; v++ )
591 vars[v] = 0.0;
592 break;
593 case VIZ_GEOREF_SPLINE_ONE_POINT :
594 for ( v = 0; v < _nof_vars; v++ )
595 vars[v] = rhs[v][3];
596 break;
597 case VIZ_GEOREF_SPLINE_TWO_POINTS :
598 fact = _dx * ( Px - x[0] ) + _dy * ( Py - y[0] );
599 for ( v = 0; v < _nof_vars; v++ )
600 vars[v] = ( 1 - fact ) * rhs[v][3] + fact * rhs[v][4];
601 break;
602 case VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL :
603 Pu = _dx * ( Px - x[0] ) + _dy * ( Py - y[0] );
604 if ( Pu <= u[index[0]] )
605 {
606 leftP = index[0];
607 rightP = index[1];
608 }
609 else if ( Pu >= u[index[_nof_points-1]] )
610 {
611 leftP = index[_nof_points-2];
612 rightP = index[_nof_points-1];
613 }
614 else
615 {
616 for ( r = 1; !found && r < _nof_points; r++ )
617 {
618 leftP = index[r-1];
619 rightP = index[r];
620 if ( Pu >= u[leftP] && Pu <= u[rightP] )
621 found = 1;
622 }
623 }
624
625 fact = ( Pu - u[leftP] ) / ( u[rightP] - u[leftP] );
626 for ( v = 0; v < _nof_vars; v++ )
627 vars[v] = ( 1.0 - fact ) * rhs[v][leftP+3] +
628 fact * rhs[v][rightP+3];
629 break;
630 case VIZ_GEOREF_SPLINE_FULL :
631 {
632 double Pxy[2] = { Px, Py };
633 for ( v = 0; v < _nof_vars; v++ )
634 vars[v] = coef[v][0] + coef[v][1] * Px + coef[v][2] * Py;
635
636 for ( r = 0; r < (_nof_points & (~3)); r+=4 )
637 {
638 double tmp[4];
639 VizGeorefSpline2DBase_func4( tmp, Pxy, &x[r], &y[r] );
640 for ( v= 0; v < _nof_vars; v++ )
641 vars[v] += coef[v][r+3] * tmp[0] +
642 coef[v][r+3+1] * tmp[1] +
643 coef[v][r+3+2] * tmp[2] +
644 coef[v][r+3+3] * tmp[3];
645 }
646 for ( ; r < _nof_points; r++ )
647 {
648 tmp = VizGeorefSpline2DBase_func( Px, Py, x[r], y[r] );
649 for ( v= 0; v < _nof_vars; v++ )
650 vars[v] += coef[v][r+3] * tmp;
651 }
652 break;
653 }
654 case VIZ_GEOREF_SPLINE_POINT_WAS_ADDED :
655 fprintf(stderr, " A point was added after the last solve\n");
656 fprintf(stderr, " NO interpolation - return values are zero\n");
657 for ( v = 0; v < _nof_vars; v++ )
658 vars[v] = 0.0;
659 return(0);
660 break;
661 case VIZ_GEOREF_SPLINE_POINT_WAS_DELETED :
662 fprintf(stderr, " A point was deleted after the last solve\n");
663 fprintf(stderr, " NO interpolation - return values are zero\n");
664 for ( v = 0; v < _nof_vars; v++ )
665 vars[v] = 0.0;
666 return(0);
667 break;
668 default :
669 return(0);
670 break;
671 }
672 return(1);
673 }
674
675 #ifndef HAVE_ARMADILLO
matrixInvert(int N,double input[],double output[])676 static int matrixInvert( int N, double input[], double output[] )
677 {
678 // Receives an array of dimension NxN as input. This is passed as a one-
679 // dimensional array of N-squared size. It produces the inverse of the
680 // input matrix, returned as output, also of size N-squared. The Gauss-
681 // Jordan Elimination method is used. (Adapted from a BASIC routine in
682 // "Basic Scientific Subroutines Vol. 1", courtesy of Scott Edwards.)
683
684 // Array elements 0...N-1 are for the first row, N...2N-1 are for the
685 // second row, etc.
686
687 // We need to have a temporary array of size N x 2N. We'll refer to the
688 // "left" and "right" halves of this array.
689
690 int row, col;
691
692 #if 0
693 fprintf(stderr, "Matrix Inversion input matrix (N=%d)\n", N);
694 for ( row=0; row<N; row++ )
695 {
696 for ( col=0; col<N; col++ )
697 {
698 fprintf(stderr, "%5.2f ", input[row*N + col ] );
699 }
700 fprintf(stderr, "\n");
701 }
702 #endif
703
704 int tempSize = 2 * N * N;
705 double* temp = (double*) new double[ tempSize ];
706 double ftemp;
707
708 if (temp == 0) {
709
710 CPLError(CE_Failure, CPLE_AppDefined, "matrixInvert(): ERROR - memory allocation failed.");
711 return false;
712 }
713
714 // First create a double-width matrix with the input array on the left
715 // and the identity matrix on the right.
716
717 for ( row=0; row<N; row++ )
718 {
719 for ( col=0; col<N; col++ )
720 {
721 // Our index into the temp array is X2 because it's twice as wide
722 // as the input matrix.
723
724 temp[ 2*row*N + col ] = input[ row*N+col ]; // left = input matrix
725 temp[ 2*row*N + col + N ] = 0.0f; // right = 0
726 }
727 temp[ 2*row*N + row + N ] = 1.0f; // 1 on the diagonal of RHS
728 }
729
730 // Now perform row-oriented operations to convert the left hand side
731 // of temp to the identity matrix. The inverse of input will then be
732 // on the right.
733
734 int max;
735 int k=0;
736 for (k = 0; k < N; k++)
737 {
738 if (k+1 < N) // if not on the last row
739 {
740 max = k;
741 for (row = k+1; row < N; row++) // find the maximum element
742 {
743 if (fabs( temp[row*2*N + k] ) > fabs( temp[max*2*N + k] ))
744 {
745 max = row;
746 }
747 }
748
749 if (max != k) // swap all the elements in the two rows
750 {
751 for (col=k; col<2*N; col++)
752 {
753 ftemp = temp[k*2*N + col];
754 temp[k*2*N + col] = temp[max*2*N + col];
755 temp[max*2*N + col] = ftemp;
756 }
757 }
758 }
759
760 ftemp = temp[ k*2*N + k ];
761 if ( ftemp == 0.0f ) // matrix cannot be inverted
762 {
763 delete[] temp;
764 return false;
765 }
766
767 for ( col=k; col<2*N; col++ )
768 {
769 temp[ k*2*N + col ] /= ftemp;
770 }
771
772 int i2 = k*2*N ;
773 for ( row=0; row<N; row++ )
774 {
775 if ( row != k )
776 {
777 int i1 = row*2*N;
778 ftemp = temp[ i1 + k ];
779 for ( col=k; col<2*N; col++ )
780 {
781 temp[ i1 + col ] -= ftemp * temp[ i2 + col ];
782 }
783 }
784 }
785 }
786
787 // Retrieve inverse from the right side of temp
788
789 for (row = 0; row < N; row++)
790 {
791 for (col = 0; col < N; col++)
792 {
793 output[row*N + col] = temp[row*2*N + col + N ];
794 }
795 }
796
797 #if 0
798 fprintf(stderr, "Matrix Inversion result matrix:\n");
799 for ( row=0; row<N; row++ )
800 {
801 for ( col=0; col<N; col++ )
802 {
803 fprintf(stderr, "%5.2f ", output[row*N + col ] );
804 }
805 fprintf(stderr, "\n");
806 }
807 #endif
808
809 delete [] temp; // free memory
810 return true;
811 }
812 #endif
813