src/INTERP_KERNEL/InterpKernelMatrixTools.cxx

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