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