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