00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "LinAlg.h"
00016 #include <math.h>
00017 #if defined(WIN32)
00018 #include <malloc.h>
00019 #else
00020 #include <alloca.h>
00021 #endif
00022
00023 namespace linalg
00024 {
00025 using namespace linalg;
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 double Matrix::dummy_determinant_ref = 0;
00058
00059 static bool is_dummy_det_ref(double &determ_ref)
00060 { return &determ_ref == &Matrix::dummy_determinant_ref; }
00061
00062 Matrix& Matrix::invert(double &determ_ref)
00063 {
00064 is_valid();
00065 if( nrows != ncols )
00066 info(),
00067 _error("Matrix to invert must be square");
00068
00069 double determinant = 1;
00070 const double singularity_tolerance = 1e-35;
00071
00072
00073 #ifdef WIN32
00074 struct Pivot { int row, col; } * const pivots =
00075 (Pivot*)malloc(ncols*sizeof(Pivot));
00076 bool * const was_pivoted = (bool*)malloc(nrows*sizeof(bool));
00077 #else
00078 struct Pivot { int row, col; } * const pivots =
00079 (Pivot*)alloca(ncols*sizeof(Pivot));
00080 bool * const was_pivoted = (bool*)alloca(nrows*sizeof(bool));
00081 #endif
00082 memset(was_pivoted,false,nrows*sizeof(bool));
00083
00084 for(register Pivot * pivotp = &pivots[0]; pivotp < &pivots[ncols]; pivotp++)
00085 {
00086 const REAL * old_ppos = 0;
00087 int prow = 0, pcol = 0;
00088 {
00089 REAL max_value = 0;
00090 register const REAL * cp = elements;
00091 for(register int j=0; j<ncols; j++)
00092 if( !was_pivoted[j] )
00093 {
00094 REAL curr_value = 0;
00095 for(register int k=0; k<nrows; k++,cp++)
00096 if( !was_pivoted[k] && (curr_value = fabs(*cp)) > max_value )
00097 max_value = curr_value, prow = k, pcol = j, old_ppos = cp;
00098 }
00099 else
00100 cp += nrows;
00101 if( max_value < singularity_tolerance )
00102 if( !is_dummy_det_ref(determ_ref) )
00103 {
00104 determ_ref = 0;
00105 return *this;
00106 }
00107 else
00108 _error("Matrix turns out to be singular: can't invert");
00109 pivotp->row = prow;
00110 pivotp->col = pcol;
00111 }
00112
00113 REAL * const old_colp = const_cast<REAL*>(old_ppos) - prow;
00114 REAL * const new_colp = ( prow == pcol ? old_colp : elements + prow*nrows );
00115 if( prow != pcol )
00116 {
00117 register REAL * cr = new_colp;
00118 register REAL * cc = old_colp;
00119 for(register int k=0; k<nrows; k++)
00120 {
00121 REAL temp = *cr; *cr++ = *cc; *cc++ = temp;
00122 }
00123 }
00124 was_pivoted[prow] = true;
00125
00126 {
00127 register REAL * pivot_cp = new_colp;
00128 const double pivot_val = pivot_cp[prow];
00129 determinant *= pivot_val;
00130 pivot_cp[prow] = 1;
00131 for(register int k=0; k<nrows; k++)
00132 *pivot_cp++ /= pivot_val;
00133 }
00134
00135 {
00136 register REAL * pivot_rp = elements + prow;
00137 register REAL * ep = elements;
00138 for(register int k=0; k<ncols; k++, pivot_rp += nrows)
00139 if( k != prow )
00140 {
00141 const double temp = *pivot_rp;
00142 *pivot_rp = 0;
00143 register const REAL * pivot_cp = new_colp;
00144 for(register int l=0; l<nrows; l++)
00145 *ep++ -= temp * *pivot_cp++;
00146 }
00147 else
00148 ep += nrows;
00149 }
00150 }
00151
00152 int no_swaps = 0;
00153 for(register const Pivot * pvp = &pivots[ncols-1];
00154 pvp >= &pivots[0]; pvp--)
00155 if( pvp->row != pvp->col )
00156 {
00157 no_swaps++;
00158 register REAL * rp = elements + pvp->row;
00159 register REAL * cp = elements + pvp->col;
00160 for(register int k=0; k<ncols; k++, rp += nrows, cp += nrows)
00161 {
00162 const REAL temp = *rp; *rp = *cp; *cp = temp;
00163 }
00164 }
00165
00166 if( !is_dummy_det_ref(determ_ref) )
00167 determ_ref = ( no_swaps & 1 ? -determinant : determinant );
00168
00169 return *this;
00170 }
00171
00172 }