00001
00002
00003
00004 #ifndef BOBMATRIX_H
00005 #define BOBMATRIX_H
00006
00007 #include <cmath>
00008
00009
00025 template <class Type>
00026 bool invert( const Type* src, Type* inverse, int order )
00027 {
00028 double t;
00029 int i, j, k, swap;
00030 Type tmp[order][order];
00031
00032 Q_ASSERT( src != 0 );
00033 Q_ASSERT( inverse != 0 );
00034
00035 for (i = 0; i < order; i++) {
00036 for (j = 0; j < order; j++) {
00037 tmp[i][j] = src[i*order+j];
00038
00039
00040 inverse[i*order+j] = ( i==j ? 1 : 0 );
00041 }
00042 }
00043
00044 for (i = 0; i < order; i++) {
00045
00046 swap = i;
00047 for (j = i + 1; j < order; j++) {
00048 if (fabs(tmp[j][i]) > fabs(tmp[i][i])) {
00049 swap = j;
00050 }
00051 }
00052
00053 if (swap != i) {
00054
00055 for (k = 0; k < order; k++) {
00056 t = tmp[i][k];
00057 tmp[i][k] = tmp[swap][k];
00058 tmp[swap][k] = t;
00059
00060 t = inverse[i*order+k];
00061 inverse[i*order+k] = inverse[swap*order+k];
00062 inverse[swap*order+k] = t;
00063 }
00064 }
00065
00066 if (tmp[i][i] == 0) {
00067
00068
00069
00070 Q_ASSERT( !"no non-zero pivot. the matrix is singular" );
00071 return false;
00072 }
00073
00074 t = tmp[i][i];
00075 for (k = 0; k < order; k++) {
00076 tmp[i][k] /= t;
00077 inverse[i*order+k] /= t;
00078 }
00079 for (j = 0; j < order; j++) {
00080 if (j != i) {
00081 t = tmp[j][i];
00082 for (k = 0; k < order; k++) {
00083 tmp[j][k] -= tmp[i][k]*t;
00084 inverse[j*order+k] -= inverse[i*order+k]*t;
00085 }
00086 }
00087 }
00088 }
00089 return true;
00090 }
00091
00092
00093
00094 #endif // ! BOBMATRIX_H