| /* @(#) Allocate, and return a pointer to, a DOUBLEMASK representing the LU |
| * @(#) decomposition of the matrix in DOUBLEMASK *mat. Give it the filename |
| * @(#) member *name. Returns NULL on error. Scale and offset are ignored. |
| * @(#) |
| * @(#) DOUBLEMASK *im_lu_decomp( const DOUBLEMASK *mat, const char *name ); |
| * @(#) |
| * @(#) Solve the system of linear equations Ax=b, where matrix A has already |
| * @(#) been decomposed into LU form in DOUBLEMASK *lu. Input vector b is in |
| * @(#) vec and is overwritten with vector x. |
| * @(#) |
| * @(#) int im_lu_solve( const DOUBLEMASK *lu, double *vec ); |
| * @(#) |
| * @(#) Allocate, and return a pointer to, a DOUBLEMASK representing the |
| * @(#) inverse of the matrix represented in mat. Give it the filename |
| * @(#) member *name. Returns NULL on error. Scale and offset are ignored. |
| * @(#) |
| * @(#) DOUBLEMASK *im_matinv( const DOUBLEMASK *mat, const char *name ); |
| * @(#) |
| * @(#) Invert the matrix represented by the DOUBLEMASK *mat, and store |
| * @(#) it in the place of *mat. Returns -1 on error. Scale and offset |
| * @(#) are ignored. |
| * @(#) |
| * @(#) int im_matinv_inplace( DOUBLEMASK *mat ); |
| * |
| * Author: Tom Vajzovic |
| * Copyright: 2006, Tom Vajzovic |
| * Written on: 2006-09-08 |
| * |
| * undated: |
| * - page 43-45 of numerical recipes in C 1998 |
| * |
| * 2006-09-08 tcv: |
| * - complete rewrite; algorithm unchanged |
| */ |
| |
| /* |
| |
| This file is part of VIPS. |
| |
| VIPS is free software; you can redistribute it and/or modify |
| it under the terms of the GNU Lesser General Public License as published by |
| the Free Software Foundation; either version 2 of the License, or |
| (at your option) any later version. |
| |
| This program is distributed in the hope that it will be useful, |
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| GNU Lesser General Public License for more details. |
| |
| You should have received a copy of the GNU Lesser General Public License |
| along with this program; if not, write to the Free Software |
| Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
| |
| */ |
| |
| /* |
| |
| These files are distributed with VIPS - http://www.vips.ecs.soton.ac.uk |
| |
| */ |
| |
| /** HEADERS **/ |
| |
| #ifdef HAVE_CONFIG_H |
| #include <config.h> |
| #endif /*HAVE_CONFIG_H*/ |
| #include <vips/intl.h> |
| |
| #include <float.h> |
| #include <math.h> |
| #include <string.h> |
| #include <stdlib.h> |
| #include <stdio.h> |
| #include <vips/vips.h> |
| |
| #ifdef WITH_DMALLOC |
| #include <dmalloc.h> |
| #endif /*WITH_DMALLOC*/ |
| |
| |
| /** CONSTANTS **/ |
| |
| #define TOO_SMALL ( 2.0 * DBL_MIN ) |
| /* DBL_MIN is smallest *normalized* double precision float */ |
| |
| |
| /** MACROS **/ |
| |
| #define MATRIX( mask, i, j ) ( (mask)-> coeff[ (j) + (i) * (mask)-> xsize ] ) |
| /* use DOUBLEMASK or INTMASK as matrix type */ |
| |
| |
| /** LOCAL FUNCTION DECLARATIONS **/ |
| |
| static int |
| mat_inv_lu( |
| DOUBLEMASK *inv, |
| const DOUBLEMASK *lu |
| ); |
| |
| static int |
| mat_inv_direct( |
| DOUBLEMASK *inv, |
| const DOUBLEMASK *mat, |
| const char *function_name |
| ); |
| |
| |
| /** EXPORTED FUNCTION DEFINITIONS **/ |
| |
| DOUBLEMASK * |
| im_lu_decomp( |
| const DOUBLEMASK *mat, |
| const char *name |
| ){ |
| #define FUNCTION_NAME "im_lu_decomp" |
| /* This function takes any square NxN DOUBLEMASK treats it as a matrix. |
| * It allocates a DOUBLEMASK which is (N+1)xN. |
| * |
| * It calculates the PLU decomposition, storing the upper and diagonal parts |
| * of U, together with the lower parts of L, as an NxN matrix in the first |
| * N rows of the new matrix. The diagonal parts of L are all set to unity |
| * and are not stored. |
| * |
| * The final row of the new DOUBLEMASK has only integer entries, which |
| * represent the row-wise permutations made by the permuatation matrix P. |
| * |
| * The scale and offset members of the input DOUBLEMASK are ignored. |
| * |
| * See: |
| * PRESS, W. et al, 1992. Numerical Recipies in C; The Art of Scientific |
| * Computing, 2nd ed. Cambridge: Cambridge University Press, pp. 43-50. |
| */ |
| int i, j, k; |
| double *row_scale; |
| DOUBLEMASK *lu; |
| |
| if( mat-> xsize != mat-> ysize ){ |
| im_error( FUNCTION_NAME, "non-square matrix" ); |
| return NULL; |
| } |
| #define N ( mat -> xsize ) |
| |
| lu= im_create_dmask( name, N, N + 1 ); |
| row_scale= IM_ARRAY( NULL, N, double ); |
| |
| if( ! row_scale || ! lu ){ |
| im_free_dmask( lu ); |
| im_free( row_scale ); |
| return NULL; |
| } |
| /* copy all coefficients and then perform decomposition in-place */ |
| |
| memcpy( lu-> coeff, mat-> coeff, N * N * sizeof( double ) ); |
| |
| #define LU( i, j ) MATRIX( lu, (i), (j) ) |
| #define perm ( lu-> coeff + N * N ) |
| |
| for( i= 0; i < N; ++i ){ |
| |
| row_scale[ i ]= 0.0; |
| |
| for( j= 0; j < N; ++j ){ |
| double abs_val= fabs( LU( i, j ) ); |
| |
| /* find largest in each ROW */ |
| |
| if( abs_val > row_scale[ i ] ) |
| row_scale[ i ]= abs_val; |
| } |
| if( ! row_scale[ i ] ){ |
| im_error( FUNCTION_NAME, "singular matrix" ); |
| im_free_dmask( lu ); |
| im_free( row_scale ); |
| return NULL; |
| } |
| /* fill array with scaling factors for each ROW */ |
| |
| row_scale[ i ]= 1.0 / row_scale[ i ]; |
| } |
| for( j= 0; j < N; ++j ){ /* loop over COLs */ |
| double max= -1.0; |
| int i_of_max; |
| |
| /* not needed, but stops a compiler warning */ |
| i_of_max= 0; |
| |
| /* loop over ROWS in upper-half, except diagonal */ |
| |
| for( i= 0; i < j; ++i ) |
| for( k= 0; k < i; ++k ) |
| LU( i, j )-= LU( i, k ) * LU( k, j ); |
| |
| /* loop over ROWS in diagonal and lower-half */ |
| |
| for( i= j; i < N; ++i ){ |
| double abs_val; |
| |
| for( k= 0; k < j; ++k ) |
| LU( i, j )-= LU( i, k ) * LU( k, j ); |
| |
| /* find largest element in each COLUMN scaled so that */ |
| /* largest in each ROW is 1.0 */ |
| |
| abs_val= row_scale[ i ] * fabs( LU( i, j ) ); |
| |
| if( abs_val > max ){ |
| max= abs_val; |
| i_of_max= i; |
| } |
| } |
| if( fabs( LU( i_of_max, j ) ) < TOO_SMALL ){ |
| /* divisor is near zero */ |
| im_error( FUNCTION_NAME, "singular or near-singular matrix" ); |
| im_free_dmask( lu ); |
| im_free( row_scale ); |
| return NULL; |
| } |
| if( i_of_max != j ){ |
| /* swap ROWS */ |
| |
| for( k= 0; k < N; ++k ){ |
| double temp= LU( j, k ); |
| LU( j, k )= LU( i_of_max, k ); |
| LU( i_of_max, k )= temp; |
| } |
| row_scale[ i_of_max ]= row_scale[ j ]; |
| /* no need to copy this scale back up - we won't use it */ |
| } |
| /* record permutation */ |
| perm[ j ]= i_of_max; |
| |
| /* divide by best (largest scaled) pivot found */ |
| for( i= j + 1; i < N; ++i ) |
| LU( i, j )/= LU( j, j ); |
| } |
| im_free( row_scale ); |
| |
| return lu; |
| |
| #undef N |
| #undef LU |
| #undef perm |
| #undef FUNCTION_NAME |
| } |
| |
| int |
| im_lu_solve( |
| const DOUBLEMASK *lu, |
| double *vec |
| ){ |
| #define FUNCTION_NAME "im_lu_solve" |
| int i, j; |
| |
| if( lu-> xsize + 1 != lu-> ysize ){ |
| im_error( FUNCTION_NAME, "not an LU decomposed matrix" ); |
| return -1; |
| } |
| #define N ( lu -> xsize ) |
| #define LU( i, j ) MATRIX( lu, (i), (j) ) |
| #define perm ( lu-> coeff + N * N ) |
| |
| for( i= 0; i < N; ++i ){ |
| int i_perm= perm[ i ]; |
| |
| if( i_perm != i ){ |
| double temp= vec[ i ]; |
| vec[ i ]= vec[ i_perm ]; |
| vec[ i_perm ]= temp; |
| } |
| for( j= 0; j < i; ++j ) |
| vec[ i ]-= LU( i, j ) * vec [ j ]; |
| } |
| |
| for( i= N - 1; i >= 0; --i ){ |
| |
| for( j= i + 1; j < N; ++j ) |
| vec[ i ]-= LU( i, j ) * vec [ j ]; |
| |
| vec[ i ]/= LU( i, i ); |
| } |
| return 0; |
| |
| #undef LU |
| #undef perm |
| #undef N |
| #undef FUNCTION_NAME |
| } |
| |
| DOUBLEMASK * |
| im_matinv( |
| const DOUBLEMASK *mat, |
| const char *name |
| ){ |
| #define FUNCTION_NAME "im_matinv" |
| |
| DOUBLEMASK *inv; |
| |
| if( mat-> xsize != mat-> ysize ){ |
| im_error( FUNCTION_NAME, "non-square matrix" ); |
| return NULL; |
| } |
| #define N ( mat -> xsize ) |
| inv= im_create_dmask( name, N, N ); |
| if( ! inv ) |
| return NULL; |
| |
| if( N < 4 ){ |
| if( mat_inv_direct( inv, (const DOUBLEMASK *) mat, FUNCTION_NAME ) ){ |
| im_free_dmask( inv ); |
| return NULL; |
| } |
| return inv; |
| } |
| else { |
| DOUBLEMASK *lu= im_lu_decomp( mat, "temp" ); |
| |
| if( ! lu || mat_inv_lu( inv, (const DOUBLEMASK*) lu ) ){ |
| im_free_dmask( lu ); |
| im_free_dmask( inv ); |
| return NULL; |
| } |
| im_free_dmask( lu ); |
| |
| return inv; |
| } |
| #undef N |
| #undef FUNCTION_NAME |
| } |
| |
| int |
| im_matinv_inplace( |
| DOUBLEMASK *mat |
| ){ |
| #define FUNCTION_NAME "im_matinv_inplace" |
| int to_return= 0; |
| |
| if( mat-> xsize != mat-> ysize ){ |
| im_error( FUNCTION_NAME, "non-square matrix" ); |
| return -1; |
| } |
| #define N ( mat -> xsize ) |
| if( N < 4 ){ |
| DOUBLEMASK *dup= im_dup_dmask( mat, "temp" ); |
| if( ! dup ) |
| return -1; |
| |
| to_return= mat_inv_direct( mat, (const DOUBLEMASK*) dup, FUNCTION_NAME ); |
| |
| im_free_dmask( dup ); |
| |
| return to_return; |
| } |
| { |
| DOUBLEMASK *lu; |
| |
| lu= im_lu_decomp( mat, "temp" ); |
| |
| if( ! lu || mat_inv_lu( mat, (const DOUBLEMASK*) lu ) ) |
| to_return= -1; |
| |
| im_free_dmask( lu ); |
| |
| return to_return; |
| } |
| #undef N |
| #undef FUNCTION_NAME |
| } |
| |
| /* Invert a square size x size matrix stored in matrix[][] |
| * result is returned in the same matrix |
| */ |
| int |
| im_invmat( |
| double **matrix, |
| int size |
| ){ |
| |
| DOUBLEMASK *mat= im_create_dmask( "temp", size, size ); |
| int i; |
| int to_return= 0; |
| |
| for( i= 0; i < size; ++i ) |
| memcpy( mat-> coeff + i * size, matrix[ i ], size * sizeof( double ) ); |
| |
| to_return= im_matinv_inplace( mat ); |
| |
| if( ! to_return ) |
| for( i= 0; i < size; ++i ) |
| memcpy( matrix[ i ], mat-> coeff + i * size, size * sizeof( double ) ); |
| |
| im_free_dmask( mat ); |
| |
| return to_return; |
| } |
| |
| |
| /** LOCAL FUNCTION DEFINITIONS **/ |
| |
| static int |
| mat_inv_lu( |
| DOUBLEMASK *inv, |
| const DOUBLEMASK *lu |
| ){ |
| #define N ( lu-> xsize ) |
| #define INV( i, j ) MATRIX( inv, (i), (j) ) |
| |
| int i, j; |
| double *vec= IM_ARRAY( NULL, N, double ); |
| |
| if( ! vec ) |
| return -1; |
| |
| for( j= 0; j < N; ++j ){ |
| |
| for( i= 0; i < N; ++i ) |
| vec[ i ]= 0.0; |
| |
| vec[ j ]= 1.0; |
| |
| im_lu_solve( lu, vec ); |
| |
| for( i= 0; i < N; ++i ) |
| INV( i, j )= vec[ i ]; |
| } |
| im_free( vec ); |
| |
| inv-> scale= 1.0; |
| inv-> offset= 0.0; |
| |
| return 0; |
| |
| #undef N |
| #undef INV |
| } |
| |
| static int |
| mat_inv_direct( |
| DOUBLEMASK *inv, |
| const DOUBLEMASK *mat, |
| const char *function_name |
| ){ |
| #define N ( mat -> xsize ) |
| #define MAT( i, j ) MATRIX( mat, (i), (j) ) |
| #define INV( i, j ) MATRIX( inv, (i), (j) ) |
| |
| inv-> scale= 1.0; |
| inv-> offset= 0.0; |
| |
| switch( N ){ |
| case 1: { |
| if( fabs( MAT( 0, 0 ) ) < TOO_SMALL ){ |
| im_error( function_name, "singular or near-singular matrix" ); |
| return -1; |
| } |
| INV( 0, 0 )= 1.0 / MAT( 0, 0 ); |
| return 0; |
| } |
| case 2: { |
| double det= MAT( 0, 0 ) * MAT( 1, 1 ) - MAT( 0, 1 ) * MAT( 1, 0 ); |
| |
| if( fabs( det ) < TOO_SMALL ){ |
| im_error( function_name, "singular or near-singular matrix" ); |
| return -1; |
| } |
| INV( 0, 0 )= MAT( 1, 1 ) / det; |
| INV( 0, 1 )= -MAT( 0, 1 ) / det; |
| INV( 1, 0 )= -MAT( 1, 0 ) / det; |
| INV( 1, 1 )= MAT( 0, 0 ) / det; |
| |
| return 0; |
| } |
| case 3: { |
| double det= MAT( 0, 0 ) * ( MAT( 1, 1 ) * MAT( 2, 2 ) - MAT( 1, 2 ) * MAT( 2, 1 ) ) |
| - MAT( 0, 1 ) * ( MAT( 1, 0 ) * MAT( 2, 2 ) - MAT( 1, 2 ) * MAT( 2, 0 ) ) |
| + MAT( 0, 2 ) * ( MAT( 1, 0 ) * MAT( 2, 1 ) - MAT( 1, 1 ) * MAT( 2, 0 ) ); |
| |
| if( fabs( det ) < TOO_SMALL ){ |
| im_error( function_name, "singular or near-singular matrix" ); |
| return -1; |
| } |
| INV( 0, 0 )= ( MAT( 1, 1 ) * MAT( 2, 2 ) - MAT( 1, 2 ) * MAT( 2, 1 ) ) / det; |
| INV( 0, 1 )= ( MAT( 0, 2 ) * MAT( 2, 1 ) - MAT( 0, 1 ) * MAT( 2, 2 ) ) / det; |
| INV( 0, 2 )= ( MAT( 0, 1 ) * MAT( 1, 2 ) - MAT( 0, 2 ) * MAT( 1, 1 ) ) / det; |
| |
| INV( 1, 0 )= ( MAT( 1, 2 ) * MAT( 2, 0 ) - MAT( 1, 0 ) * MAT( 2, 2 ) ) / det; |
| INV( 1, 1 )= ( MAT( 0, 0 ) * MAT( 2, 2 ) - MAT( 0, 2 ) * MAT( 2, 0 ) ) / det; |
| INV( 1, 2 )= ( MAT( 0, 2 ) * MAT( 1, 0 ) - MAT( 0, 0 ) * MAT( 1, 2 ) ) / det; |
| |
| INV( 2, 0 )= ( MAT( 1, 0 ) * MAT( 2, 1 ) - MAT( 1, 1 ) * MAT( 2, 0 ) ) / det; |
| INV( 2, 1 )= ( MAT( 0, 1 ) * MAT( 2, 0 ) - MAT( 0, 0 ) * MAT( 2, 1 ) ) / det; |
| INV( 2, 2 )= ( MAT( 0, 0 ) * MAT( 1, 1 ) - MAT( 0, 1 ) * MAT( 1, 0 ) ) / det; |
| |
| return 0; |
| } |
| default: |
| return -1; |
| } |
| |
| #undef N |
| #undef MAT |
| #undef INV |
| } |
| |