src/INTERP_KERNEL/InterpKernelMatrixTools.cxx

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