00001
00002 #include <math.h>
00003 #include <grass/libtrans.h>
00004
00005 #define EPSILON 1.0e-16
00006
00007
00008 #define N DIM_matrix
00009
00010
00011
00012
00013
00014
00015
00016
00017 int inverse (double m[N][N])
00018 {
00019 int i, j, k, l, ir=0, ic=0 ;
00020 int ipivot[N], itemp[N][2];
00021 double pivot[N], t;
00022 double fabs();
00023
00024
00025 if (isnull (m))
00026 return (-1) ;
00027
00028
00029
00030 for (i = 0; i < N; i++)
00031 ipivot[i] = 0;
00032
00033 for (i = 0; i < N; i++)
00034 {
00035 t = 0.0;
00036
00037 for (j = 0; j < N; j++)
00038 {
00039 if (ipivot[j] == 1)
00040 continue;
00041
00042 for (k = 0; k < N; k++)
00043 switch (ipivot[k]-1)
00044 {
00045 case 0:
00046 break;
00047 case -1:
00048 if (fabs (t) < fabs (m[j][k]))
00049 {
00050 ir = j;
00051 ic = k;
00052 t = m[j][k];
00053 }
00054 break;
00055 case 1:
00056 return (-1);
00057 break;
00058 default:
00059 return (-1);
00060 break;
00061 }
00062 }
00063
00064 ipivot[ic] += 1;
00065 if (ipivot[ic] > 1)
00066 {
00067 return (-1);
00068 }
00069
00070
00071 if (ir != ic)
00072 for (l = 0; l < N; l++)
00073 {
00074 t = m[ir][l];
00075 m[ir][l] = m[ic][l];
00076 m[ic][l] = t;
00077 }
00078
00079 itemp[i][0] = ir;
00080 itemp[i][1] = ic;
00081 pivot[i] = m[ic][ic];
00082
00083
00084 if (fabs (pivot[i]) < EPSILON)
00085 {
00086 return (-1);
00087 }
00088
00089
00090 m[ic][ic] = 1.0;
00091
00092 for (j = 0; j < N; j++)
00093 m[ic][j] /= pivot[i];
00094
00095
00096 for (k = 0; k < N; k++)
00097 if (k != ic)
00098 {
00099 t = m[k][ic];
00100 m[k][ic] = 0.0;
00101
00102 for (l = 0; l < N; l++)
00103 m[k][l] -= (m[ic][l] * t);
00104 }
00105 }
00106
00107
00108 for (i = 0; i < N; i++)
00109 {
00110 l = N - i - 1;
00111 if (itemp[l][0] == itemp[l][1])
00112 continue;
00113
00114 ir = itemp[l][0];
00115 ic = itemp[l][1];
00116
00117 for (k = 0; k < N; k++)
00118 {
00119 t = m[k][ir];
00120 m[k][ir] = m[k][ic];
00121 m[k][ic] = t;
00122 }
00123 }
00124
00125 return 1;
00126 }
00127
00128
00129
00130
00131 #define ZERO 1.0e-8
00132
00133
00134
00135
00136
00137 int isnull (double a[N][N])
00138 {
00139 register int i, j;
00140 double fabs();
00141
00142
00143 for (i = 0; i < N; i++)
00144 for (j = 0; j < N; j++)
00145 if ((fabs (a[i][j]) - ZERO) > ZERO)
00146 return 0;
00147
00148 return 1;
00149 }