casadi_qr.hpp
1 //
2 // MIT No Attribution
3 //
4 // Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl, KU Leuven.
5 //
6 // Permission is hereby granted, free of charge, to any person obtaining a copy of this
7 // software and associated documentation files (the "Software"), to deal in the Software
8 // without restriction, including without limitation the rights to use, copy, modify,
9 // merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
10 // permit persons to whom the Software is furnished to do so.
11 //
12 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
13 // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
14 // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
15 // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
16 // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
17 // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
18 //
19 
20 // SYMBOL "house"
21 // Householder reflection
22 // Ref: Chapter 5, Direct Methods for Sparse Linear Systems by Tim Davis
23 template<typename T1>
24 T1 casadi_house(T1* v, T1* beta, casadi_int nv) {
25  // Local variable
26  casadi_int i;
27  T1 v0, sigma, s, sigma_is_zero, v0_nonpos;
28  // Calculate norm
29  v0 = v[0]; // Save v0 (overwritten below)
30  sigma=0;
31  for (i=1; i<nv; ++i) sigma += v[i]*v[i];
32  s = sqrt(v0*v0 + sigma); // s = norm(v)
33  // Calculate consistently with symbolic datatypes (SXElem)
34  sigma_is_zero = sigma==0;
35  v0_nonpos = v0<=0;
36  // C-REPLACE "if_else" "casadi_if_else"
37  v[0] = if_else(sigma_is_zero, 1,
38  if_else(v0_nonpos, v0-s, -sigma/(v0+s))); // NOLINT
39  *beta = if_else(sigma_is_zero, 2*v0_nonpos, -1/(s*v[0])); // NOLINT
40  return s;
41 }
42 
43 // SYMBOL "qr"
44 // Numeric QR factorization
45 // Ref: Chapter 5, Direct Methods for Sparse Linear Systems by Tim Davis
46 // len[x] = nrow
47 // sp_v = [nrow, ncol, 0, 0, ...] len[3 + ncol + nnz_v]
48 // len[v] nnz_v
49 // sp_r = [nrow, ncol, 0, 0, ...] len[3 + ncol + nnz_r]
50 // len[r] nnz_r
51 // len[beta] ncol
52 template<typename T1>
53 void casadi_qr(const casadi_int* sp_a, const T1* nz_a, T1* x,
54  const casadi_int* sp_v, T1* nz_v, const casadi_int* sp_r, T1* nz_r, T1* beta,
55  const casadi_int* prinv, const casadi_int* pc) {
56  // Local variables
57  casadi_int ncol, nrow, r, c, k, k1;
58  T1 alpha;
59  const casadi_int *a_colind, *a_row, *v_colind, *v_row, *r_colind, *r_row;
60  // Extract sparsities
61  ncol = sp_a[1];
62  a_colind=sp_a+2; a_row=sp_a+2+ncol+1;
63  nrow = sp_v[0];
64  v_colind=sp_v+2; v_row=sp_v+2+ncol+1;
65  r_colind=sp_r+2; r_row=sp_r+2+ncol+1;
66  // Clear work vector
67  for (r=0; r<nrow; ++r) x[r] = 0;
68  // Loop over columns of R, A and V
69  for (c=0; c<ncol; ++c) {
70  // Copy (permuted) column of A to x
71  for (k=a_colind[pc[c]]; k<a_colind[pc[c]+1]; ++k) x[prinv[a_row[k]]] = nz_a[k];
72  // Use the equality R = (I-betan*vn*vn')*...*(I-beta1*v1*v1')*A to get
73  // strictly upper triangular entries of R
74  for (k=r_colind[c]; k<r_colind[c+1] && (r=r_row[k])<c; ++k) {
75  // Calculate scalar factor alpha = beta(r)*dot(v(:,r), x)
76  alpha = 0;
77  for (k1=v_colind[r]; k1<v_colind[r+1]; ++k1) alpha += nz_v[k1]*x[v_row[k1]];
78  alpha *= beta[r];
79  // x -= alpha*v(:,r)
80  for (k1=v_colind[r]; k1<v_colind[r+1]; ++k1) x[v_row[k1]] -= alpha*nz_v[k1];
81  // Get r entry
82  *nz_r++ = x[r];
83  // Strictly upper triangular entries in x no longer needed
84  x[r] = 0;
85  }
86  // Get V column
87  for (k=v_colind[c]; k<v_colind[c+1]; ++k) {
88  nz_v[k] = x[v_row[k]];
89  // Lower triangular entries of x no longer needed
90  x[v_row[k]] = 0;
91  }
92  // Get diagonal entry of R, normalize V column
93  *nz_r++ = casadi_house(nz_v + v_colind[c], beta + c, v_colind[c+1] - v_colind[c]);
94  }
95  }
96 
97 // SYMBOL "qr_mv"
98 // Multiply QR Q matrix from the right with a vector, with Q represented
99 // by the Householder vectors V and beta
100 // x = Q*x or x = Q'*x
101 // with Q = (I-beta(1)*v(:,1)*v(:,1)')*...*(I-beta(n)*v(:,n)*v(:,n)')
102 // len[x] >= nrow_ext
103 template<typename T1>
104 void casadi_qr_mv(const casadi_int* sp_v, const T1* v, const T1* beta, T1* x,
105  casadi_int tr) {
106  // Local variables
107  casadi_int ncol, c, c1, k;
108  T1 alpha;
109  const casadi_int *colind, *row;
110  // Extract sparsity
111  ncol=sp_v[1];
112  colind=sp_v+2; row=sp_v+2+ncol+1;
113  // Loop over vectors
114  for (c1=0; c1<ncol; ++c1) {
115  // Forward order for transpose, otherwise backwards
116  c = tr ? c1 : ncol-1-c1;
117  // Calculate scalar factor alpha = beta(c)*dot(v(:,c), x)
118  alpha=0;
119  for (k=colind[c]; k<colind[c+1]; ++k) alpha += v[k]*x[row[k]];
120  alpha *= beta[c];
121  // x -= alpha*v(:,c)
122  for (k=colind[c]; k<colind[c+1]; ++k) x[row[k]] -= alpha*v[k];
123  }
124 }
125 
126 // SYMBOL "qr_trs"
127 // Solve for an (optionally transposed) upper triangular matrix R
128 template<typename T1>
129 void casadi_qr_trs(const casadi_int* sp_r, const T1* nz_r, T1* x, casadi_int tr) {
130  // Local variables
131  casadi_int ncol, r, c, k;
132  const casadi_int *colind, *row;
133  // Extract sparsity
134  ncol=sp_r[1];
135  colind=sp_r+2; row=sp_r+2+ncol+1;
136  if (tr) {
137  // Forward substitution
138  for (c=0; c<ncol; ++c) {
139  for (k=colind[c]; k<colind[c+1]; ++k) {
140  r = row[k];
141  if (r==c) {
142  x[c] /= nz_r[k];
143  } else {
144  x[c] -= nz_r[k]*x[r];
145  }
146  }
147  }
148  } else {
149  // Backward substitution
150  for (c=ncol-1; c>=0; --c) {
151  for (k=colind[c+1]-1; k>=colind[c]; --k) {
152  r=row[k];
153  if (r==c) {
154  x[r] /= nz_r[k];
155  } else {
156  x[r] -= nz_r[k]*x[c];
157  }
158  }
159  }
160  }
161 }
162 
163 // SYMBOL "qr_solve"
164 // Solve a factorized linear system
165 // len[w] >= max(ncol, nrow_ext)
166 template<typename T1>
167 void casadi_qr_solve(T1* x, casadi_int nrhs, casadi_int tr,
168  const casadi_int* sp_v, const T1* v, const casadi_int* sp_r, const T1* r,
169  const T1* beta, const casadi_int* prinv, const casadi_int* pc, T1* w) {
170  casadi_int k, c, nrow_ext, ncol;
171  nrow_ext = sp_v[0]; ncol = sp_v[1];
172  for (k=0; k<nrhs; ++k) {
173  if (tr) {
174  // (PR' Q R PC)' x = PC' R' Q' PR x = b <-> x = PR' Q R' \ PC b
175  // Multiply by PC
176  for (c=0; c<ncol; ++c) w[c] = x[pc[c]];
177  // Solve for R'
178  casadi_qr_trs(sp_r, r, w, 1);
179  // Multiply by Q
180  casadi_qr_mv(sp_v, v, beta, w, 0);
181  // Multiply by PR'
182  for (c=0; c<ncol; ++c) x[c] = w[prinv[c]];
183  } else {
184  //PR' Q R PC x = b <-> x = PC' R \ Q' PR b
185  // Multiply with PR
186  for (c=0; c<nrow_ext; ++c) w[c] = 0;
187  for (c=0; c<ncol; ++c) w[prinv[c]] = x[c];
188  // Multiply with Q'
189  casadi_qr_mv(sp_v, v, beta, w, 1);
190  // Solve for R
191  casadi_qr_trs(sp_r, r, w, 0);
192  // Multiply with PC'
193  for (c=0; c<ncol; ++c) x[pc[c]] = w[c];
194  }
195  x += ncol;
196  }
197 }
198 
199 // SYMBOL "qr_singular"
200 // Check if QR factorization corresponds to a singular matrix
201 template<typename T1>
202 casadi_int casadi_qr_singular(T1* rmin, casadi_int* irmin, const T1* nz_r,
203  const casadi_int* sp_r, const casadi_int* pc, T1 eps) {
204  // Local variables
205  T1 rd, rd_min;
206  casadi_int ncol, c, nullity;
207  const casadi_int* r_colind;
208  // Nullity
209  nullity = 0;
210  // Extract sparsity
211  ncol = sp_r[1];
212  r_colind = sp_r + 2;
213  // Find the smallest diagonal entry
214  for (c=0; c<ncol; ++c) {
215  rd = fabs(nz_r[r_colind[c+1]-1]);
216  // Increase nullity if smaller than eps
217  if (rd<eps) nullity++;
218  // Check if smallest so far
219  if (c==0 || rd < rd_min) {
220  rd_min = rd;
221  if (rmin) *rmin = rd;
222  if (irmin) *irmin = pc[c];
223  }
224  }
225  // Return number of zero-ish eigenvalues
226  return nullity;
227 }
228 
229 // SYMBOL "qr_colcomb"
230 // Get a vector v such that A*v = 0 and |v| == 1
231 template<typename T1>
232 void casadi_qr_colcomb(T1* v, const T1* nz_r, const casadi_int* sp_r,
233  const casadi_int* pc, T1 eps, casadi_int ind) {
234  // Local variables
235  casadi_int ncol, r, c, k;
236  const casadi_int *r_colind, *r_row;
237  // Extract sparsity
238  ncol = sp_r[1];
239  r_colind = sp_r + 2;
240  r_row = r_colind + ncol + 1;
241  // Find the ind-th diagonal which is smaller than eps, overwrite ind with c
242  for (c=0; c<ncol; ++c) {
243  if (fabs(nz_r[r_colind[c+1]-1])<eps && 0==ind--) {
244  ind = c;
245  break;
246  }
247  }
248  // Reset w
249  casadi_clear(v, ncol);
250  v[pc[ind]] = 1.;
251  // Copy ind-th column to v
252  for (k=r_colind[ind]; k<r_colind[ind+1]-1; ++k) {
253  v[pc[r_row[k]]] = -nz_r[k];
254  }
255  // Backward substitution
256  for (c=ind-1; c>=0; --c) {
257  for (k=r_colind[c+1]-1; k>=r_colind[c]; --k) {
258  r=r_row[k];
259  if (r==c) {
260  if (fabs(nz_r[k])<eps) {
261  v[pc[r]] = 0;
262  } else {
263  v[pc[r]] /= nz_r[k];
264  }
265  } else {
266  v[pc[r]] -= nz_r[k]*v[pc[c]];
267  }
268  }
269  }
270  // Normalize v
271  casadi_scal(ncol, 1./sqrt(casadi_dot(ncol, v, v)), v);
272 }