src/INTERP_KERNEL/LinearAlgebra/InterpKernelQRDecomp.cxx

   1 // Copyright (C) 2022-2024  CEA, EDF
   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
  20 // Implementation coming from Numerical Recipes in C of 1994 (version 2.04)
  21
  22 #include "InterpKernelQRDecomp.hxx"
  23 #include "InterpKernelException.hxx"
  24
  25 #include <cmath>
  26
  27 using namespace INTERP_KERNEL;
  28
  29 template<class T>
  30 inline T sqr(const T a) { return a*a; }
  31
  32 template<class T>
  33 inline T sign(const T &a, const T &b) { return b >= 0 ? (a >= 0 ? a : -a) : (a >= 0 ? -a : a); }
  34
  35 QRDecomp::QRDecomp(const INTERP_KERNEL::DenseMatrix& a): n(a.nrows()), qt(n,n), r(a), sing(false)
  36 {
  37         mcIdType i,j,k;
  38         std::vector<double> c(n), d(n);
  39         double scale,sigma,sum,tau;
  40         for (k=0;k<n-1;k++) {
  41                 scale=0.0;
  42                 for (i=k;i<n;i++) scale=std::fmax(scale,std::abs(r[i][k]));
  43                 if (scale == 0.0) {
  44                         sing=true;
  45                         c[k]=d[k]=0.0;
  46                 } else {
  47                         for (i=k;i<n;i++) r[i][k] /= scale;
  48                         for (sum=0.0,i=k;i<n;i++) sum += sqr(r[i][k]);
  49                         sigma=sign(std::sqrt(sum),r[k][k]);
  50                         r[k][k] += sigma;
  51                         c[k]=sigma*r[k][k];
  52                         d[k] = -scale*sigma;
  53                         for (j=k+1;j<n;j++) {
  54                                 for (sum=0.0,i=k;i<n;i++) sum += r[i][k]*r[i][j];
  55                                 tau=sum/c[k];
  56                                 for (i=k;i<n;i++) r[i][j] -= tau*r[i][k];
  57                         }
  58                 }
  59         }
  60         d[n-1]=r[n-1][n-1];
  61         if (d[n-1] == 0.0) sing=true;
  62         for (i=0;i<n;i++) {
  63                 for (j=0;j<n;j++) qt[i][j]=0.0;
  64                 qt[i][i]=1.0;
  65         }
  66         for (k=0;k<n-1;k++) {
  67                 if (c[k] != 0.0) {
  68                         for (j=0;j<n;j++) {
  69                                 sum=0.0;
  70                                 for (i=k;i<n;i++)
  71                                         sum += r[i][k]*qt[i][j];
  72                                 sum /= c[k];
  73                                 for (i=k;i<n;i++)
  74                                         qt[i][j] -= sum*r[i][k];
  75                         }
  76                 }
  77         }
  78         for (i=0;i<n;i++) {
  79                 r[i][i]=d[i];
  80                 for (j=0;j<i;j++) r[i][j]=0.0;
  81         }
  82 }
  83
  84 void QRDecomp::solve(const std::vector<double>& b, std::vector<double> &x)
  85 {
  86         qtmult(b,x);
  87         rsolve(x,x);
  88 }
  89
  90 void QRDecomp::qtmult(const std::vector<double>& b, std::vector<double> &x)
  91 {
  92         mcIdType i,j;
  93         double sum;
  94         for (i=0;i<n;i++)
  95   {
  96                 sum = 0.;
  97                 for (j=0;j<n;j++) sum += qt[i][j]*b[j];
  98                 x[i] = sum;
  99         }
 100 }
 101
 102 void QRDecomp::rsolve(const std::vector<double>& b, std::vector<double> &x)
 103 {
 104         mcIdType i,j;
 105         double sum;
 106         if (sing) THROW_IK_EXCEPTION("attempting solve in a singular QR");
 107         for (i=n-1;i>=0;i--) {
 108                 sum=b[i];
 109                 for (j=i+1;j<n;j++) sum -= r[i][j]*x[j];
 110                 x[i]=sum/r[i][i];
 111         }
 112 }
 113 void QRDecomp::update(const std::vector<double>& u, const std::vector<double>& v)
 114 {
 115         mcIdType i,k;
 116         std::vector<double> w(u);
 117         for (k=n-1;k>=0;k--)
 118                 if (w[k] != 0.0) break;
 119         if (k < 0) k=0;
 120         for (i=k-1;i>=0;i--) {
 121                 rotate(i,w[i],-w[i+1]);
 122                 if (w[i] == 0.0)
 123                         w[i]=std::abs(w[i+1]);
 124                 else if (std::abs(w[i]) > std::abs(w[i+1]))
 125                         w[i]=std::abs(w[i])*std::sqrt(1.0+sqr(w[i+1]/w[i]));
 126                 else w[i]=std::abs(w[i+1])*std::sqrt(1.0+sqr(w[i]/w[i+1]));
 127         }
 128         for (i=0;i<n;i++) r[0][i] += w[0]*v[i];
 129         for (i=0;i<k;i++)
 130                 rotate(i,r[i][i],-r[i+1][i]);
 131         for (i=0;i<n;i++)
 132                 if (r[i][i] == 0.0) sing=true;
 133 }
 134
 135 void QRDecomp::rotate(const mcIdType i, const double a, const double b)
 136 {
 137         mcIdType j;
 138         double c,fact,s,w,y;
 139         if (a == 0.0)
 140   {
 141                 c=0.0;
 142                 s=(b >= 0.0 ? 1.0 : -1.0);
 143         }
 144   else if (std::abs(a) > std::abs(b))
 145   {
 146                 fact=b/a;
 147                 c=sign(1.0/std::sqrt(1.0+(fact*fact)),a);
 148                 s=fact*c;
 149         }
 150   else
 151   {
 152                 fact=a/b;
 153                 s=sign(1.0/std::sqrt(1.0+(fact*fact)),b);
 154                 c=fact*s;
 155         }
 156         for (j=i;j<n;j++)
 157   {
 158                 y=r[i][j];
 159                 w=r[i+1][j];
 160                 r[i][j]=c*y-s*w;
 161                 r[i+1][j]=s*y+c*w;
 162         }
 163         for (j=0;j<n;j++)
 164   {
 165                 y=qt[i][j];
 166                 w=qt[i+1][j];
 167                 qt[i][j]=c*y-s*w;
 168                 qt[i+1][j]=s*y+c*w;
 169         }
 170 }