casadi_dense_lsqr.hpp
1 // C-REPLACE "fabs" "casadi_fabs"
2 // C-REPLACE "sign" "casadi_sign"
3 
4 //
5 // MIT No Attribution
6 //
7 // Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl, KU Leuven.
8 //
9 // Permission is hereby granted, free of charge, to any person obtaining a copy of this
10 // software and associated documentation files (the "Software"), to deal in the Software
11 // without restriction, including without limitation the rights to use, copy, modify,
12 // merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
13 // permit persons to whom the Software is furnished to do so.
14 //
15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
16 // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
17 // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
18 // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
19 // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
20 // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
21 //
22 
23 // SYMBOL "lsqr_sym_ortho"
24 template<typename T1>
25 void casadi_dense_lsqr_sym_ortho(T1 a, T1 b, T1* cs, T1* sn, T1* rho) {
26  T1 tau;
27  if (b == 0) {
28  *cs = sign(a);
29  *sn = 0;
30  *rho = fabs(a);
31  } else if (a==0) {
32  *cs = 0;
33  *sn = sign(b);
34  *rho = fabs(b);
35  } else if (fabs(b)>fabs(a)) {
36  tau = a/b;
37  *sn = sign(b)/sqrt(1+tau*tau);
38  *cs = (*sn)*tau;
39  *rho = b/(*sn);
40  } else {
41  tau = b/a;
42  *cs = sign(a)/sqrt(1+tau*tau);
43  *sn = (*cs)*tau;
44  *rho = a/(*cs);
45  }
46 }
47 
48 //
49 // MIT No Attribution
50 //
51 // Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl, KU Leuven.
52 //
53 // Permission is hereby granted, free of charge, to any person obtaining a copy of this
54 // software and associated documentation files (the "Software"), to deal in the Software
55 // without restriction, including without limitation the rights to use, copy, modify,
56 // merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
57 // permit persons to whom the Software is furnished to do so.
58 //
59 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
60 // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
61 // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
62 // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
63 // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
64 // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
65 //
66 
67 // SYMBOL "lsqr_single_solve"
68 // Ref: scipy
69 template<typename T1>
70 int casadi_dense_lsqr_single_solve(const T1* A, T1* x, casadi_int tr, const casadi_int ncol,
71  const casadi_int nrow, T1* w) {
72  casadi_int m, n, i;
73  T1 damp, atol, btol, conlim, ctol, anorm, acond, dampsq, ddnorm, res2, xnorm, xxnorm, z;
74  T1 cs2, sn2, alpha, beta, rhobar, phibar, bnorm, rnorm, arnorm, rhobar1, cs1, sn1, psi;
75  T1 cs, sn, rho, theta, phi, tau, t1, t2, n2dk, delta, gambar, rhs, zbar, gamma, res1;
76  T1 r1sq, r1norm, test1, test2, test3, rtol;
77  casadi_int iter_lim, itn, istop;
78  T1 *u, *v, *xx, *ww, *dk;
79 
80  m = ncol;//sp[0];
81  n = nrow;//sp[1];
82 
83  damp = 0;
84  atol = 1e-15;
85  btol = 1e-15;
86  conlim = 1e8;
87  iter_lim = 10000;
88 
89  itn = 0;
90  istop = 0;
91 
92  ctol = 0;
93  if (conlim > 0) ctol = 1/conlim;
94  anorm = 0;
95  acond = 0;
96  dampsq = damp*damp;
97  ddnorm = 0;
98  res2 = 0;
99  xnorm = 0;
100  xxnorm = 0;
101  z = 0;
102  cs2 = -1;
103  sn2 = 0;
104 
105  u = w; w+= m; casadi_copy(x, m, u);
106  v = w; w+= n; casadi_clear(v, n);
107  xx = w; w+= n; casadi_clear(xx, n);
108  ww = w; w+= n; casadi_clear(v, n);
109  dk = w; w+= n;
110 
111  alpha = 0;
112  beta = casadi_norm_2(m, u);
113 
114  if (beta>0) {
115  for (i=0;i<m;++i) u[i]*=1/beta;
116  casadi_mv_dense(A, nrow, ncol, u, v, !tr);
117  // casadi_mv(A, sp, u, v, !tr);
118  alpha = casadi_norm_2(n, v);
119  }
120 
121  if (alpha>0) {
122  for (i=0;i<n;++i) v[i]*=1/alpha;
123  casadi_copy(v, n, ww);
124  }
125 
126  rhobar = alpha;
127  phibar = beta;
128  bnorm = beta;
129  rnorm = beta;
130  arnorm = alpha * beta;
131 
132  while (itn<iter_lim) {
133  itn++;
134  for (i=0;i<m;++i) u[i]*=-alpha;
135  // casadi_mv(A, sp, v, u, tr);
136  casadi_mv_dense(A, nrow, ncol, u, v, !tr);
137  beta = casadi_norm_2(m, u);
138 
139  if (beta>0) {
140  for (i=0;i<m;++i) u[i]*=1/beta;
141  anorm = sqrt(anorm*anorm + alpha*alpha+beta*beta+damp*damp);
142  for (i=0;i<n;++i) v[i]*=-beta;
143  // casadi_mv(A, sp, u, v, !tr);
144  casadi_mv_dense(A, nrow, ncol, u, v, !tr);
145  alpha = casadi_norm_2(n, v);
146  if (alpha>0) for (i=0;i<n;++i) v[i]*=1/alpha;
147  }
148 
149  rhobar1 = sqrt(rhobar*rhobar+damp*damp);
150 
151  cs1 = rhobar / rhobar1;
152  sn1 = damp / rhobar1;
153  psi = sn1 * phibar;
154  phibar *= cs1;
155 
156  casadi_dense_lsqr_sym_ortho(rhobar1, beta, &cs, &sn, &rho);
157 
158  theta = sn * alpha;
159  rhobar = -cs * alpha;
160  phi = cs * phibar;
161  phibar *= sn;
162  tau = sn * phi;
163 
164  t1 = phi / rho;
165  t2 = -theta / rho;
166 
167  for (i=0;i<n;++i) dk[i]=ww[i]/rho;
168 
169  for (i=0; i<n; ++i) xx[i] += t1*ww[i];
170  for (i=0; i<n; ++i) ww[i] = v[i] + t2*ww[i];
171 
172  n2dk = casadi_norm_2(n, dk);
173  ddnorm += n2dk*n2dk;
174 
175  delta = sn2 * rho;
176  gambar = -cs2 * rho;
177  rhs = phi - delta * z;
178  zbar = rhs / gambar;
179  xnorm = sqrt(xxnorm + zbar*zbar);
180  gamma = sqrt(gambar*gambar + theta*theta);
181  cs2 = gambar / gamma;
182  sn2 = theta / gamma;
183  z = rhs / gamma;
184  xxnorm += z*z;
185 
186  acond = anorm * sqrt(ddnorm);
187  res1 = phibar*phibar;
188  res2 += psi*psi;
189  rnorm = sqrt(res1+res2);
190  arnorm = alpha*fabs(tau);
191 
192  r1sq = rnorm*rnorm - dampsq * xxnorm;
193  r1norm = sqrt(fabs(r1sq));
194  if (r1sq < 0) r1norm = -r1norm;
195 
196  test1 = rnorm / bnorm;
197  test2 = arnorm / (anorm * rnorm);
198  test3 = 1 / acond;
199  t1 = test1 / (1 + anorm * xnorm / bnorm);
200  rtol = btol + atol * anorm * xnorm / bnorm;
201 
202  if (itn >= iter_lim) istop = 7;
203  if (1 + test3 <= 1) istop = 6;
204  if (1 + test2 <= 1) istop = 5;
205  if (1 + t1 <= 1) istop = 4;
206 
207  if (test3 <= ctol) istop = 3;
208  if (test2 <= atol) istop = 2;
209  if (test1 <= rtol) istop = 1;
210 
211  if (istop != 0) break;
212 
213  }
214  casadi_copy(xx, m, x);
215  return 0;
216 }
217 
218 //
219 // MIT No Attribution
220 //
221 // Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl, KU Leuven.
222 //
223 // Permission is hereby granted, free of charge, to any person obtaining a copy of this
224 // software and associated documentation files (the "Software"), to deal in the Software
225 // without restriction, including without limitation the rights to use, copy, modify,
226 // merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
227 // permit persons to whom the Software is furnished to do so.
228 //
229 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
230 // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
231 // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
232 // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
233 // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
234 // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
235 //
236 
237 // SYMBOL "lsqr_solve"
238 template<typename T1>
239 int casadi_dense_lsqr_solve(const T1* A, T1* x, casadi_int nrhs, casadi_int tr,
240  const casadi_int ncol, const casadi_int nrow, T1* w) {
241  casadi_int i;
242  for (i=0; i<nrhs;++i) {
243  // if (casadi_dense_lsqr_single_solve(A, x+i*sp[1], tr, ncol, nrow, w)) return 1;
244  if (casadi_dense_lsqr_single_solve(A, x+i*nrow, tr, ncol, nrow, w)) return 1;
245  }
246  return 0;
247 }