00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include <stdio.h>
00013 #include <math.h>
00014 #include <AR/matrix.h>
00015
00016 static double *minv( double *ap, int dimen, int rowa );
00017
00018 int arMatrixSelfInv(ARMat *m)
00019 {
00020 if(minv(m->m, m->row, m->row) == NULL) return -1;
00021
00022 return 0;
00023 }
00024
00025
00026
00027
00028
00029
00030
00031 static double *minv( double *ap, int dimen, int rowa )
00032 {
00033 double *wap, *wcp, *wbp;
00034 int i,j,n,ip,nwork;
00035 int nos[50];
00036 double epsl;
00037 double p,pbuf,work;
00038 double fabs();
00039
00040 epsl = 1.0e-10;
00041
00042 switch (dimen) {
00043 case (0): return(NULL);
00044 case (1): *ap = 1.0 / (*ap);
00045 return(ap);
00046 }
00047
00048 for(n = 0; n < dimen ; n++)
00049 nos[n] = n;
00050
00051 for(n = 0; n < dimen ; n++) {
00052 wcp = ap + n * rowa;
00053
00054 for(i = n, wap = wcp, p = 0.0; i < dimen ; i++, wap += rowa)
00055 if( p < ( pbuf = fabs(*wap)) ) {
00056 p = pbuf;
00057 ip = i;
00058 }
00059 if (p <= epsl)
00060 return(NULL);
00061
00062 nwork = nos[ip];
00063 nos[ip] = nos[n];
00064 nos[n] = nwork;
00065
00066 for(j = 0, wap = ap + ip * rowa, wbp = wcp; j < dimen ; j++) {
00067 work = *wap;
00068 *wap++ = *wbp;
00069 *wbp++ = work;
00070 }
00071
00072 for(j = 1, wap = wcp, work = *wcp; j < dimen ; j++, wap++)
00073 *wap = *(wap + 1) / work;
00074 *wap = 1.0 / work;
00075
00076 for(i = 0; i < dimen ; i++) {
00077 if(i != n) {
00078 wap = ap + i * rowa;
00079 for(j = 1, wbp = wcp, work = *wap;
00080 j < dimen ; j++, wap++, wbp++)
00081 *wap = *(wap + 1) - work * (*wbp);
00082 *wap = -work * (*wbp);
00083 }
00084 }
00085 }
00086
00087 for(n = 0; n < dimen ; n++) {
00088 for(j = n; j < dimen ; j++)
00089 if( nos[j] == n) break;
00090 nos[j] = nos[n];
00091 for(i = 0, wap = ap + j, wbp = ap + n; i < dimen ;
00092 i++, wap += rowa, wbp += rowa) {
00093 work = *wap;
00094 *wap = *wbp;
00095 *wbp = work;
00096 }
00097 }
00098 return(ap);
00099 }