invertmatrix.h

Go to the documentation of this file.
00001 // -*- c++ -*-  (for Emacs)
00002 // CREATED: Jul 13, 2005
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       // set identity matrix to the inverse matrix
00040       inverse[i*order+j] = ( i==j ? 1 : 0 );
00041     }
00042   }
00043     
00044   for (i = 0; i < order; i++) {
00045     /* look for largest element in column. */
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       /* swap rows. */
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       /* no non-zero pivot.  the matrix is singular, which
00068          shouldn't happen.  This means the user gave us a bad
00069          matrix. */
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

Generated on Mon Jul 30 09:46:50 2007 for Digest by  doxygen 1.5.2