src/medtool/src/INTERP_KERNEL/InterpKernelMatrixTools.cxx

   1 // Copyright (C) 2007-2015  CEA/DEN, EDF R&D
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Lesser General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2.1 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Lesser General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Lesser General Public
  14 // License along with this library; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  16 //
  17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
  18 //
  19 // Author : Anthony Geay (CEA/DEN)
  20
  21 #include "InterpKernelMatrixTools.hxx"
  22 #include "InterpKernelException.hxx"
  23 #include "InterpKernelAutoPtr.hxx"
  24
  25 #include <sstream>
  26 #include <algorithm>
  27
  28 namespace INTERP_KERNEL
  29 {
  30   /*
  31    *  Computes the dot product of two vectors.
  32    *  This routine uses unrolled loops for increments equal to one.
  33    *
  34    *  Reference:
  35    *
  36    *    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
  37    *    LINPACK User's Guide,
  38    *    SIAM, 1979,
  39    *    ISBN13: 978-0-898711-72-1,
  40    *    LC: QA214.L56.
  41    *
  42    *    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
  43    *    Basic Linear Algebra Subprograms for Fortran Usage,
  44    *    Algorithm 539,
  45    *    ACM Transactions on Mathematical Software,
  46    *    Volume 5, Number 3, September 1979, pages 308-323.
  47    *
  48    *  \param [in] n the number of entries in the vectors.
  49    *  \param [in] dx the first vector.
  50    *  \param [in] incx the increment between successive entries in \a dx.
  51    *  \param [in] dy the second vector.
  52    *  \param [in] incy the increment between successive entries in \a dy.
  53    *  \return the sum of the product of the corresponding entries of \a dx and \a dy.
  54    */
  55   double ddot(int n, const double *dx, int incx, const double *dy, int incy)
  56   {
  57     double dtemp=0.0;
  58     int i,ix,iy,m;
  59     if(n<=0)
  60       return dtemp;
  61     // Code for unequal increments or equal increments not equal to 1.
  62     if(incx!=1 || incy!=1)
  63       {
  64         if (incx>=0)
  65           ix=0;
  66         else
  67           ix=(-n+1)*incx;
  68
  69         if(incy>=0)
  70           iy=0;
  71         else
  72           iy=(-n+1)*incy;
  73         for(i=0;i<n;i++,ix+=incx,iy+=incy)
  74           dtemp+=dx[ix]*dy[iy];
  75       }
  76     //Code for both increments equal to 1.
  77     else
  78       {
  79         m=n%5;
  80         for(i=0;i<m;i++)
  81           dtemp+=dx[i]*dy[i];
  82         for (i=m;i<n;i+=5)
  83           dtemp+=dx[i]*dy[i]+dx[i+1]*dy[i+1]+dx[i+2]*dy[i+2]+dx[i+3]*dy[i+3]+dx[i+4]*dy[i+4];
  84       }
  85     return dtemp;
  86   }
  87
  88
  89   void dscal(int n, double sa, double *x, int incx)
  90   {
  91     int i,ix,m;
  92
  93     if(n<=0) { }
  94     else if(incx==1)
  95       {
  96         m=n%5;
  97         for(i=0;i<m;i++ )
  98           x[i]=sa*x[i];
  99
 100         for(i=m;i<n;i+=5)
 101           {
 102             x[i]  =sa*x[i];
 103             x[i+1]=sa*x[i+1];
 104             x[i+2]=sa*x[i+2];
 105             x[i+3]=sa*x[i+3];
 106             x[i+4]=sa*x[i+4];
 107           }
 108       }
 109     else
 110       {
 111         if(0<=incx)
 112           ix=0;
 113         else
 114           ix=(-n+1)*incx;
 115
 116         for(i=0;i<n;i++,ix+=incx)
 117           x[ix]=sa*x[ix];
 118       }
 119   }
 120
 121   void daxpy(int n, double da, const double *dx, int incx, double *dy, int incy)
 122   {
 123     int i,ix,iy,m;
 124     if (n<=0)
 125       return;
 126     if (da==0.0)
 127       return;
 128     // Code for unequal increments or equal increments not equal to 1.
 129     if(incx!=1 || incy!=1)
 130       {
 131         if(0<=incx)
 132           ix=0;
 133         else
 134           ix=(-n+1)*incx;
 135
 136         if(0<=incy)
 137           iy=0;
 138         else
 139           iy=(-n+1)*incy;
 140
 141         for(i=0;i<n;i++,ix+=incx,iy+=incy)
 142           dy[iy]=dy[iy]+da*dx[ix];
 143       }
 144     // Code for both increments equal to 1.
 145     else
 146       {
 147         m=n%4;
 148         for (i=0;i<m;i++)
 149           dy[i]=dy[i]+da*dx[i];
 150         for(i=m;i<n;i+=4)
 151           {
 152             dy[i  ]=dy[i  ]+da*dx[i  ];
 153             dy[i+1]=dy[i+1]+da*dx[i+1];
 154             dy[i+2]=dy[i+2]+da*dx[i+2];
 155             dy[i+3]=dy[i+3]+da*dx[i+3];
 156           }
 157       }
 158   }
 159
 160   double r8_abs(double x)
 161   {
 162     if(x>=0.0)
 163       return x;
 164     else
 165       return -x;
 166   }
 167
 168   void dswap(int n, double *x, int incx, double *y, int incy)
 169   {
 170     int i,ix,iy,m;
 171     double temp;
 172
 173     if(n<=0) { }
 174     else if(incx==1 && incy==1)
 175       {
 176         m=n%3;
 177         for(i=0;i<m;i++)
 178           { temp=x[i]; x[i]=y[i]; y[i]=temp; }
 179         for(i=m;i<n;i+=3)
 180           {
 181             temp=x[i]; x[i]=y[i]; y[i]=temp;
 182             temp=x[i+1]; x[i+1]=y[i+1]; y[i+1]=temp;
 183             temp=x[i+2]; x[i+2]=y[i+2]; y[i+2]=temp;
 184           }
 185       }
 186     else
 187       {
 188         if(0<=incx)
 189           ix=0;
 190         else
 191           ix=(-n+1)*incx;
 192         if(0<=incy)
 193           iy=0;
 194         else
 195           iy=(-n+1)*incy;
 196         for(i=0;i<n;i++,ix+=incx,iy+=incy)
 197           {
 198             temp=x[ix]; x[ix]=y[iy];
 199           }
 200       }
 201   }
 202
 203   /*
 204    *  Finds the index of the vector element of maximum absolute value.
 205    *  \warning This index is a 1-based index, not a 0-based index !
 206    *
 207    *  Reference:
 208    *    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
 209    *    LINPACK User's Guide,
 210    *    SIAM, 1979,
 211    *    ISBN13: 978-0-898711-72-1,
 212    *    LC: QA214.L56.
 213    *
 214    *    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
 215    *    Basic Linear Algebra Subprograms for Fortran Usage,
 216    *    Algorithm 539,
 217    *    ACM Transactions on Mathematical Software,
 218    *    Volume 5, Number 3, September 1979, pages 308-323.
 219    *
 220    *    \param [in] n the number of entries in the vector.
 221    *    \param [in] dx the vector to be examined.
 222    *    \param [in] incx the increment between successive entries of SX.
 223    *    \return the index of the element of maximum absolute value (in C convention).
 224    */
 225   int idamax(int n, const double *dx, int incx)
 226   {
 227     double dmax;
 228     int i,ix,value;
 229     value=-1;
 230     if ( n < 1 || incx <= 0 )
 231       return value;
 232     value=0;
 233     if(n==1)
 234       return value;
 235     if(incx==1)
 236       {
 237         dmax=r8_abs(dx[0]);
 238         for(i=1;i<n;i++)
 239           {
 240             if(dmax<r8_abs(dx[i]))
 241               {
 242                 value=i;
 243                 dmax=r8_abs(dx[i]);
 244               }
 245           }
 246       }
 247     else
 248       {
 249         ix=0;
 250         dmax=r8_abs(dx[0]);
 251         ix+=incx;
 252         for(i=1;i<n;i++)
 253           {
 254             if(dmax<r8_abs(dx[ix]))
 255               {
 256                 value=i;
 257                 dmax=r8_abs(dx[ix]);
 258               }
 259             ix+=incx;
 260           }
 261       }
 262     return value;
 263   }
 264
 265
 266   /*
 267    *  Purpose:
 268    *
 269    *     factors a real general matrix.
 270    *
 271    *  Reference:
 272    *
 273    *    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
 274    *    LINPACK User's Guide,
 275    *    SIAM, (Society for Industrial and Applied Mathematics),
 276    *    3600 University City Science Center,
 277    *    Philadelphia, PA, 19104-2688.
 278    *    ISBN 0-89871-172-X
 279    *
 280    *  Parameters:
 281    *
 282    *  \param [in,out] a a matrix of size LDA*N. On intput, the matrix to be factored.
 283    *    On output, an upper triangular matrix and the multipliers used to obtain
 284    *    it.  The factorization can be written A=L*U, where L is a product of
 285    *    permutation and unit lower triangular matrices, and U is upper triangular.
 286    *
 287    *  \param [in] lda the leading dimension of \a a.
 288    *  \param [in] n the order of the matrix \a a.
 289    *  \param [out] ipvt the pivot indices of size \a n.
 290    *  \return the singularity indicator.
 291    *  - 0, normal value.
 292    *  - K, if U(K-1,K-1) == 0.  This is not an error condition for this subroutine,
 293    *    but it does indicate that DGESL or DGEDI will divide by zero if called.
 294    */
 295   int dgefa(double *a, int lda, int n, int *ipvt)
 296   {
 297     int info=0;
 298     int l;
 299     double t;
 300     // Gaussian elimination with partial pivoting.
 301     for(int k=0;k<n-1;k++)
 302       {
 303         //  Find L=pivot index.
 304         l=idamax(n-k-1,a+k+k*lda,1)+k;
 305         ipvt[k]=l;
 306         // Zero pivot implies this column already triangularized.
 307         if(a[l+k*lda]==0.0)
 308           {
 309             info=k+1;
 310             continue;
 311           }
 312         //Interchange if necessary.
 313         if(l!=k)
 314           {
 315             t=a[l+k*lda];
 316             a[l+k*lda]=a[k+k*lda];
 317             a[k+k*lda]=t;
 318           }
 319         // Compute multipliers.
 320         t=-1.0/a[k+k*lda];
 321         dscal(n-k-1,t,a+k+1+k*lda,1);
 322         // Row elimination with column indexing.
 323         for(int j=k+1;j<n;j++)
 324           {
 325             t=a[l+j*lda];
 326             if(l!=k-1)
 327               {
 328                 a[l+j*lda]=a[k+j*lda];
 329                 a[k+j*lda]=t;
 330               }
 331             daxpy(n-k-1,t,a+k+1+k*lda,1,a+k+1+j*lda,1);
 332           }
 333       }
 334     ipvt[n-1]=n-1;
 335     if(a[n-1+(n-1)*lda]==0.0)
 336       info=n;
 337     return info;
 338   }
 339
 340   /*
 341    *  - Purpose: computes inverse (job=1) of a matrix factored by dgefa.
 342    *
 343    *  - Discussion: A division by zero will occur if the input factor contains
 344    *    a zero on the diagonal. It will not occur if the subroutines are called correctly
 345    *    and dgfa has set info to 0.
 346    *
 347    *  Reference:
 348    *
 349    *    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
 350    *    LINPACK User's Guide,
 351    *    SIAM, (Society for Industrial and Applied Mathematics),
 352    *    3600 University City Science Center,
 353    *    Philadelphia, PA, 19104-2688.
 354    *    ISBN 0-89871-172-X
 355    *
 356    *  Parameters:
 357    *
 358    *    \param [in,out] a matrix of size lda*n, on input, the LU factor information, as output of dgfa. On output, the inverse matrix.
 359    *    \param [in] lda, the leading dimension of the array \a a.
 360    *    \param [in] n, the order of the matrix \a a.
 361    *    \param [in] ipvt, the pivot vector from dgfa.
 362    *    \param [in,out] work a work array of size at least equal to \a n.
 363    */
 364   void dgedi(double *a, int lda, int n, const int *ipvt, double *work)
 365   {
 366     double t;
 367     for(int k=0;k<n;k++)
 368       {
 369         a[k+k*lda]=1.0/a[k+k*lda];
 370         t=-a[k+k*lda];
 371         dscal(k,t,a+0+k*lda,1);
 372         for(int j=k+1;j<n;j++)
 373           {
 374             t=a[k+j*lda];
 375             a[k+j*lda]=0.0;
 376             daxpy(k+1,t,a+0+k*lda,1,a+0+j*lda,1);
 377           }
 378       }
 379     // Form inverse(U) * inverse(L).
 380     for(int k=n-2;k>=0;k--)
 381       {
 382         for(int i=k+1;i<n;i++)
 383           {
 384             work[i]=a[i+k*lda];
 385             a[i+k*lda]=0.0;
 386           }
 387
 388         for(int j=k+1;j<n;j++)
 389           {
 390             t=work[j];
 391             daxpy(n,t,a+0+j*lda,1,a+0+k*lda,1);
 392           }
 393         int l=ipvt[k];
 394         if(l!=k-1)
 395           dswap(n,a+0+k*lda,1,a+0+l*lda,1);
 396       }
 397   }
 398
 399   void matrixProduct(const double *A, int n1, int p1, const double *B, int n2, int p2, double *C)
 400   {
 401     if(p1!=n2)
 402       {
 403         std::ostringstream oss; oss << "matrixProduct : the size of input matrix are not coherent the nb of cols of input matrix #0 is " <<  p1 << " whereas the number of rows of input matrix #1 is " << n2 << " !";
 404         throw INTERP_KERNEL::Exception(oss.str().c_str());
 405       }
 406     for(int i=0;i<n1;i++)
 407       {
 408         for(int j=0;j<p2;j++)
 409           {
 410             C[i*p2+j] = 0.;
 411             for(int k=0;k<p1;k++)
 412               C[i*p2+j]+=A[i*p1+k]*B[k*p2+j];
 413           }
 414       }
 415   }
 416
 417   void inverseMatrix(const double *A, int n, double *iA)
 418   {
 419     INTERP_KERNEL::AutoPtr<int> ipvt=new int[n];
 420     INTERP_KERNEL::AutoPtr<double> work=new double[n*n];
 421     std::copy(A,A+n*n,iA);
 422     dgefa(iA,n,n,ipvt);
 423     dgedi(iA,n,n,ipvt,work);
 424   }
 425 }