1 : /******************************************************************************
2 : * $Id: thinplatespline.cpp 15547 2008-10-17 17:45:03Z 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 : *
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 : #include "thinplatespline.h"
34 :
35 : #ifdef HAVE_FLOAT_H
36 : # include <float.h>
37 : #elif defined(HAVE_VALUES_H)
38 : # include <values.h>
39 : #endif
40 :
41 : #ifndef FLT_MAX
42 : # define FLT_MAX 1e+37
43 : # define FLT_MIN 1e-37
44 : #endif
45 :
46 : VizGeorefSpline2D* viz_xy2llz;
47 : VizGeorefSpline2D* viz_llz2xy;
48 :
49 : /////////////////////////////////////////////////////////////////////////////////////
50 : //// vizGeorefSpline2D
51 : /////////////////////////////////////////////////////////////////////////////////////
52 :
53 : #define A(r,c) _AA[ _nof_eqs * (r) + (c) ]
54 : #define Ainv(r,c) _Ainv[ _nof_eqs * (r) + (c) ]
55 :
56 :
57 : #define VIZ_GEOREF_SPLINE_DEBUG 0
58 :
59 : int matrixInvert( int N, double input[], double output[] );
60 :
61 16 : void VizGeorefSpline2D::grow_points()
62 :
63 : {
64 16 : int new_max = _max_nof_points*2 + 2 + 3;
65 : int i;
66 :
67 16 : if( _max_nof_points == 0 )
68 : {
69 8 : x = (double *) VSIMalloc( sizeof(double) * new_max );
70 8 : y = (double *) VSIMalloc( sizeof(double) * new_max );
71 8 : u = (double *) VSIMalloc( sizeof(double) * new_max );
72 8 : unused = (int *) VSIMalloc( sizeof(int) * new_max );
73 8 : index = (int *) VSIMalloc( sizeof(int) * new_max );
74 24 : for( i = 0; i < VIZGEOREF_MAX_VARS; i++ )
75 : {
76 16 : rhs[i] = (double *) VSICalloc( sizeof(double), new_max );
77 16 : coef[i] = (double *) VSICalloc( sizeof(double), new_max );
78 : }
79 : }
80 : else
81 : {
82 8 : x = (double *) VSIRealloc( x, sizeof(double) * new_max );
83 8 : y = (double *) VSIRealloc( y, sizeof(double) * new_max );
84 8 : u = (double *) VSIRealloc( u, sizeof(double) * new_max );
85 8 : unused = (int *) VSIRealloc( unused, sizeof(int) * new_max );
86 8 : index = (int *) VSIRealloc( index, sizeof(int) * new_max );
87 24 : for( i = 0; i < VIZGEOREF_MAX_VARS; i++ )
88 : {
89 16 : rhs[i] = (double *)
90 16 : VSIRealloc( rhs[i], sizeof(double) * new_max );
91 16 : coef[i] = (double *)
92 16 : VSIRealloc( coef[i], sizeof(double) * new_max );
93 : }
94 : }
95 :
96 16 : _max_nof_points = new_max - 3;
97 16 : }
98 :
99 32 : int VizGeorefSpline2D::add_point( const double Px, const double Py, const double *Pvars )
100 : {
101 32 : type = VIZ_GEOREF_SPLINE_POINT_WAS_ADDED;
102 : int i;
103 :
104 32 : if( _nof_points == _max_nof_points )
105 8 : grow_points();
106 :
107 32 : i = _nof_points;
108 : //A new point is added
109 32 : x[i] = Px;
110 32 : y[i] = Py;
111 96 : for ( int j = 0; j < _nof_vars; j++ )
112 64 : rhs[j][i+3] = Pvars[j];
113 32 : _nof_points++;
114 32 : return 1;
115 : }
116 :
117 0 : bool VizGeorefSpline2D::change_point(int index, double Px, double Py, double* Pvars)
118 : {
119 0 : if ( index < _nof_points )
120 : {
121 0 : int i = index;
122 0 : x[i] = Px;
123 0 : y[i] = Py;
124 0 : for ( int j = 0; j < _nof_vars; j++ )
125 0 : rhs[j][i+3] = Pvars[j];
126 : }
127 :
128 0 : return( true );
129 : }
130 :
131 0 : bool VizGeorefSpline2D::get_xy(int index, double& outX, double& outY)
132 : {
133 : bool ok;
134 :
135 0 : if ( index < _nof_points )
136 : {
137 0 : ok = true;
138 0 : outX = x[index];
139 0 : outY = y[index];
140 : }
141 : else
142 : {
143 0 : ok = false;
144 0 : outX = outY = 0.0f;
145 : }
146 :
147 0 : return(ok);
148 : }
149 :
150 0 : int VizGeorefSpline2D::delete_point(const double Px, const double Py )
151 : {
152 0 : for ( int i = 0; i < _nof_points; i++ )
153 : {
154 0 : if ( ( fabs(Px - x[i]) <= _tx ) && ( fabs(Py - y[i]) <= _ty ) )
155 : {
156 0 : for ( int j = i; j < _nof_points - 1; j++ )
157 : {
158 0 : x[j] = x[j+1];
159 0 : y[j] = y[j+1];
160 0 : for ( int k = 0; k < _nof_vars; k++ )
161 0 : rhs[k][j+3] = rhs[k][j+3+1];
162 : }
163 0 : _nof_points--;
164 0 : type = VIZ_GEOREF_SPLINE_POINT_WAS_DELETED;
165 0 : return(1);
166 : }
167 : }
168 0 : return(0);
169 : }
170 :
171 8 : int VizGeorefSpline2D::solve(void)
172 : {
173 : int r, c, v;
174 : int p;
175 :
176 : // No points at all
177 8 : if ( _nof_points < 1 )
178 : {
179 0 : type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
180 0 : return(0);
181 : }
182 :
183 : // Only one point
184 8 : if ( _nof_points == 1 )
185 : {
186 0 : type = VIZ_GEOREF_SPLINE_ONE_POINT;
187 0 : return(1);
188 : }
189 : // Just 2 points - it is necessarily 1D case
190 8 : if ( _nof_points == 2 )
191 : {
192 0 : _dx = x[1] - x[0];
193 0 : _dy = y[1] - y[0];
194 0 : double fact = 1.0 / ( _dx * _dx + _dy * _dy );
195 0 : _dx *= fact;
196 0 : _dy *= fact;
197 :
198 0 : type = VIZ_GEOREF_SPLINE_TWO_POINTS;
199 0 : return(2);
200 : }
201 :
202 : // More than 2 points - first we have to check if it is 1D or 2D case
203 :
204 8 : double xmax = x[0], xmin = x[0], ymax = y[0], ymin = y[0];
205 : double delx, dely;
206 : double xx, yy;
207 8 : double sumx = 0.0f, sumy= 0.0f, sumx2 = 0.0f, sumy2 = 0.0f, sumxy = 0.0f;
208 : double SSxx, SSyy, SSxy;
209 :
210 40 : for ( p = 0; p < _nof_points; p++ )
211 : {
212 32 : xx = x[p];
213 32 : yy = y[p];
214 :
215 32 : xmax = MAX( xmax, xx );
216 32 : xmin = MIN( xmin, xx );
217 32 : ymax = MAX( ymax, yy );
218 32 : ymin = MIN( ymin, yy );
219 :
220 32 : sumx += xx;
221 32 : sumx2 += xx * xx;
222 32 : sumy += yy;
223 32 : sumy2 += yy * yy;
224 32 : sumxy += xx * yy;
225 : }
226 8 : delx = xmax - xmin;
227 8 : dely = ymax - ymin;
228 :
229 8 : SSxx = sumx2 - sumx * sumx / _nof_points;
230 8 : SSyy = sumy2 - sumy * sumy / _nof_points;
231 8 : SSxy = sumxy - sumx * sumy / _nof_points;
232 :
233 8 : if ( delx < 0.001 * dely || dely < 0.001 * delx ||
234 : fabs ( SSxy * SSxy / ( SSxx * SSyy ) ) > 0.99 )
235 : {
236 : int p1;
237 :
238 0 : type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL;
239 :
240 0 : _dx = _nof_points * sumx2 - sumx * sumx;
241 0 : _dy = _nof_points * sumy2 - sumy * sumy;
242 0 : double fact = 1.0 / sqrt( _dx * _dx + _dy * _dy );
243 0 : _dx *= fact;
244 0 : _dy *= fact;
245 :
246 0 : for ( p = 0; p < _nof_points; p++ )
247 : {
248 0 : double dxp = x[p] - x[0];
249 0 : double dyp = y[p] - y[0];
250 0 : u[p] = _dx * dxp + _dy * dyp;
251 0 : unused[p] = 1;
252 : }
253 :
254 0 : for ( p = 0; p < _nof_points; p++ )
255 : {
256 0 : int min_index = -1;
257 0 : double min_u = 0;
258 0 : for ( p1 = 0; p1 < _nof_points; p1++ )
259 : {
260 0 : if ( unused[p1] )
261 : {
262 0 : if ( min_index < 0 || u[p1] < min_u )
263 : {
264 0 : min_index = p1;
265 0 : min_u = u[p1];
266 : }
267 : }
268 : }
269 0 : index[p] = min_index;
270 0 : unused[min_index] = 0;
271 : }
272 :
273 0 : return(3);
274 : }
275 :
276 8 : type = VIZ_GEOREF_SPLINE_FULL;
277 : // Make the necessary memory allocations
278 8 : if ( _AA )
279 0 : CPLFree(_AA);
280 8 : if ( _Ainv )
281 0 : CPLFree(_Ainv);
282 :
283 8 : _nof_eqs = _nof_points + 3;
284 :
285 8 : _AA = ( double * )CPLCalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
286 8 : _Ainv = ( double * )CPLCalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
287 :
288 : // Calc the values of the matrix A
289 32 : for ( r = 0; r < 3; r++ )
290 96 : for ( c = 0; c < 3; c++ )
291 72 : A(r,c) = 0.0;
292 :
293 40 : for ( c = 0; c < _nof_points; c++ )
294 : {
295 32 : A(0,c+3) = 1.0;
296 32 : A(1,c+3) = x[c];
297 32 : A(2,c+3) = y[c];
298 :
299 32 : A(c+3,0) = 1.0;
300 32 : A(c+3,1) = x[c];
301 32 : A(c+3,2) = y[c];
302 : }
303 :
304 40 : for ( r = 0; r < _nof_points; r++ )
305 112 : for ( c = r; c < _nof_points; c++ )
306 : {
307 80 : A(r+3,c+3) = base_func( x[r], y[r], x[c], y[c] );
308 80 : if ( r != c )
309 48 : A(c+3,r+3 ) = A(r+3,c+3);
310 : }
311 :
312 : #if VIZ_GEOREF_SPLINE_DEBUG
313 :
314 : for ( r = 0; r < _nof_eqs; r++ )
315 : {
316 : for ( c = 0; c < _nof_eqs; c++ )
317 : fprintf(stderr, "%f", A(r,c));
318 : fprintf(stderr, "\n");
319 : }
320 :
321 : #endif
322 :
323 : // Invert the matrix
324 8 : int status = matrixInvert( _nof_eqs, _AA, _Ainv );
325 :
326 8 : if ( !status )
327 : {
328 0 : fprintf(stderr, " There is a problem to invert the interpolation matrix\n");
329 0 : return 0;
330 : }
331 :
332 : // calc the coefs
333 24 : for ( v = 0; v < _nof_vars; v++ )
334 128 : for ( r = 0; r < _nof_eqs; r++ )
335 : {
336 112 : coef[v][r] = 0.0;
337 896 : for ( c = 0; c < _nof_eqs; c++ )
338 784 : coef[v][r] += Ainv(r,c) * rhs[v][c];
339 : }
340 :
341 8 : return(4);
342 : }
343 :
344 1725 : int VizGeorefSpline2D::get_point( const double Px, const double Py, double *vars )
345 : {
346 : int v, r;
347 : double tmp, Pu;
348 : double fact;
349 1725 : int leftP=0, rightP=0, found = 0;
350 :
351 1725 : switch ( type )
352 : {
353 : case VIZ_GEOREF_SPLINE_ZERO_POINTS :
354 0 : for ( v = 0; v < _nof_vars; v++ )
355 0 : vars[v] = 0.0;
356 0 : break;
357 : case VIZ_GEOREF_SPLINE_ONE_POINT :
358 0 : for ( v = 0; v < _nof_vars; v++ )
359 0 : vars[v] = rhs[v][3];
360 0 : break;
361 : case VIZ_GEOREF_SPLINE_TWO_POINTS :
362 0 : fact = _dx * ( Px - x[0] ) + _dy * ( Py - y[0] );
363 0 : for ( v = 0; v < _nof_vars; v++ )
364 0 : vars[v] = ( 1 - fact ) * rhs[v][3] + fact * rhs[v][4];
365 0 : break;
366 : case VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL :
367 0 : Pu = _dx * ( Px - x[0] ) + _dy * ( Py - y[0] );
368 0 : if ( Pu <= u[index[0]] )
369 : {
370 0 : leftP = index[0];
371 0 : rightP = index[1];
372 : }
373 0 : else if ( Pu >= u[index[_nof_points-1]] )
374 : {
375 0 : leftP = index[_nof_points-2];
376 0 : rightP = index[_nof_points-1];
377 : }
378 : else
379 : {
380 0 : for ( r = 1; !found && r < _nof_points; r++ )
381 : {
382 0 : leftP = index[r-1];
383 0 : rightP = index[r];
384 0 : if ( Pu >= u[leftP] && Pu <= u[rightP] )
385 0 : found = 1;
386 : }
387 : }
388 :
389 0 : fact = ( Pu - u[leftP] ) / ( u[rightP] - u[leftP] );
390 0 : for ( v = 0; v < _nof_vars; v++ )
391 0 : vars[v] = ( 1.0 - fact ) * rhs[v][leftP+3] +
392 0 : fact * rhs[v][rightP+3];
393 0 : break;
394 : case VIZ_GEOREF_SPLINE_FULL :
395 5175 : for ( v = 0; v < _nof_vars; v++ )
396 3450 : vars[v] = coef[v][0] + coef[v][1] * Px + coef[v][2] * Py;
397 :
398 8625 : for ( r = 0; r < _nof_points; r++ )
399 : {
400 6900 : tmp = base_func( Px, Py, x[r], y[r] );
401 20700 : for ( v= 0; v < _nof_vars; v++ )
402 13800 : vars[v] += coef[v][r+3] * tmp;
403 : }
404 1725 : break;
405 : case VIZ_GEOREF_SPLINE_POINT_WAS_ADDED :
406 0 : fprintf(stderr, " A point was added after the last solve\n");
407 0 : fprintf(stderr, " NO interpolation - return values are zero\n");
408 0 : for ( v = 0; v < _nof_vars; v++ )
409 0 : vars[v] = 0.0;
410 0 : return(0);
411 : break;
412 : case VIZ_GEOREF_SPLINE_POINT_WAS_DELETED :
413 0 : fprintf(stderr, " A point was deleted after the last solve\n");
414 0 : fprintf(stderr, " NO interpolation - return values are zero\n");
415 0 : for ( v = 0; v < _nof_vars; v++ )
416 0 : vars[v] = 0.0;
417 0 : return(0);
418 : break;
419 : default :
420 0 : return(0);
421 : break;
422 : }
423 1725 : return(1);
424 : }
425 :
426 6980 : double VizGeorefSpline2D::base_func( const double x1, const double y1,
427 : const double x2, const double y2 )
428 : {
429 6980 : if ( ( x1 == x2 ) && (y1 == y2 ) )
430 48 : return 0.0;
431 :
432 6932 : double dist = ( x2 - x1 ) * ( x2 - x1 ) + ( y2 - y1 ) * ( y2 - y1 );
433 :
434 6932 : return dist * log( dist );
435 : }
436 :
437 8 : int matrixInvert( int N, double input[], double output[] )
438 : {
439 : // Receives an array of dimension NxN as input. This is passed as a one-
440 : // dimensional array of N-squared size. It produces the inverse of the
441 : // input matrix, returned as output, also of size N-squared. The Gauss-
442 : // Jordan Elimination method is used. (Adapted from a BASIC routine in
443 : // "Basic Scientific Subroutines Vol. 1", courtesy of Scott Edwards.)
444 :
445 : // Array elements 0...N-1 are for the first row, N...2N-1 are for the
446 : // second row, etc.
447 :
448 : // We need to have a temporary array of size N x 2N. We'll refer to the
449 : // "left" and "right" halves of this array.
450 :
451 : int row, col;
452 :
453 : #if 0
454 : fprintf(stderr, "Matrix Inversion input matrix (N=%d)\n", N);
455 : for ( row=0; row<N; row++ )
456 : {
457 : for ( col=0; col<N; col++ )
458 : {
459 : fprintf(stderr, "%5.2f ", input[row*N + col ] );
460 : }
461 : fprintf(stderr, "\n");
462 : }
463 : #endif
464 :
465 8 : int tempSize = 2 * N * N;
466 8 : double* temp = (double*) new double[ tempSize ];
467 : double ftemp;
468 :
469 8 : if (temp == 0) {
470 :
471 0 : fprintf(stderr, "matrixInvert(): ERROR - memory allocation failed.\n");
472 0 : return false;
473 : }
474 :
475 : // First create a double-width matrix with the input array on the left
476 : // and the identity matrix on the right.
477 :
478 64 : for ( row=0; row<N; row++ )
479 : {
480 448 : for ( col=0; col<N; col++ )
481 : {
482 : // Our index into the temp array is X2 because it's twice as wide
483 : // as the input matrix.
484 :
485 392 : temp[ 2*row*N + col ] = input[ row*N+col ]; // left = input matrix
486 392 : temp[ 2*row*N + col + N ] = 0.0f; // right = 0
487 : }
488 56 : temp[ 2*row*N + row + N ] = 1.0f; // 1 on the diagonal of RHS
489 : }
490 :
491 : // Now perform row-oriented operations to convert the left hand side
492 : // of temp to the identity matrix. The inverse of input will then be
493 : // on the right.
494 :
495 : int max;
496 8 : int k=0;
497 64 : for (k = 0; k < N; k++)
498 : {
499 56 : if (k+1 < N) // if not on the last row
500 : {
501 48 : max = k;
502 216 : for (row = k+1; row < N; row++) // find the maximum element
503 : {
504 168 : if (fabs( temp[row*2*N + k] ) > fabs( temp[max*2*N + k] ))
505 : {
506 44 : max = row;
507 : }
508 : }
509 :
510 48 : if (max != k) // swap all the elements in the two rows
511 : {
512 506 : for (col=k; col<2*N; col++)
513 : {
514 467 : ftemp = temp[k*2*N + col];
515 467 : temp[k*2*N + col] = temp[max*2*N + col];
516 467 : temp[max*2*N + col] = ftemp;
517 : }
518 : }
519 : }
520 :
521 56 : ftemp = temp[ k*2*N + k ];
522 56 : if ( ftemp == 0.0f ) // matrix cannot be inverted
523 : {
524 0 : delete[] temp;
525 0 : return false;
526 : }
527 :
528 672 : for ( col=k; col<2*N; col++ )
529 : {
530 616 : temp[ k*2*N + col ] /= ftemp;
531 : }
532 :
533 448 : for ( row=0; row<N; row++ )
534 : {
535 392 : if ( row != k )
536 : {
537 336 : ftemp = temp[ row*2*N + k ];
538 4032 : for ( col=k; col<2*N; col++ )
539 : {
540 3696 : temp[ row*2*N + col ] -= ftemp * temp[ k*2*N + col ];
541 : }
542 : }
543 : }
544 : }
545 :
546 : // Retrieve inverse from the right side of temp
547 :
548 64 : for (row = 0; row < N; row++)
549 : {
550 448 : for (col = 0; col < N; col++)
551 : {
552 392 : output[row*N + col] = temp[row*2*N + col + N ];
553 : }
554 : }
555 :
556 : #if 0
557 : fprintf(stderr, "Matrix Inversion result matrix:\n");
558 : for ( row=0; row<N; row++ )
559 : {
560 : for ( col=0; col<N; col++ )
561 : {
562 : fprintf(stderr, "%5.2f ", output[row*N + col ] );
563 : }
564 : fprintf(stderr, "\n");
565 : }
566 : #endif
567 :
568 8 : delete [] temp; // free memory
569 8 : return true;
570 : }
|
