1 // Copyright (C) 2022-2023 CEA, EDF
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.
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.
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
17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
20 // Implementation coming from Numerical Recipes in C of 1994 (version 2.04)
22 #include "InterpKernelQRDecomp.hxx"
23 #include "InterpKernelException.hxx"
27 using namespace INTERP_KERNEL;
30 inline T sqr(const T a) { return a*a; }
33 inline T sign(const T &a, const T &b) { return b >= 0 ? (a >= 0 ? a : -a) : (a >= 0 ? -a : a); }
35 QRDecomp::QRDecomp(const INTERP_KERNEL::DenseMatrix& a): n(a.nrows()), qt(n,n), r(a), sing(false)
38 std::vector<double> c(n), d(n);
39 double scale,sigma,sum,tau;
42 for (i=k;i<n;i++) scale=std::fmax(scale,std::abs(r[i][k]));
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]);
54 for (sum=0.0,i=k;i<n;i++) sum += r[i][k]*r[i][j];
56 for (i=k;i<n;i++) r[i][j] -= tau*r[i][k];
61 if (d[n-1] == 0.0) sing=true;
63 for (j=0;j<n;j++) qt[i][j]=0.0;
71 sum += r[i][k]*qt[i][j];
74 qt[i][j] -= sum*r[i][k];
80 for (j=0;j<i;j++) r[i][j]=0.0;
84 void QRDecomp::solve(const std::vector<double>& b, std::vector<double> &x)
90 void QRDecomp::qtmult(const std::vector<double>& b, std::vector<double> &x)
97 for (j=0;j<n;j++) sum += qt[i][j]*b[j];
102 void QRDecomp::rsolve(const std::vector<double>& b, std::vector<double> &x)
106 if (sing) THROW_IK_EXCEPTION("attempting solve in a singular QR");
107 for (i=n-1;i>=0;i--) {
109 for (j=i+1;j<n;j++) sum -= r[i][j]*x[j];
113 void QRDecomp::update(const std::vector<double>& u, const std::vector<double>& v)
116 std::vector<double> w(u);
118 if (w[k] != 0.0) break;
120 for (i=k-1;i>=0;i--) {
121 rotate(i,w[i],-w[i+1]);
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]));
128 for (i=0;i<n;i++) r[0][i] += w[0]*v[i];
130 rotate(i,r[i][i],-r[i+1][i]);
132 if (r[i][i] == 0.0) sing=true;
135 void QRDecomp::rotate(const mcIdType i, const double a, const double b)
142 s=(b >= 0.0 ? 1.0 : -1.0);
144 else if (std::abs(a) > std::abs(b))
147 c=sign(1.0/std::sqrt(1.0+(fact*fact)),a);
153 s=sign(1.0/std::sqrt(1.0+(fact*fact)),b);