1 /* solve and invert matrices
2 *
3 * Author: Tom Vajzovic
4 * Copyright: 2006, Tom Vajzovic
5 * Written on: 2006-09-08
6 *
7 * undated:
8 * - page 43-45 of numerical recipes in C 1998
9 *
10 * 2006-09-08 tcv:
11 * - complete rewrite; algorithm unchanged
12 *
13 * 22/10/10
14 * - gtkdoc
15 */
16
17 /*
18
19 This file is part of VIPS.
20
21 VIPS is free software; you can redistribute it and/or modify
22 it under the terms of the GNU Lesser General Public License as published by
23 the Free Software Foundation; either version 2 of the License, or
24 (at your option) any later version.
25
26 This program is distributed in the hope that it will be useful,
27 but WITHOUT ANY WARRANTY; without even the implied warranty of
28 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
29 GNU Lesser General Public License for more details.
30
31 You should have received a copy of the GNU Lesser General Public License
32 along with this program; if not, write to the Free Software
33 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
34 02110-1301 USA
35
36 */
37
38 /*
39
40 These files are distributed with VIPS - http://www.vips.ecs.soton.ac.uk
41
42 */
43
44 /** HEADERS **/
45
46 #ifdef HAVE_CONFIG_H
47 #include <config.h>
48 #endif /*HAVE_CONFIG_H*/
49 #include <vips/intl.h>
50
51 #include <float.h>
52 #include <math.h>
53 #include <string.h>
54 #include <stdlib.h>
55 #include <stdio.h>
56 #include <vips/vips.h>
57 #include <vips/vips7compat.h>
58
59
60 /** CONSTANTS **/
61
62 #define TOO_SMALL ( 2.0 * DBL_MIN )
63 /* DBL_MIN is smallest *normalized* double precision float */
64
65
66 /** MACROS **/
67
68 #define MATRIX( mask, i, j ) ( (mask)-> coeff[ (j) + (i) * (mask)-> xsize ] )
69 /* use DOUBLEMASK or INTMASK as matrix type */
70
71
72 /** LOCAL FUNCTION DECLARATIONS **/
73
74 static int
75 mat_inv_lu(
76 DOUBLEMASK *inv,
77 const DOUBLEMASK *lu
78 );
79
80 static int
81 mat_inv_direct(
82 DOUBLEMASK *inv,
83 const DOUBLEMASK *mat,
84 const char *function_name
85 );
86
87
88 /** EXPORTED FUNCTION DEFINITIONS **/
89
90 /**
91 * im_lu_decomp:
92 * @mat: matrix to decompose
93 * @filename: name for output matrix
94 *
95 * This function takes any square NxN #DOUBLEMASK.
96 * It returns a #DOUBLEMASK which is (N+1)xN.
97 *
98 * It calculates the PLU decomposition, storing the upper and diagonal parts
99 * of U, together with the lower parts of L, as an NxN matrix in the first
100 * N rows of the new matrix. The diagonal parts of L are all set to unity
101 * and are not stored.
102 *
103 * The final row of the new #DOUBLEMASK has only integer entries, which
104 * represent the row-wise permutations made by the permuatation matrix P.
105 *
106 * The scale and offset members of the input #DOUBLEMASK are ignored.
107 *
108 * See:
109 *
110 * PRESS, W. et al, 1992. Numerical Recipies in C; The Art of Scientific
111 * Computing, 2nd ed. Cambridge: Cambridge University Press, pp. 43-50.
112 *
113 * See also: im_mattrn(), im_matinv().
114 *
115 * Returns: the decomposed matrix on success, or NULL on error.
116 */
117
118 DOUBLEMASK *
im_lu_decomp(const DOUBLEMASK * mat,const char * name)119 im_lu_decomp(
120 const DOUBLEMASK *mat,
121 const char *name
122 ){
123 #define FUNCTION_NAME "im_lu_decomp"
124
125 int i, j, k;
126 double *row_scale;
127 DOUBLEMASK *lu;
128
129 if( mat-> xsize != mat-> ysize ){
130 im_error( FUNCTION_NAME, "non-square matrix" );
131 return NULL;
132 }
133 #define N ( mat -> xsize )
134
135 lu= im_create_dmask( name, N, N + 1 );
136 row_scale= IM_ARRAY( NULL, N, double );
137
138 if( ! row_scale || ! lu ){
139 im_free_dmask( lu );
140 im_free( row_scale );
141 return NULL;
142 }
143 /* copy all coefficients and then perform decomposition in-place */
144
145 memcpy( lu-> coeff, mat-> coeff, N * N * sizeof( double ) );
146
147 #define LU( i, j ) MATRIX( lu, (i), (j) )
148 #define perm ( lu-> coeff + N * N )
149
150 for( i= 0; i < N; ++i ){
151
152 row_scale[ i ]= 0.0;
153
154 for( j= 0; j < N; ++j ){
155 double abs_val= fabs( LU( i, j ) );
156
157 /* find largest in each ROW */
158
159 if( abs_val > row_scale[ i ] )
160 row_scale[ i ]= abs_val;
161 }
162 if( ! row_scale[ i ] ){
163 im_error( FUNCTION_NAME, "singular matrix" );
164 im_free_dmask( lu );
165 im_free( row_scale );
166 return NULL;
167 }
168 /* fill array with scaling factors for each ROW */
169
170 row_scale[ i ]= 1.0 / row_scale[ i ];
171 }
172 for( j= 0; j < N; ++j ){ /* loop over COLs */
173 double max= -1.0;
174 int i_of_max;
175
176 /* not needed, but stops a compiler warning */
177 i_of_max= 0;
178
179 /* loop over ROWS in upper-half, except diagonal */
180
181 for( i= 0; i < j; ++i )
182 for( k= 0; k < i; ++k )
183 LU( i, j )-= LU( i, k ) * LU( k, j );
184
185 /* loop over ROWS in diagonal and lower-half */
186
187 for( i= j; i < N; ++i ){
188 double abs_val;
189
190 for( k= 0; k < j; ++k )
191 LU( i, j )-= LU( i, k ) * LU( k, j );
192
193 /* find largest element in each COLUMN scaled so that */
194 /* largest in each ROW is 1.0 */
195
196 abs_val= row_scale[ i ] * fabs( LU( i, j ) );
197
198 if( abs_val > max ){
199 max= abs_val;
200 i_of_max= i;
201 }
202 }
203 if( fabs( LU( i_of_max, j ) ) < TOO_SMALL ){
204 /* divisor is near zero */
205 im_error( FUNCTION_NAME, "singular or near-singular matrix" );
206 im_free_dmask( lu );
207 im_free( row_scale );
208 return NULL;
209 }
210 if( i_of_max != j ){
211 /* swap ROWS */
212
213 for( k= 0; k < N; ++k ){
214 double temp= LU( j, k );
215 LU( j, k )= LU( i_of_max, k );
216 LU( i_of_max, k )= temp;
217 }
218 row_scale[ i_of_max ]= row_scale[ j ];
219 /* no need to copy this scale back up - we won't use it */
220 }
221 /* record permutation */
222 perm[ j ]= i_of_max;
223
224 /* divide by best (largest scaled) pivot found */
225 for( i= j + 1; i < N; ++i )
226 LU( i, j )/= LU( j, j );
227 }
228 im_free( row_scale );
229
230 return lu;
231
232 #undef N
233 #undef LU
234 #undef perm
235 #undef FUNCTION_NAME
236 }
237
238 /**
239 * im_lu_solve:
240 * @lu: matrix to solve
241 * @vec: name for output matrix
242 *
243 * Solve the system of linear equations Ax=b, where matrix A has already
244 * been decomposed into LU form in DOUBLEMASK *lu. Input vector b is in
245 * vec and is overwritten with vector x.
246 *
247 * See:
248 *
249 * PRESS, W. et al, 1992. Numerical Recipies in C; The Art of Scientific
250 * Computing, 2nd ed. Cambridge: Cambridge University Press, pp. 43-50.
251 *
252 * See also: im_mattrn(), im_matinv().
253 *
254 * Returns: the decomposed matrix on success, or NULL on error.
255 */
256
257 int
im_lu_solve(const DOUBLEMASK * lu,double * vec)258 im_lu_solve(
259 const DOUBLEMASK *lu,
260 double *vec
261 ){
262 #define FUNCTION_NAME "im_lu_solve"
263 int i, j;
264
265 if( lu-> xsize + 1 != lu-> ysize ){
266 im_error( FUNCTION_NAME, "not an LU decomposed matrix" );
267 return -1;
268 }
269 #define N ( lu -> xsize )
270 #define LU( i, j ) MATRIX( lu, (i), (j) )
271 #define perm ( lu-> coeff + N * N )
272
273 for( i= 0; i < N; ++i ){
274 int i_perm= perm[ i ];
275
276 if( i_perm != i ){
277 double temp= vec[ i ];
278 vec[ i ]= vec[ i_perm ];
279 vec[ i_perm ]= temp;
280 }
281 for( j= 0; j < i; ++j )
282 vec[ i ]-= LU( i, j ) * vec [ j ];
283 }
284
285 for( i= N - 1; i >= 0; --i ){
286
287 for( j= i + 1; j < N; ++j )
288 vec[ i ]-= LU( i, j ) * vec [ j ];
289
290 vec[ i ]/= LU( i, i );
291 }
292 return 0;
293
294 #undef LU
295 #undef perm
296 #undef N
297 #undef FUNCTION_NAME
298 }
299
300 /**
301 * im_matinv:
302 * @mat: matrix to invert
303 * @filename: name for output matrix
304 *
305 * Allocate, and return a pointer to, a DOUBLEMASK representing the
306 * inverse of the matrix represented in @mat. Give it the filename
307 * member @filename. Returns NULL on error. Scale and offset are ignored.
308 *
309 * See also: im_mattrn().
310 *
311 * Returns: the inverted matrix on success, or %NULL on error.
312 */
313 DOUBLEMASK *
im_matinv(const DOUBLEMASK * mat,const char * filename)314 im_matinv(
315 const DOUBLEMASK *mat,
316 const char *filename
317 ){
318 #define FUNCTION_NAME "im_matinv"
319
320 DOUBLEMASK *inv;
321
322 if( mat-> xsize != mat-> ysize ){
323 im_error( FUNCTION_NAME, "non-square matrix" );
324 return NULL;
325 }
326 #define N ( mat -> xsize )
327 inv= im_create_dmask( filename, N, N );
328 if( ! inv )
329 return NULL;
330
331 if( N < 4 ){
332 if( mat_inv_direct( inv, (const DOUBLEMASK *) mat, FUNCTION_NAME ) ){
333 im_free_dmask( inv );
334 return NULL;
335 }
336 return inv;
337 }
338 else {
339 DOUBLEMASK *lu= im_lu_decomp( mat, "temp" );
340
341 if( ! lu || mat_inv_lu( inv, (const DOUBLEMASK*) lu ) ){
342 im_free_dmask( lu );
343 im_free_dmask( inv );
344 return NULL;
345 }
346 im_free_dmask( lu );
347
348 return inv;
349 }
350 #undef N
351 #undef FUNCTION_NAME
352 }
353
354 /**
355 * im_matinv_inplace:
356 * @mat: matrix to invert
357 *
358 * Invert the matrix represented by the DOUBLEMASK @mat, and store
359 * it in the place of @mat. Scale and offset
360 * are ignored.
361 *
362 * See also: im_mattrn().
363 *
364 * Returns: 0 on success, or -1 on error.
365 */
366 int
im_matinv_inplace(DOUBLEMASK * mat)367 im_matinv_inplace(
368 DOUBLEMASK *mat
369 ){
370 #define FUNCTION_NAME "im_matinv_inplace"
371 int to_return= 0;
372
373 if( mat-> xsize != mat-> ysize ){
374 im_error( FUNCTION_NAME, "non-square matrix" );
375 return -1;
376 }
377 #define N ( mat -> xsize )
378 if( N < 4 ){
379 DOUBLEMASK *dup= im_dup_dmask( mat, "temp" );
380 if( ! dup )
381 return -1;
382
383 to_return= mat_inv_direct( mat, (const DOUBLEMASK*) dup, FUNCTION_NAME );
384
385 im_free_dmask( dup );
386
387 return to_return;
388 }
389 {
390 DOUBLEMASK *lu;
391
392 lu= im_lu_decomp( mat, "temp" );
393
394 if( ! lu || mat_inv_lu( mat, (const DOUBLEMASK*) lu ) )
395 to_return= -1;
396
397 im_free_dmask( lu );
398
399 return to_return;
400 }
401 #undef N
402 #undef FUNCTION_NAME
403 }
404
405 /* Invert a square size x size matrix stored in matrix[][]
406 * result is returned in the same matrix
407 */
408 int
im_invmat(double ** matrix,int size)409 im_invmat(
410 double **matrix,
411 int size
412 ){
413
414 DOUBLEMASK *mat= im_create_dmask( "temp", size, size );
415 int i;
416 int to_return= 0;
417
418 for( i= 0; i < size; ++i )
419 memcpy( mat-> coeff + i * size, matrix[ i ], size * sizeof( double ) );
420
421 to_return= im_matinv_inplace( mat );
422
423 if( ! to_return )
424 for( i= 0; i < size; ++i )
425 memcpy( matrix[ i ], mat-> coeff + i * size, size * sizeof( double ) );
426
427 im_free_dmask( mat );
428
429 return to_return;
430 }
431
432
433 /** LOCAL FUNCTION DEFINITIONS **/
434
435 static int
mat_inv_lu(DOUBLEMASK * inv,const DOUBLEMASK * lu)436 mat_inv_lu(
437 DOUBLEMASK *inv,
438 const DOUBLEMASK *lu
439 ){
440 #define N ( lu-> xsize )
441 #define INV( i, j ) MATRIX( inv, (i), (j) )
442
443 int i, j;
444 double *vec= IM_ARRAY( NULL, N, double );
445
446 if( ! vec )
447 return -1;
448
449 for( j= 0; j < N; ++j ){
450
451 for( i= 0; i < N; ++i )
452 vec[ i ]= 0.0;
453
454 vec[ j ]= 1.0;
455
456 im_lu_solve( lu, vec );
457
458 for( i= 0; i < N; ++i )
459 INV( i, j )= vec[ i ];
460 }
461 im_free( vec );
462
463 inv-> scale= 1.0;
464 inv-> offset= 0.0;
465
466 return 0;
467
468 #undef N
469 #undef INV
470 }
471
472 static int
mat_inv_direct(DOUBLEMASK * inv,const DOUBLEMASK * mat,const char * function_name)473 mat_inv_direct(
474 DOUBLEMASK *inv,
475 const DOUBLEMASK *mat,
476 const char *function_name
477 ){
478 #define N ( mat -> xsize )
479 #define MAT( i, j ) MATRIX( mat, (i), (j) )
480 #define INV( i, j ) MATRIX( inv, (i), (j) )
481
482 inv-> scale= 1.0;
483 inv-> offset= 0.0;
484
485 switch( N ){
486 case 1: {
487 if( fabs( MAT( 0, 0 ) ) < TOO_SMALL ){
488 im_error( function_name, "singular or near-singular matrix" );
489 return -1;
490 }
491 INV( 0, 0 )= 1.0 / MAT( 0, 0 );
492 return 0;
493 }
494 case 2: {
495 double det= MAT( 0, 0 ) * MAT( 1, 1 ) - MAT( 0, 1 ) * MAT( 1, 0 );
496
497 if( fabs( det ) < TOO_SMALL ){
498 im_error( function_name, "singular or near-singular matrix" );
499 return -1;
500 }
501 INV( 0, 0 )= MAT( 1, 1 ) / det;
502 INV( 0, 1 )= -MAT( 0, 1 ) / det;
503 INV( 1, 0 )= -MAT( 1, 0 ) / det;
504 INV( 1, 1 )= MAT( 0, 0 ) / det;
505
506 return 0;
507 }
508 case 3: {
509 double det= MAT( 0, 0 ) * ( MAT( 1, 1 ) * MAT( 2, 2 ) - MAT( 1, 2 ) * MAT( 2, 1 ) )
510 - MAT( 0, 1 ) * ( MAT( 1, 0 ) * MAT( 2, 2 ) - MAT( 1, 2 ) * MAT( 2, 0 ) )
511 + MAT( 0, 2 ) * ( MAT( 1, 0 ) * MAT( 2, 1 ) - MAT( 1, 1 ) * MAT( 2, 0 ) );
512
513 if( fabs( det ) < TOO_SMALL ){
514 im_error( function_name, "singular or near-singular matrix" );
515 return -1;
516 }
517 INV( 0, 0 )= ( MAT( 1, 1 ) * MAT( 2, 2 ) - MAT( 1, 2 ) * MAT( 2, 1 ) ) / det;
518 INV( 0, 1 )= ( MAT( 0, 2 ) * MAT( 2, 1 ) - MAT( 0, 1 ) * MAT( 2, 2 ) ) / det;
519 INV( 0, 2 )= ( MAT( 0, 1 ) * MAT( 1, 2 ) - MAT( 0, 2 ) * MAT( 1, 1 ) ) / det;
520
521 INV( 1, 0 )= ( MAT( 1, 2 ) * MAT( 2, 0 ) - MAT( 1, 0 ) * MAT( 2, 2 ) ) / det;
522 INV( 1, 1 )= ( MAT( 0, 0 ) * MAT( 2, 2 ) - MAT( 0, 2 ) * MAT( 2, 0 ) ) / det;
523 INV( 1, 2 )= ( MAT( 0, 2 ) * MAT( 1, 0 ) - MAT( 0, 0 ) * MAT( 1, 2 ) ) / det;
524
525 INV( 2, 0 )= ( MAT( 1, 0 ) * MAT( 2, 1 ) - MAT( 1, 1 ) * MAT( 2, 0 ) ) / det;
526 INV( 2, 1 )= ( MAT( 0, 1 ) * MAT( 2, 0 ) - MAT( 0, 0 ) * MAT( 2, 1 ) ) / det;
527 INV( 2, 2 )= ( MAT( 0, 0 ) * MAT( 1, 1 ) - MAT( 0, 1 ) * MAT( 1, 0 ) ) / det;
528
529 return 0;
530 }
531 default:
532 return -1;
533 }
534
535 #undef N
536 #undef MAT
537 #undef INV
538 }
539
540