src/math/smallsquarematrix.h

   1 //
   2 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   3 // +                                                                      +
   4 // + This file is part of enGrid.                                         +
   5 // +                                                                      +
   6 // + Copyright 2008-2013 enGits GmbH                                      +
   7 // +                                                                      +
   8 // + enGrid is free software: you can redistribute it and/or modify       +
   9 // + it under the terms of the GNU General Public License as published by +
  10 // + the Free Software Foundation, either version 3 of the License, or    +
  11 // + (at your option) any later version.                                  +
  12 // +                                                                      +
  13 // + enGrid is distributed in the hope that it will be useful,            +
  14 // + but WITHOUT ANY WARRANTY; without even the implied warranty of       +
  15 // + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the        +
  16 // + GNU General Public License for more details.                         +
  17 // +                                                                      +
  18 // + You should have received a copy of the GNU General Public License    +
  19 // + along with enGrid. If not, see <http://www.gnu.org/licenses/>.       +
  20 // +                                                                      +
  21 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  22 //
  23
  24 #ifndef SmallSquareMatrix_HH
  25 #define SmallSquareMatrix_HH
  26
  27 #include "mathvector.h"
  28
  29 template <class T, uint_t N> class SmallSquareMatrix;
  30
  31 #include <vector>
  32 #include <cstdarg>
  33 #include <cstdlib>
  34
  35 template <class T, uint_t N>
  36 class SmallSquareMatrix : public MathVector<StaticVector<MathVector<StaticVector<double,N> >,N> >
  37 {
  38   typedef StaticVector<T,N> svec_t;
  39   typedef MathVector<StaticVector<double,N> > rvec_t;
  40
  41 protected:
  42
  43   /** Indicator for precision safe mode
  44    */
  45   bool prec_safe;
  46   T prec_limit;
  47
  48 public:
  49
  50   /** get a component of the matrix.
  51    *  @param row the row of the component
  52    *  @param column the column of the component
  53    *  @return the component M[row,column]
  54    */
  55   T comp(uint_t row, uint_t column) const { return (*this)[row][column]; }
  56
  57   /** Empty Constructor I */
  58   SmallSquareMatrix<T,N>(T a_limit = 1e-20)
  59     : MathVector<StaticVector<MathVector<StaticVector<double,N> >,N> >() { prec_safe = false; a_limit = a_limit; }
  60
  61   /** Copy Constructor */
  62   SmallSquareMatrix<T,N>(const SmallSquareMatrix<T,N> &M)
  63     : MathVector<StaticVector<MathVector<StaticVector<double,N> >,N> >(M) {prec_safe = false;}
  64
  65   /** Constructor upon an open set of vectors
  66    *  @param row_col_type string indicating "row" or "column" type setting
  67    *         (only these two key-words allowed)
  68    *  @param col_vects an stl-type vector of double vectors
  69    */
  70   SmallSquareMatrix<T,N>(string row_col_type, vector<svec_t*> col_vects);
  71
  72   /** Constructor upon an open set of vectors containing doubles [T = double]
  73    *  @param row_col_type string indicating "row" or "column" type setting
  74    *         (only these two key-words allowed)
  75    *  @param col_vects an stl-type vector of double vectors
  76    */
  77   SmallSquareMatrix<T,N>(string row_col_type, vector<rvec_t*> col_vects);
  78
  79   /** Set a row vector
  80    *  @param row the row number to be set
  81    *  @param row_vec a double vector to set
  82    */
  83   template<class Tvec>
  84   void row(uint_t row, Tvec row_vec)
  85   {
  86     (*this)[row] = row_vec;
  87   }
  88
  89   /** Set a column vector
  90    *  @param column the column number to be set
  91    *  @param col_vec a double vector to set
  92    */
  93   template<class Tvec>
  94   void column(uint_t column, Tvec col_vec)
  95   {
  96     for(uint_t k_row = 0; k_row<N; k_row++) {
  97       (*this)[k_row][column] = col_vec[k_row];
  98     }
  99   }
 100
 101   /** Set safe mode. Safe mode throws a Precision_error in
 102    *  case of a precision limit (e.g. floating exception for "double")
 103    *  @param a_prec_limit precision limit
 104    */
 105   void setSafe(T a_prec_limit) {
 106     prec_limit = a_prec_limit;
 107     prec_safe = true;
 108   }
 109
 110   /** Set unsafe mode. Safe mode throws a Precision_error in
 111    *  case of a precision limit (e.g. floating exception for "double")
 112    */
 113   void setUnSafe() {
 114     prec_safe = false;
 115   }
 116
 117   /** Access safe mode. Safe mode throws a Precision_error in
 118    *  case of a precision limit (e.g. floating exception for "double")
 119    */
 120   bool isSafe() {
 121     return prec_safe;
 122   }
 123
 124   /** Access safe limit. Safe mode throws a Precision_error in
 125    *  case of a precision limit (e.g. floating exception for "double")
 126    */
 127   T safeLimit() {
 128     return prec_limit;
 129   }
 130
 131   /** Analyse Precision and throw error, if needed
 132    *  @param det determinant
 133    *  @param ele_max a linear norm
 134    */
 135   void precisionHandling(T det, T ele_max) {
 136     T qdet, compare_det;
 137     qdet = det * det;
 138     double expo = 0.5 / N;
 139     compare_det = pow(qdet, expo);
 140     //.... Compare the det^(1/N) with ele_max
 141     if(compare_det < safeLimit() * ele_max) {
 142       cerr << "matrix appears to be singular within given precision" << endl;
 143       exit(EXIT_FAILURE);
 144     }
 145   }
 146
 147   /** Get maximum absolute value of all elements
 148    *  @param det determinant
 149    *  @param ele_max a linear norm
 150    */
 151   T linNorm_0() {
 152     uint_t i,j;
 153     T ele_max, qq;
 154     ele_max = (*this)[0][0] * (*this)[0][0];
 155     for(i=0;i<N;i++) {
 156       for(j=0;j<N;j++) {
 157         qq = (*this)[i][j] * (*this)[i][j];
 158         if(qq > ele_max) {
 159           ele_max = qq;
 160         }
 161       }
 162     }
 163     ele_max = sqrt(ele_max);
 164     return ele_max;
 165   }
 166
 167   /** get an identity matrix
 168    *  @return an identity matrix of the dimension N
 169    */
 170   static SmallSquareMatrix<T,N> identity();
 171
 172   /** get the determinant of the matrix
 173    *  @return the determinant
 174    */
 175   T det();
 176
 177   /** get the inverse of the matrix
 178    *  @return the inverse of the matrix
 179    */
 180   SmallSquareMatrix<T,N> inverse();
 181
 182   /** get a sub-matrix.
 183    *  This returns a submatrix where one row and one column
 184    *  from the original matrix will be missing.
 185    *  @param row the row to exclude
 186    *  @param column the column to exclude
 187    *  @return the sub-matrix
 188    */
 189   SmallSquareMatrix<T,N-1> subMatrix(uint_t row, uint_t column);
 190
 191   /** get the transposed matrix.
 192    *  @return the transposed matrix
 193    */
 194   SmallSquareMatrix<T,N> transp();
 195
 196   /// matrix vector multiplication operator
 197   MathVector<StaticVector<T,N> > operator* (const MathVector<StaticVector<T,N> > &vec) const;
 198
 199   /// matrix matrix multiplication operator
 200   SmallSquareMatrix<T,N> operator* (const SmallSquareMatrix<T,N> &mat) const;
 201
 202   /** Initialize all matrix elements with initvalue
 203    *  @param initvalue a value
 204    */
 205   void initAll(double initvalue);
 206
 207 };
 208
 209 template <class T, uint_t N>
 210 inline MathVector<StaticVector<T,N> >
 211 SmallSquareMatrix<T,N>::operator* (const MathVector<StaticVector<T,N> > &vec) const
 212 {
 213   MathVector<StaticVector<T,N> > result_vec;
 214   for (uint_t i = 0; i < N; i++) {
 215     result_vec[i] = 0;
 216     for (uint_t j = 0; j < N; j++)
 217       result_vec[i] += comp(i,j) * vec[j];
 218   }
 219   return result_vec;
 220 }
 221
 222 template <class T, uint_t N>
 223 inline SmallSquareMatrix<T,N> SmallSquareMatrix<T,N>::operator* (const SmallSquareMatrix<T,N> &mat) const
 224 {
 225   SmallSquareMatrix<T,N> result_mat;
 226   for (uint_t i = 0; i < N; ++i) {
 227     for (uint_t j = 0; j < N; ++j) {
 228       result_mat[i][j] = 0;
 229       for (uint_t k = 0; k < N; ++k) result_mat[i][j] += this->comp(i,k)*mat[k][j];
 230     }
 231   }
 232   return result_mat;
 233 }
 234
 235 template <class T, uint_t N>
 236 inline void SmallSquareMatrix<T,N>::initAll(double initvalue)
 237 {
 238   for (uint_t i = 0; i < N; i++) {
 239     for (uint_t j = 0; j < N; j++) {
 240       (*this)[i][j] = initvalue;
 241     }
 242   }
 243 }
 244
 245 template <class T, uint_t N>
 246 inline T SmallSquareMatrix<T,N>::det()
 247 {
 248   // Determinant of the matrix
 249   // copy yourself to protect matrix entries
 250   SmallSquareMatrix<T,N> a = *this;
 251
 252   int n;
 253   n=N;
 254   int k,i,j;
 255   vector<int> p(n);
 256   T q,s,max,h,det;
 257
 258   det=1;
 259   for(k=0;k<n-1;k++)
 260     {
 261       max=0.0;
 262       p[k]=0;
 263       for(i=k;i<n;i++)
 264         {
 265           s=0.0;
 266           for(j=k;j<n;j++)
 267             {
 268               s=s+fabs(a[i][j]);
 269             }
 270           q=fabs(a[i][k])/s;
 271           if(q>max)
 272             {
 273               max=q;
 274               p[k]=i;
 275             }
 276         }
 277       if(!(p[k]==k))
 278         {
 279           det=-det;
 280           for(j=0;j<n;j++)
 281             {
 282               h=a[k][j];
 283               a[k][j]=a[p[k]][j];
 284               a[p[k]][j]=h;
 285             }
 286         }
 287       det=det*a[k][k];
 288       for(i=k+1;i<n;i++)
 289         {
 290           a[i][k]=a[i][k]/a[k][k];
 291           for(j=k+1;j<n;j++)
 292             a[i][j]=a[i][j]-a[i][k]*a[k][j];
 293         }
 294     }
 295   det=det*a[n-1][n-1];
 296
 297   return det;
 298 }
 299
 300 // Rainers inverter
 301 template <class T, uint_t N>
 302 class InvSmallSquareMatrix
 303 {
 304 protected:
 305   SmallSquareMatrix<T,N> b;
 306
 307 public:
 308   /** constructor.
 309    */
 310   InvSmallSquareMatrix<T,N>(SmallSquareMatrix<T,N> a,
 311                             bool a_prec_safe, T a_prec_limit) {
 312
 313     int Smalldim = N;
 314     int n;
 315     n=N;
 316     int k,i,j,l;
 317     vector<int> p(n);
 318     T q,s,max,h,det;
 319     T ele_max = 0;
 320
 321     if(a_prec_safe) {a.setSafe(a_prec_limit);}
 322
 323     //.. Find maximum element to get a relative value
 324     if(a.isSafe()) {
 325       ele_max = a.linNorm_0();
 326     }
 327
 328     //.. Get in matrix reduction
 329     for(k=0;k<Smalldim;k++)
 330       for(j=0;j<Smalldim;j++)
 331         b[j][k]=0.0;
 332     for(j=0;j<Smalldim;j++)
 333       b[j][j]=1.0;
 334     det=1;
 335     for(k=0;k<n-1;k++){
 336       max=0.0;
 337       p[k]=0;
 338       for(i=k;i<n;i++){
 339         s=0.0;
 340         for(j=k;j<n;j++) s=s+fabs(a[i][j]);
 341         q=fabs(a[i][k])/s;
 342         if(q>max){
 343           max=q;
 344           p[k]=i;
 345         }
 346       }
 347       if(!(p[k]==k)){
 348         det=-det;
 349         for(j=0;j<n;j++){
 350           h=a[k][j];
 351           a[k][j]=a[p[k]][j];
 352           a[p[k]][j]=h;
 353         }
 354       }
 355       det=det*a[k][k];
 356       for(i=k+1;i<n;i++){
 357         a[i][k]=a[i][k]/a[k][k];
 358         for(j=k+1;j<n;j++) a[i][j]=a[i][j]-a[i][k]*a[k][j];
 359       }
 360     }
 361     det=det*a[n-1][n-1];
 362
 363     //.. Proceed with rest of system reduction
 364     for(k=0;k<n-1;k++)
 365       if(!(p[k]==k)){
 366         for(l=0;l<n;l++){
 367           h=b[k][l];
 368           b[k][l]=b[p[k]][l];
 369           b[p[k]][l]=h;
 370         }
 371       }
 372     for(i=0;i<n;i++){
 373       for(j=0;j<i;j++){
 374         for(l=0;l<n;l++)
 375           b[i][l]=b[i][l]-a[i][j]*b[j][l];
 376       }
 377     }
 378     for(i=n-1;i>=0;i--){
 379       for(l=0;l<n;l++){
 380         s=b[i][l];
 381         for(k=i+1;k<n;k++)
 382           s=s-a[i][k]*b[k][l];
 383         b[i][l]=s/a[i][i];
 384       }
 385     }
 386
 387     //.. Check Determinant and throw error, if needed
 388     if(a.isSafe()) {
 389       a.precisionHandling(det, ele_max);
 390     }
 391
 392   }
 393
 394   SmallSquareMatrix<T,N> inverse() { return b; }
 395 };
 396
 397
 398 template <class T>
 399 class InvSmallSquareMatrix<T,2>
 400 {
 401 protected:
 402   SmallSquareMatrix<T,2> INV;
 403
 404 public:
 405   // constructor.
 406
 407   InvSmallSquareMatrix<T,2>(SmallSquareMatrix<T,2> SSM,
 408                             bool a_prec_safe, T a_prec_limit)
 409   {
 410     T ele_max;
 411     if(a_prec_safe) {
 412       SSM.setSafe(a_prec_limit);
 413       ele_max = SSM.linNorm_0();
 414       // sorce: maple
 415       T det = (SSM[0][0]*SSM[1][1]-SSM[0][1]*SSM[1][0]);
 416       SSM.precisionHandling(det, ele_max);
 417       T t4 = 1/det;
 418       INV[0][0] = SSM[1][1]*t4;
 419       INV[0][1] = -SSM[0][1]*t4;
 420       INV[1][0] = -SSM[1][0]*t4;
 421       INV[1][1] = SSM[0][0]*t4;
 422     }
 423     else {
 424       // sorce: maple
 425       T t4 = 1/(SSM[0][0]*SSM[1][1]-SSM[0][1]*SSM[1][0]);
 426       INV[0][0] = SSM[1][1]*t4;
 427       INV[0][1] = -SSM[0][1]*t4;
 428       INV[1][0] = -SSM[1][0]*t4;
 429       INV[1][1] = SSM[0][0]*t4;
 430     }
 431   }
 432
 433   SmallSquareMatrix<T,2> inverse() { return INV; }
 434 };
 435
 436 template <class T>
 437 class InvSmallSquareMatrix<T,3>
 438 {
 439 protected:
 440   SmallSquareMatrix<T,3> INV;
 441
 442 public:
 443   // constructor.
 444
 445   InvSmallSquareMatrix<T,3>(SmallSquareMatrix<T,3> SSM,
 446                             bool a_prec_safe, T a_prec_limit)
 447   {
 448     if(a_prec_safe) {
 449       SSM.setSafe(a_prec_limit);
 450       T ele_max = SSM.linNorm_0();
 451       // Source (maple)
 452       T t4 = SSM[0][0]*SSM[1][1];
 453       T t6 = SSM[0][0]*SSM[1][2];
 454       T t8 = SSM[0][1]*SSM[1][0];
 455       T t10 = SSM[0][2]*SSM[1][0];
 456       T t12 = SSM[0][1]*SSM[2][0];
 457       T t14 = SSM[0][2]*SSM[2][0];
 458       T det = (t4*SSM[2][2]-t6*SSM[2][1]-t8*SSM[2][2]+t10*SSM[2][1]+
 459                t12*SSM[1][2]-t14*SSM[1][1]);
 460       SSM.precisionHandling(det, ele_max);
 461       T t17 = 1/det;
 462       INV[0][0] = (SSM[1][1]*SSM[2][2]-SSM[1][2]*SSM[2][1])*t17;
 463       INV[0][1] = -(SSM[0][1]*SSM[2][2]-SSM[0][2]*SSM[2][1])*t17;
 464       INV[0][2] = -(-SSM[0][1]*SSM[1][2]+SSM[0][2]*SSM[1][1])*t17;
 465       INV[1][0] = -(SSM[1][0]*SSM[2][2]-SSM[1][2]*SSM[2][0])*t17;
 466       INV[1][1] = (SSM[0][0]*SSM[2][2]-t14)*t17;
 467       INV[1][2] = -(t6-t10)*t17;
 468       INV[2][0] = -(-SSM[1][0]*SSM[2][1]+SSM[1][1]*SSM[2][0])*t17;
 469       INV[2][1] = -(SSM[0][0]*SSM[2][1]-t12)*t17;
 470       INV[2][2] = (t4-t8)*t17;
 471     }
 472     else {
 473       // Source (maple)
 474       T t4 = SSM[0][0]*SSM[1][1];
 475       T t6 = SSM[0][0]*SSM[1][2];
 476       T t8 = SSM[0][1]*SSM[1][0];
 477       T t10 = SSM[0][2]*SSM[1][0];
 478       T t12 = SSM[0][1]*SSM[2][0];
 479       T t14 = SSM[0][2]*SSM[2][0];
 480       T t17 = 1/(t4*SSM[2][2]-t6*SSM[2][1]-t8*SSM[2][2]+t10*SSM[2][1]+
 481                  t12*SSM[1][2]-t14*SSM[1][1]);
 482       INV[0][0] = (SSM[1][1]*SSM[2][2]-SSM[1][2]*SSM[2][1])*t17;
 483       INV[0][1] = -(SSM[0][1]*SSM[2][2]-SSM[0][2]*SSM[2][1])*t17;
 484       INV[0][2] = -(-SSM[0][1]*SSM[1][2]+SSM[0][2]*SSM[1][1])*t17;
 485       INV[1][0] = -(SSM[1][0]*SSM[2][2]-SSM[1][2]*SSM[2][0])*t17;
 486       INV[1][1] = (SSM[0][0]*SSM[2][2]-t14)*t17;
 487       INV[1][2] = -(t6-t10)*t17;
 488       INV[2][0] = -(-SSM[1][0]*SSM[2][1]+SSM[1][1]*SSM[2][0])*t17;
 489       INV[2][1] = -(SSM[0][0]*SSM[2][1]-t12)*t17;
 490       INV[2][2] = (t4-t8)*t17;
 491     }
 492   }
 493   SmallSquareMatrix<T,3> inverse() { return INV; }
 494 };
 495
 496
 497
 498 typedef SmallSquareMatrix<double,2> mat2_t;
 499 typedef SmallSquareMatrix<double,3> mat3_t;
 500 typedef SmallSquareMatrix<double,4> mat4_t;
 501 typedef SmallSquareMatrix<double,5> mat5_t;
 502
 503 template <class T, uint_t N>
 504 SmallSquareMatrix<T,N>::SmallSquareMatrix(string row_col_type,
 505                                           vector<svec_t*> col_vects)
 506   : MathVector<StaticVector<double,N> >()
 507 {
 508   if(N < col_vects.size()) {
 509     cout
 510       << "SmallSquareMatrix<T,N>(string row_col_type, vector<SmallVector<T,N>*> col_vects)"
 511       << "\n"
 512       << "too many input vectors" << endl;
 513   }
 514   uint_t direct_it = 0;
 515   if(row_col_type == "Column") {
 516     for(typename vector<svec_t*>::iterator kk = col_vects.begin();
 517         kk != col_vects.end();
 518         kk++)
 519       {
 520         (*this).Column(direct_it, *(*kk));
 521         direct_it++;
 522       }
 523   } else if(row_col_type == "Row") {
 524     for(typename vector<svec_t*>::iterator kk = col_vects.begin();
 525         kk != col_vects.end();
 526         kk++)
 527       {
 528         (*this).Row(direct_it, *(*kk));
 529         direct_it++;
 530       }
 531   } else {
 532     cout
 533       << "SmallSquareMatrix<T,N>(string row_col_type, uint_t num_smvects, ...)"
 534       << "\n"
 535       << "Only Row or Column allowed as first argument" << endl;
 536     exit(EXIT_FAILURE);
 537   }
 538   prec_safe = false;
 539 }
 540
 541 template <class T, uint_t N>
 542 SmallSquareMatrix<T,N>::SmallSquareMatrix(string row_col_type,
 543                                           vector<rvec_t*> col_vects)
 544   : MathVector<StaticVector<MathVector<StaticVector<double,N> >,N> >()
 545 {
 546   if(N < col_vects.size()) {
 547     cout
 548       << "SmallSquareMatrix<real,N>(string row_col_type, vector<rvec_t*> col_vects)"
 549       << "\n"
 550       << "too many input vectors" << endl;
 551   }
 552   uint_t direct_it = 0;
 553   if(row_col_type == "Column") {
 554     for(typename vector<rvec_t*>::iterator kk = col_vects.begin();
 555         kk != col_vects.end();
 556         kk++)
 557       {
 558         (*this).Column(direct_it, *(*kk));
 559         direct_it++;
 560       }
 561   } else if(row_col_type == "Row") {
 562     for(typename vector<rvec_t*>::iterator kk = col_vects.begin();
 563         kk != col_vects.end();
 564         kk++)
 565       {
 566         (*this).Row(direct_it, *(*kk));
 567         direct_it++;
 568       }
 569   } else {
 570     cout
 571       <<  "SmallSquareMatrix<real,N>(string row_col_type, vector<rvec_t*> col_vects)"
 572       << "\n"
 573       << "Only Row or Column allowed as first argument" << endl;
 574     exit(EXIT_FAILURE);
 575   }
 576   prec_safe = false;
 577 }
 578
 579 template <class T, uint_t N>
 580 SmallSquareMatrix<T, N> SmallSquareMatrix<T, N>::identity()
 581 {
 582   SmallSquareMatrix<T, N> I;
 583   for (uint_t i = 0; i < N; i++) {
 584     for (uint_t j = 0; j < N; j++) {
 585       if (i==j) I[i][j] = 1;
 586       else I[i][j] = 0;
 587     }
 588   }
 589   return I;
 590 }
 591
 592 /*
 593 template <class T, uint_t N>
 594 T SmallSquareMatrix<T,N>::Det()
 595 {
 596   // This construction is required, since a prtioal specializations on a
 597   // method with multiple template arguments does not work without specializing
 598   // the whole class :-(
 599   DetSmallSquareMatrix<T,N> DET(*this);
 600   return DET.Det();
 601 }
 602 */
 603
 604 template <class T, uint_t N>
 605 SmallSquareMatrix<T,N> SmallSquareMatrix<T,N>::transp()
 606 {
 607  SmallSquareMatrix<T,N> M_t;
 608   for (uint_t i = 0; i < N; i++) {
 609     for (uint_t j = 0; j < N; j++) {
 610       M_t[i][j] = comp(j,i);
 611     }
 612   }
 613   return M_t;
 614 }
 615
 616 template <class T, uint_t N>
 617 SmallSquareMatrix<T,N> SmallSquareMatrix<T,N>::inverse()
 618 {
 619   InvSmallSquareMatrix<T,N> INV(*this, isSafe(), prec_limit);
 620   return INV.inverse();
 621 }
 622
 623 template <class T, uint_t N>
 624 SmallSquareMatrix<T,N-1> SmallSquareMatrix<T,N>::subMatrix(uint_t row, uint_t column)
 625 {
 626   uint_t i_new, j_new;
 627   i_new = 0;
 628   SmallSquareMatrix<T,N-1> M;
 629   for (uint_t i = 0; i < N; i++) {
 630     if (i == row) continue;
 631     j_new = 0;
 632     for (uint_t j = 0; j < N; j++) {
 633       if (j == column) continue;
 634       M[i_new][j_new] = comp(i,j);
 635       j_new++;
 636     }
 637     i_new++;
 638   }
 639   return M;
 640 }
 641
 642 #endif