Invert matrix according LDU decomposition. 
   76    {
   78 
   79      size_t  i, row, col, 
root;
 
   80 
   81      double* p0;
   83      double* p2;
   84      double* p3;
   85 
   86      const double* c0;
   87 
   88      double v0;
   89      double v1;
   90      double v2;
   91      double v3;
   92 
   93      double w0;
   94 
   95      P.resize(this->
size());
 
   96 
   97      
   98 
   99      for (root = 0; 
root != this->
size(); ++root) {
 
  100 
  103 
  104        for (row = 
root; ++row != this->
size(); ) {
 
  105          if (fabs((*
this)(row, 
root)) > fabs(value)) {
 
  106            value = (*this)(row, 
root);
 
  107            pivot = row;
  108          }
  109        }
  110 
  111        if (value == 0.0) {
  112          THROW(JDivisionByZero, 
"LDU decomposition zero pivot at " << 
root);
 
  113        }
  114 
  116          this->
rswap(root, pivot);
 
  117        }
  118 
  120 
  121        for (row = 
root + 1; row + 4 <= this->
size(); row += 4) {            
 
  122 
  123          p0 = (*this)[row + 0]  +  
root;
 
  124          p1 = (*this)[row + 1]  +  
root;
 
  125          p2 = (*this)[row + 2]  +  
root;
 
  126          p3 = (*this)[row + 3]  +  
root;
 
  127 
  128          v0 = (*p0++ /= value);
  129          v1 = (*
p1++ /= value);
 
  130          v2 = (*p2++ /= value);
  131          v3 = (*p3++ /= value);
  132 
  134 
  135          for (i = 
root + 1; i + 4 <= this->
size(); i += 4, p0 += 4, 
p1 += 4, p2 += 4, p3 += 4, c0 += 4) {
 
  136            p0[0] -= v0 * c0[0];  p0[1] -= v0 * c0[1];  p0[2] -= v0 * c0[2];  p0[3] -= v0 * c0[3];
  137            p1[0] -= v1 * c0[0];  
p1[1] -= v1 * c0[1];  
p1[2] -= v1 * c0[2];  
p1[3] -= v1 * c0[3];
 
  138            p2[0] -= v2 * c0[0];  p2[1] -= v2 * c0[1];  p2[2] -= v2 * c0[2];  p2[3] -= v2 * c0[3];
  139            p3[0] -= v3 * c0[0];  p3[1] -= v3 * c0[1];  p3[2] -= v3 * c0[2];  p3[3] -= v3 * c0[3];
  140          }
  141 
  142          for ( ; i != this->
size(); ++i, ++p0, ++
p1, ++p2, ++p3, ++c0) {
 
  143            *p0 -= v0 * (*c0);
  145            *p2 -= v2 * (*c0);
  146            *p3 -= v3 * (*c0);
  147          }
  148        }
  149 
  150        for ( ; row != this->
size(); ++row) {                                
 
  151 
  152          p0 = (*this)[row + 0]  +  
root;
 
  153 
  154          v0 = (*p0++ /= value);
  155 
  157 
  158          for (i = 
root + 1; i + 4 <= this->
size(); i += 4, p0 += 4, c0 += 4) {
 
  159            *p0 -= v0 * c0[0];  p0[1] -= v0 * c0[1];  p0[2] -= v0 * c0[2];  p0[3] -= v0 * c0[3];
  160          }
  161 
  162          for ( ; i != this->
size(); ++i, ++p0, ++c0) {
 
  163            *p0 -= v0 * (*c0);
  164          }
  165        }
  166      }
  167 
  168 
  170 
  171      A.reset();
  172 
  173      for (i = 0; i != this->
size(); ++i) {
 
  174        A(i,i) = 1.0;
  175      }
  176 
  177 
  178      
  179 
  180      col = 0;
  181      
  182      for ( ; col + 4 <= this->
size(); col += 4) {
 
  183          
  184        for (row = 0; row != this->
size(); ++row) {
 
  185 
  186          p0 = A[col + 0];
  188          p2 = A[col + 2];
  189          p3 = A[col + 3];
  190 
  192 
  193          v0 = p0[i];
  195          v2 = p2[i];
  196          v3 = p3[i];
  197 
  198          p0[i] = p0[row];
  200          p2[i] = p2[row];
  201          p3[i] = p3[row];
  202 
  203          for (i = 0, c0 = (*
this)[row]; i != row; ++c0, ++p0, ++
p1, ++p2, ++p3, ++i) {
 
  204            v0 -= (*c0) * (*p0);
  205            v1 -= (*c0) * (*p1);
  206            v2 -= (*c0) * (*p2);
  207            v3 -= (*c0) * (*p3);
  208          }
  209 
  210          A[col + 0][row] = v0;
  211          A[col + 1][row] = v1;
  212          A[col + 2][row] = v2;
  213          A[col + 3][row] = v3;
  214        }
  215      }
  216 
  217      for ( ; col != this->
size(); ++col) {
 
  218 
  219        for (row = 0; row != this->
size(); ++row) {
 
  220 
  221          p0 = A[col];
  222          
  224 
  225          v0 = p0[i];
  226 
  227          p0[i] = p0[row];
  228 
  229          for (i = 0, c0 = (*this)[row] + i; i != row; ++c0, ++p0, ++i) {
  230            v0 -= (*c0) * (*p0);
  231          }
  232 
  233          A[col][row] = v0;
  234        }
  235      }
  236 
  237      
  238 
  239      col = 0;
  240      
  241      for ( ; col + 4 <= this->
size(); col += 4) {
 
  242 
  243        for (
int row = this->
size() - 1; row >= 0; --row) {
 
  244 
  245          p0 = A[col + 0] + row;
  246          p1 = A[col + 1] + row;
 
  247          p2 = A[col + 2] + row;
  248          p3 = A[col + 3] + row;
  249 
  250          v0 = *p0;
  252          v2 = *p2;
  253          v3 = *p3;
  254 
  255          ++p0;
  257          ++p2;
  258          ++p3;
  259 
  260          for (i = row + 1, c0 = (*
this)[row] + i; i != this->
size(); ++c0, ++p0, ++
p1, ++p2, ++p3, ++i) {
 
  261            v0 -= (*c0) * (*p0);
  262            v1 -= (*c0) * (*p1);
  263            v2 -= (*c0) * (*p2);
  264            v3 -= (*c0) * (*p3);
  265          }
  266 
  267          w0 = 1.0 / (*this)[row][row];
  268 
  269          A[col + 0][row] = v0 * w0;
  270          A[col + 1][row] = v1 * w0;
  271          A[col + 2][row] = v2 * w0;
  272          A[col + 3][row] = v3 * w0;
  273        }
  274      }
  275     
  276      for ( ; col != this->
size(); ++col) {
 
  277 
  278        for (
int row = this->
size() - 1; row >= 0; --row) {
 
  279 
  280          p0 = A[col] + row;
  281 
  282          v0 = *p0;
  283 
  284          ++p0;
  285 
  286          for (i = row + 1, c0 = (*
this)[row] + i; i != this->
size(); ++c0, ++p0, ++i) {
 
  287            v0 -= (*c0) * (*p0);
  288          }
  289 
  290          w0 = 1.0 / (*this)[row][row];
  291 
  292          A[col][row] = v0 * w0;
  293        }
  294      }
  295 
  296      A.transpose();
  297 
  299    }
#define THROW(JException_t, A)
Marco for throwing exception with std::ostream compatible message.
 
void swap(JMatrixND_t &A)
Swap matrices.
 
JMatrixND_t()
Default constructor.
 
JMatrixND_t & getInstance()
Get work space.
 
void rswap(size_t r1, size_t r2)
 
std::vector< int > P
permutation matrix