/*--------------------------------------------------------------------------------
LU分解による逆行列の計算
calculate inverse matrix using LU decomposition
inv_matrix.c
--------------------------------------------------------------------------------*/
#include "least_sq.h"
int inv_matrix( double a[COL_NUM][COL_NUM] )
{
int i;
int j;
int index[COL_NUM];
double col[COL_NUM];
double a_temp[COL_NUM][COL_NUM];
/*---------------------------------------------------------------------------*/
if ( LU_decomp( a, index ) == -1 ){
return -1;
}
for ( j = 0; j < COL_NUM; j++ ) {
for ( i = 0; i < COL_NUM; i++ ) {
col[i] = 0.0;
}
col[j] = 1.0;
LU_solve( a, index, col );
for ( i = 0; i < COL_NUM; i++ ) {
a_temp[i][j] = col[i];
}
}
for ( i = 0; i < COL_NUM; i++ ) {
for ( j = 0; j < COL_NUM; j++ ){
a[i][j] = a_temp[i][j];
}
}
return 0;
}
/*------------------------------------------------------------------*/
int LU_decomp(double a[COL_NUM][COL_NUM], int indx[COL_NUM] )
{
int i, j, k;
int imax = 0;
double big, dum, sum, temp;
double vv[COL_NUM];
/*---------------------------------------------------------------------------*/
for ( i = 0; i < COL_NUM; i++ ) {
big = 0.0;
for ( j = 0; j < COL_NUM; j++ )
if ( (temp = fabs(a[i][j])) > big ) {
big = temp;
}
if ( big == 0.0 ) {
printf("Singular matrix in routine LU_decom\n");
return -1;
}
vv[i] = 1.0/(double)big;
}
for ( j = 0; j < COL_NUM; j++ ) {
for ( i = 0; i < j; i++ ) {
sum=a[i][j];
for ( k = 0; k < i; k++ ) {
sum -= a[i][k] * a[k][j];
}
a[i][j] = sum;
}
big = 0.0;
for ( i = j; i < COL_NUM; i++ ) {
sum = a[i][j];
for ( k = 0; k < j; k++ ) {
sum -= a[i][k] * a[k][j];
}
a[i][j] = sum;
if ( (dum = vv[i]*fabs(sum)) >= big) {
big = dum;
imax = i;
}
}
if (j != imax) {
for ( k = 0; k < COL_NUM; k++ ) {
dum = a[imax][k];
a[imax][k] = a[j][k];
a[j][k] = dum;
}
vv[imax] = vv[j];
}
indx[j] = imax;
if ( a[j][j] == 0.0 ) {
a[j][j] = TINY;
}
if (j != COL_NUM-1) {
dum=1.0/(double)a[j][j];
for ( i = j+1; i < COL_NUM; i++ ) {
a[i][j] *= dum;
}
}
}
return 0;
}
/*------------------------------------------------------------------*/
void LU_solve( double a[COL_NUM][COL_NUM], int indx[COL_NUM], double b[COL_NUM] )
{
int i, j, ip;
int ii = 0;
double sum;
for ( i = 0; i < COL_NUM; i++ ) {
ip = indx[i];
sum = b[ip];
b[ip] = b[i];
if (ii) {
for ( j = ii-1; j <= i-1; j++ ) {
sum -= a[i][j] * b[j];
}
}
else if (sum) {
ii = i+1;
}
b[i] = sum;
}
for ( i = COL_NUM-1; i >= 0; i-- ) {
sum = b[i];
for ( j = i+1; j < COL_NUM; j++ ) {
sum -= a[i][j] * b[j];
}
b[i] = sum/(double)a[i][i];
}
}