1 // NOLINT(legal/copyright)
2 
3 // C-REPLACE "fmin" "casadi_fmin"
4 // C-REPLACE "fmax" "casadi_fmax"
5 // C-REPLACE "std::numeric_limits<T1>::min()" "casadi_real_min"
6 // C-REPLACE "std::numeric_limits<T1>::infinity()" "casadi_inf"
7 // C-REPLACE "static_cast<int>" "(int) "
8 // SYMBOL "qp_prob"
9 template<typename T1>
10 struct casadi_qp_prob {
11   // Sparsity patterns
12   const casadi_int *sp_a, *sp_h, *sp_at, *sp_kkt;
13   // Symbolic QR factorization
14   const casadi_int *prinv, *pc, *sp_v, *sp_r;
15   // Dimensions
16   casadi_int nx, na, nz;
17   // Smallest nonzero number
18   T1 dmin;
19   // Infinity
20   T1 inf;
21   // Smallest multiplier treated as inactive for the initial active set
22   T1 min_lam;
23   // Maximum number of iterations
24   casadi_int max_iter;
25   // Primal and dual error tolerance
26   T1 constr_viol_tol, dual_inf_tol;
27 };
28 // C-REPLACE "casadi_qp_prob<T1>" "struct casadi_qp_prob"
29 
30 // SYMBOL "qp_setup"
31 template<typename T1>
casadi_qp_setup(casadi_qp_prob<T1> * p)32 void casadi_qp_setup(casadi_qp_prob<T1>* p) {
33   p->na = p->sp_a[0];
34   p->nx = p->sp_a[1];
35   p->nz = p->nx + p->na;
36   p->dmin = std::numeric_limits<T1>::min();
37   p->inf = std::numeric_limits<T1>::infinity();
38   p->min_lam = 0;
39   p->max_iter = 1000;
40   p->constr_viol_tol = 1e-8;
41   p->dual_inf_tol = 1e-8;
42 }
43 
44 // SYMBOL "qp_work"
45 template<typename T1>
casadi_qp_work(const casadi_qp_prob<T1> * p,casadi_int * sz_iw,casadi_int * sz_w)46 void casadi_qp_work(const casadi_qp_prob<T1>* p, casadi_int* sz_iw, casadi_int* sz_w) {
47   // Local variables
48   casadi_int nnz_a, nnz_kkt, nnz_v, nnz_r;
49   // Get matrix number of nonzeros
50   nnz_a = p->sp_a[2+p->sp_a[1]];
51   nnz_kkt = p->sp_kkt[2+p->sp_kkt[1]];
52   nnz_v = p->sp_v[2+p->sp_v[1]];
53   nnz_r = p->sp_r[2+p->sp_r[1]];
54   // Reset sz_w, sz_iw
55   *sz_w = *sz_iw = 0;
56   // Temporary work vectors
57   *sz_w = casadi_max(*sz_w, p->nz); // casadi_project, tau memory
58   *sz_iw = casadi_max(*sz_iw, p->nz); // casadi_trans, tau type, allzero
59   *sz_w = casadi_max(*sz_w, 2*p->nz); // casadi_qr
60   // Persistent work vectors
61   *sz_w += nnz_kkt; // kkt
62   *sz_w += p->nz; // z=[xk,gk]
63   *sz_w += p->nz; // lbz
64   *sz_w += p->nz; // ubz
65   *sz_w += p->nz; // lam
66   *sz_w += nnz_a; // trans(a)
67   *sz_w += p->nz; // dz
68   *sz_w += p->nz; // dlam
69   *sz_w += p->nx; // infeas
70   *sz_w += p->nx; // tinfeas
71   *sz_w += p->nz; // sens
72   *sz_iw += p->nz; // neverzero
73   *sz_iw += p->nz; // neverupper
74   *sz_iw += p->nz; // neverlower
75   *sz_iw += p->nz; // lincomb
76   *sz_w += casadi_max(nnz_v+nnz_r, nnz_kkt); // [v,r] or trans(kkt)
77   *sz_w += p->nz; // beta
78 }
79 
80 // SYMBOL "qp_flag_t"
81 typedef enum {
82   QP_SUCCESS,
83   QP_MAX_ITER,
84   QP_NO_SEARCH_DIR,
85   QP_PRINTING_ERROR
86 } casadi_qp_flag_t;
87 
88 // SYMBOL "qp_data"
89 template<typename T1>
90 struct casadi_qp_data {
91   // Problem structure
92   const casadi_qp_prob<T1>* prob;
93   // Solver status
94   casadi_qp_flag_t status;
95   // Cost
96   T1 f;
97   // QP data
98   const T1 *nz_a, *nz_h, *g;
99   // Vectors
100   T1 *z, *lbz, *ubz, *infeas, *tinfeas, *sens, *lam, *w, *dz, *dlam;
101   casadi_int *iw, *neverzero, *neverlower, *neverupper, *lincomb;
102   // Numeric QR factorization
103   T1 *nz_at, *nz_kkt, *beta, *nz_v, *nz_r;
104   // Message buffer
105   const char *msg;
106   // Message index
107   casadi_int msg_ind;
108   // Stepsize
109   T1 tau;
110   // Singularity
111   casadi_int sing;
112   // Do we already have a search direction?
113   int has_search_dir;
114   // Smallest diagonal value for the QR factorization
115   T1 mina;
116   casadi_int imina;
117   // Primal and dual error, corresponding index
118   T1 pr, du, epr, edu;
119   casadi_int ipr, idu;
120   // Pending active-set change
121   casadi_int index, sign;
122   // Feasibility restoration active-set change
123   casadi_int r_index, r_sign;
124   // Iteration
125   casadi_int iter;
126 };
127 // C-REPLACE "casadi_qp_data<T1>" "struct casadi_qp_data"
128 
129 // SYMBOL "qp_init"
130 template<typename T1>
casadi_qp_init(casadi_qp_data<T1> * d,casadi_int ** iw,T1 ** w)131 void casadi_qp_init(casadi_qp_data<T1>* d, casadi_int** iw, T1** w) {
132   // Local variables
133   casadi_int nnz_a, nnz_kkt, nnz_v, nnz_r;
134   const casadi_qp_prob<T1>* p = d->prob;
135   // Get matrix number of nonzeros
136   nnz_a = p->sp_a[2+p->sp_a[1]];
137   nnz_kkt = p->sp_kkt[2+p->sp_kkt[1]];
138   nnz_v = p->sp_v[2+p->sp_v[1]];
139   nnz_r = p->sp_r[2+p->sp_r[1]];
140   d->nz_kkt = *w; *w += nnz_kkt;
141   d->z = *w; *w += p->nz;
142   d->lbz = *w; *w += p->nz;
143   d->ubz = *w; *w += p->nz;
144   d->lam = *w; *w += p->nz;
145   d->dz = *w; *w += p->nz;
146   d->dlam = *w; *w += p->nz;
147   d->nz_v = *w; *w += casadi_max(nnz_v+nnz_r, nnz_kkt);
148   d->nz_r = d->nz_v + nnz_v;
149   d->beta = *w; *w += p->nz;
150   d->nz_at = *w; *w += nnz_a;
151   d->infeas = *w; *w += p->nx;
152   d->tinfeas = *w; *w += p->nx;
153   d->sens = *w; *w += p->nz;
154   d->neverzero = *iw; *iw += p->nz;
155   d->neverupper = *iw; *iw += p->nz;
156   d->neverlower = *iw; *iw += p->nz;
157   d->lincomb = *iw; *iw += p->nz;
158   d->w = *w;
159   d->iw = *iw;
160 }
161 
162 // SYMBOL "qp_reset"
163 template<typename T1>
casadi_qp_reset(casadi_qp_data<T1> * d)164 int casadi_qp_reset(casadi_qp_data<T1>* d) {
165   // Local variables
166   casadi_int i;
167   const casadi_qp_prob<T1>* p = d->prob;
168   // Reset variables corresponding to previous iteration
169   d->msg = 0;
170   d->tau = 0.;
171   d->sing = 0;
172   // Correct lam if needed, determine permitted signs
173   for (i=0; i<p->nz; ++i) {
174     // Permitted signs for lam
175     d->neverzero[i] = d->lbz[i] == d->ubz[i];
176     d->neverupper[i] = d->ubz[i] == p->inf;
177     d->neverlower[i] = d->lbz[i] == -p->inf;
178     if (d->neverzero[i] && d->neverupper[i] && d->neverlower[i]) return 1;
179     // Small enough lambdas are treated as inactive
180     if (!d->neverzero[i] && fabs(d->lam[i]) < p->min_lam) d->lam[i] = 0.;
181     // Prevent illegal active sets
182     if (d->neverzero[i] && d->lam[i] == 0.) {
183       d->lam[i] = d->neverupper[i]
184                 || d->z[i]-d->lbz[i] <= d->ubz[i]-d->z[i] ? -p->dmin : p->dmin;
185     } else if (d->neverupper[i] && d->lam[i]>0.) {
186       d->lam[i] = d->neverzero[i] ? -p->dmin : 0.;
187     } else if (d->neverlower[i] && d->lam[i]<0.) {
188       d->lam[i] = d->neverzero[i] ? p->dmin : 0.;
189     }
190   }
191   // Transpose A
192   casadi_trans(d->nz_a, p->sp_a, d->nz_at, p->sp_at, d->iw);
193   // No pending active-set change
194   d->index = -2;
195   d->sign = 0;
196   // No restoration index
197   d->r_index = -2;
198   d->r_sign = 0;
199   // Reset iteration counter
200   d->iter = 0;
201   return 0;
202 }
203 
204 // SYMBOL "qp_pr"
205 template<typename T1>
casadi_qp_pr(casadi_qp_data<T1> * d)206 void casadi_qp_pr(casadi_qp_data<T1>* d) {
207   // Calculate largest constraint violation
208   casadi_int i;
209   const casadi_qp_prob<T1>* p = d->prob;
210   d->pr = 0;
211   d->ipr = -1;
212   for (i=0; i<p->nz; ++i) {
213     if (d->z[i] > d->ubz[i]+d->pr) {
214       d->pr = d->z[i]-d->ubz[i];
215       d->ipr = i;
216     } else if (d->z[i] < d->lbz[i]-d->pr) {
217       d->pr = d->lbz[i]-d->z[i];
218       d->ipr = i;
219     }
220   }
221 }
222 
223 // SYMBOL "qp_du"
224 template<typename T1>
casadi_qp_du(casadi_qp_data<T1> * d)225 void casadi_qp_du(casadi_qp_data<T1>* d) {
226   // Calculate largest constraint violation
227   casadi_int i;
228   const casadi_qp_prob<T1>* p = d->prob;
229   d->du = 0;
230   d->idu = -1;
231   for (i=0; i<p->nx; ++i) {
232     if (d->infeas[i] > d->du) {
233       d->du = d->infeas[i];
234       d->idu = i;
235     } else if (d->infeas[i] < -d->du) {
236       d->du = -d->infeas[i];
237       d->idu = i;
238     }
239   }
240 }
241 
242 // SYMBOL "qp_du_check"
243 template<typename T1>
casadi_qp_du_check(casadi_qp_data<T1> * d,casadi_int i)244 int casadi_qp_du_check(casadi_qp_data<T1>* d, casadi_int i) {
245   // Local variables
246   casadi_int k;
247   T1 new_du;
248   const casadi_int *at_colind, *at_row;
249   const casadi_qp_prob<T1>* p = d->prob;
250   // AT sparsity
251   at_colind = p->sp_at + 2;
252   at_row = at_colind + p->na + 1;
253   // Maximum infeasibility from setting from setting lam[i]=0
254   if (i<p->nx) {
255     new_du = fabs(d->infeas[i]-d->lam[i]);
256   } else {
257     new_du = 0.;
258     for (k=at_colind[i-p->nx]; k<at_colind[i-p->nx+1]; ++k) {
259       new_du = fmax(new_du, fabs(d->infeas[at_row[k]]-d->nz_at[k]*d->lam[i]));
260     }
261   }
262   return new_du <= d->du;
263 }
264 
265 // SYMBOL "qp_du_index"
266 template<typename T1>
casadi_qp_du_index(casadi_qp_data<T1> * d)267 void casadi_qp_du_index(casadi_qp_data<T1>* d) {
268   // Try to improve dual feasibility by removing a constraint
269   // Local variables
270   casadi_int i, s;
271   T1 best_sens;
272   const casadi_qp_prob<T1>* p = d->prob;
273   // Find the best lam[i] to make zero
274   d->index = -1;
275   best_sens = -1;
276   for (i = 0; i < p->nz; ++i) {
277     // Skip if no dual infeasibility sensitivity
278     if (d->sens[i] == 0.) continue;
279     // Is the constraint enforced?
280     if (d->lam[i] == 0) {
281       // We're enforcing constraints
282       s = d->sens[i] > 0 ? 1 : -1;
283       // Make sure that enforcing the constraint is possible
284       if (s > 0 ? d->neverupper[i] : d->neverlower[i]) continue;
285     } else {
286       // We're removing constraints
287       s = 0;
288       // Make sure that it's a constraint that can be removed
289       if (d->neverzero[i]) continue;
290       // If variable influences du, make sure sign is right
291       if (d->lam[i] > 0. ? d->sens[i] > 0. : d->sens[i] < 0.) continue;
292       // Skip if maximum infeasibility increases
293       if (!casadi_qp_du_check(d, i)) continue;
294     }
295     // Check if best so far
296     if (fabs(d->sens[i]) > best_sens) {
297       best_sens = fabs(d->sens[i]);
298       d->index = i;
299       d->sign = s;
300     }
301   }
302   // Accept, if any
303   if (d->index >= 0) {
304     if (d->sign > 0) {
305       d->msg = "Enforced ubz to reduce |du|";
306     } else if (d->sign < 0) {
307       d->msg = "Enforced lbz to reduce |du|";
308     } else if (d->lam[d->index] > 0) {
309       d->msg = "Dropped ubz to reduce |du|";
310     } else {
311       d->msg = "Dropped lbz to reduce |du|";
312     }
313     d->msg_ind = d->index;
314   }
315 }
316 
317 // SYMBOL "qp_pr_index"
318 template<typename T1>
casadi_qp_pr_index(casadi_qp_data<T1> * d)319 void casadi_qp_pr_index(casadi_qp_data<T1>* d) {
320   // Try to improve primal feasibility by adding a constraint
321   if (d->lam[d->ipr] == 0.) {
322     // Add the most violating constraint
323     if (d->z[d->ipr] < d->lbz[d->ipr]) {
324       d->sign = -1;
325       d->msg = "Added lbz to reduce |pr|";
326     } else {
327       d->sign = 1;
328       d->msg = "Added ubz to reduce |pr|";
329     }
330     d->msg_ind = d->ipr;
331     d->index = d->ipr;
332   } else {
333     // No improvement possible
334     d->index = -1;
335   }
336 }
337 
338 // SYMBOL "qp_kkt"
339 template<typename T1>
casadi_qp_kkt(casadi_qp_data<T1> * d)340 void casadi_qp_kkt(casadi_qp_data<T1>* d) {
341   // Local variables
342   casadi_int i, k;
343   const casadi_int *h_colind, *h_row, *a_colind, *a_row, *at_colind, *at_row,
344                    *kkt_colind, *kkt_row;
345   const casadi_qp_prob<T1>* p = d->prob;
346   // Extract sparsities
347   a_row = (a_colind = p->sp_a+2) + p->nx + 1;
348   at_row = (at_colind = p->sp_at+2) + p->na + 1;
349   h_row = (h_colind = p->sp_h+2) + p->nx + 1;
350   kkt_row = (kkt_colind = p->sp_kkt+2) + p->nz + 1;
351   // Reset w to zero
352   casadi_clear(d->w, p->nz);
353   // Loop over rows of the (transposed) KKT
354   for (i=0; i<p->nz; ++i) {
355     // Copy row of KKT to w
356     if (i<p->nx) {
357       if (d->lam[i]==0) {
358         for (k=h_colind[i]; k<h_colind[i+1]; ++k) d->w[h_row[k]] = d->nz_h[k];
359         for (k=a_colind[i]; k<a_colind[i+1]; ++k) d->w[p->nx+a_row[k]] = d->nz_a[k];
360       } else {
361         d->w[i] = 1.;
362       }
363     } else {
364       if (d->lam[i]==0) {
365         d->w[i] = -1.;
366       } else {
367         for (k=at_colind[i-p->nx]; k<at_colind[i-p->nx+1]; ++k) {
368           d->w[at_row[k]] = d->nz_at[k];
369         }
370       }
371     }
372     // Copy row to KKT, zero out w
373     for (k=kkt_colind[i]; k<kkt_colind[i+1]; ++k) {
374       d->nz_kkt[k] = d->w[kkt_row[k]];
375       d->w[kkt_row[k]] = 0;
376     }
377   }
378 }
379 
380 // SYMBOL "qp_kkt_vector"
381 template<typename T1>
casadi_qp_kkt_vector(casadi_qp_data<T1> * d,T1 * kkt_i,casadi_int i)382 void casadi_qp_kkt_vector(casadi_qp_data<T1>* d, T1* kkt_i, casadi_int i) {
383   // Local variables
384   casadi_int k;
385   const casadi_int *h_colind, *h_row, *a_colind, *a_row, *at_colind, *at_row;
386   const casadi_qp_prob<T1>* p = d->prob;
387   // Extract sparsities
388   a_row = (a_colind = p->sp_a+2) + p->nx + 1;
389   at_row = (at_colind = p->sp_at+2) + p->na + 1;
390   h_row = (h_colind = p->sp_h+2) + p->nx + 1;
391   // Reset kkt_i to zero
392   casadi_clear(kkt_i, p->nz);
393   // Copy sparse entries
394   if (i<p->nx) {
395     for (k=h_colind[i]; k<h_colind[i+1]; ++k) kkt_i[h_row[k]] = d->nz_h[k];
396     for (k=a_colind[i]; k<a_colind[i+1]; ++k) kkt_i[p->nx+a_row[k]] = d->nz_a[k];
397   } else {
398     for (k=at_colind[i-p->nx]; k<at_colind[i-p->nx+1]; ++k) {
399       kkt_i[at_row[k]] = -d->nz_at[k];
400     }
401   }
402   // Add diagonal entry
403   kkt_i[i] -= 1.;
404 }
405 
406 // SYMBOL "qp_kkt_dot"
407 template<typename T1>
casadi_qp_kkt_dot(casadi_qp_data<T1> * d,const T1 * v,casadi_int i)408 T1 casadi_qp_kkt_dot(casadi_qp_data<T1>* d, const T1* v, casadi_int i) {
409   // Local variables
410   casadi_int k;
411   const casadi_int *h_colind, *h_row, *a_colind, *a_row, *at_colind, *at_row;
412   T1 r;
413   const casadi_qp_prob<T1>* p = d->prob;
414   // Extract sparsities
415   a_row = (a_colind = p->sp_a + 2) + p->nx + 1;
416   at_row = (at_colind = p->sp_at + 2) + p->na + 1;
417   h_row = (h_colind = p->sp_h + 2) + p->nx + 1;
418   // Scalar product with the diagonal
419   r = v[i];
420   // Scalar product with the sparse entries
421   if (i < p->nx) {
422     for (k=h_colind[i]; k<h_colind[i+1]; ++k) r -= v[h_row[k]] * d->nz_h[k];
423     for (k=a_colind[i]; k<a_colind[i+1]; ++k) r -= v[p->nx+a_row[k]] * d->nz_a[k];
424   } else {
425     for (k=at_colind[i-p->nx]; k<at_colind[i-p->nx+1]; ++k) {
426       r += v[at_row[k]] * d->nz_at[k];
427     }
428   }
429   return r;
430 }
431 
432 // SYMBOL "qp_kkt_residual"
433 template<typename T1>
casadi_qp_kkt_residual(casadi_qp_data<T1> * d,T1 * r)434 void casadi_qp_kkt_residual(casadi_qp_data<T1>* d, T1* r) {
435   casadi_int i;
436   const casadi_qp_prob<T1>* p = d->prob;
437   for (i=0; i<p->nz; ++i) {
438     if (d->lam[i]>0.) {
439       r[i] = d->ubz[i]-d->z[i];
440     } else if (d->lam[i]<0.) {
441       r[i] = d->lbz[i]-d->z[i];
442     } else if (i<p->nx) {
443       r[i] = d->lam[i]-d->infeas[i];
444     } else {
445       r[i] = d->lam[i];
446     }
447   }
448 }
449 
450 // SYMBOL "qp_zero_blocking"
451 template<typename T1>
casadi_qp_zero_blocking(casadi_qp_data<T1> * d)452 int casadi_qp_zero_blocking(casadi_qp_data<T1>* d) {
453   // Local variables
454   casadi_int i;
455   T1 dz_max = 0;
456   const casadi_qp_prob<T1>* p = d->prob;
457   // Look for violated constraints that are not improving
458   for (i = 0; i < p->nz; ++i) {
459     if (d->dz[i] < -dz_max && d->lbz[i] - d->z[i] >= d->epr) {
460       dz_max = -d->dz[i];
461       d->index = i;
462       d->sign = -1;
463       d->msg = "lbz violated with zero step";
464       d->msg_ind = d->index;
465     } else if (d->dz[i] > dz_max && d->z[i] - d->ubz[i] >= d->epr) {
466       dz_max = d->dz[i];
467       d->index = i;
468       d->sign = 1;
469       d->msg = "ubz violated with zero step";
470       d->msg_ind = d->index;
471     }
472   }
473   return dz_max > 0;
474 }
475 
476 // SYMBOL "qp_primal_blocking"
477 template<typename T1>
casadi_qp_primal_blocking(casadi_qp_data<T1> * d)478 void casadi_qp_primal_blocking(casadi_qp_data<T1>* d) {
479   // Local variables
480   casadi_int i;
481   T1 trial_z;
482   const casadi_qp_prob<T1>* p = d->prob;
483   // Check if violation with tau=0 and not improving
484   if (casadi_qp_zero_blocking(d)) {
485     d->tau = 0.;
486     return;
487   }
488   // Loop over all primal variables
489   for (i = 0; i < p->nz; ++i) {
490     if (d->dz[i] == 0.) continue; // Skip zero steps
491     // Trial primal step
492     trial_z = d->z[i] + d->tau * d->dz[i];
493     if (d->dz[i] < 0 && trial_z < d->lbz[i] - d->epr) {
494       // Trial would increase maximum infeasibility
495       d->tau = (d->lbz[i] - d->epr - d->z[i]) / d->dz[i];
496       d->index = d->lam[i] < 0. ? -1 : i;
497       d->sign = -1;
498       d->msg = "Enforcing lbz";
499       d->msg_ind = i;
500     } else if (d->dz[i] > 0 && trial_z > d->ubz[i] + d->epr) {
501       // Trial would increase maximum infeasibility
502       d->tau = (d->ubz[i] + d->epr - d->z[i]) / d->dz[i];
503       d->index = d->lam[i] > 0. ? -1 : i;
504       d->sign = 1;
505       d->msg = "Enforcing ubz";
506       d->msg_ind = i;
507     }
508     if (d->tau <= 0) return;
509   }
510 }
511 
512 // SYMBOL "qp_dual_breakpoints"
513 template<typename T1>
casadi_qp_dual_breakpoints(casadi_qp_data<T1> * d,T1 * tau_list,casadi_int * ind_list,T1 tau)514 casadi_int casadi_qp_dual_breakpoints(casadi_qp_data<T1>* d, T1* tau_list,
515                                       casadi_int* ind_list, T1 tau) {
516   // Local variables
517   casadi_int i, n_tau, loc, next_ind, tmp_ind, j;
518   T1 trial_lam, new_tau, next_tau, tmp_tau;
519   const casadi_qp_prob<T1>* p = d->prob;
520   // Dual feasibility is piecewise linear. Start with one interval [0,tau]:
521   tau_list[0] = tau;
522   ind_list[0] = -1; // no associated index
523   n_tau = 1;
524   // Find the taus corresponding to lam crossing zero and insert into list
525   for (i=0; i<p->nz; ++i) {
526     if (d->dlam[i]==0.) continue; // Skip zero steps
527     if (d->lam[i]==0.) continue; // Skip inactive constraints
528     // Trial dual step
529     trial_lam = d->lam[i] + tau*d->dlam[i];
530     // Skip if no sign change
531     if (d->lam[i]>0 ? trial_lam>=0 : trial_lam<=0) continue;
532     // Location of the sign change
533     new_tau = -d->lam[i]/d->dlam[i];
534     // Where to insert the w[i]
535     for (loc=0; loc<n_tau-1; ++loc) {
536       if (new_tau<tau_list[loc]) break;
537     }
538     // Insert element
539     n_tau++;
540     next_tau=new_tau;
541     next_ind=i;
542     for (j=loc; j<n_tau; ++j) {
543       tmp_tau = tau_list[j];
544       tau_list[j] = next_tau;
545       next_tau = tmp_tau;
546       tmp_ind = ind_list[j];
547       ind_list[j] = next_ind;
548       next_ind = tmp_ind;
549     }
550   }
551   return n_tau;
552 }
553 
554 // SYMBOL "qp_dual_blocking"
555 template<typename T1>
casadi_qp_dual_blocking(casadi_qp_data<T1> * d)556 casadi_int casadi_qp_dual_blocking(casadi_qp_data<T1>* d) {
557   // Local variables
558   casadi_int i, n_tau, j, k, du_index;
559   T1 tau_k, dtau, new_infeas, tau1, infeas, tinfeas;
560   const casadi_int *at_colind, *at_row;
561   const casadi_qp_prob<T1>* p = d->prob;
562   // Extract sparsities
563   at_row = (at_colind = p->sp_at+2) + p->na + 1;
564   // Dual feasibility is piecewise linear in tau. Get the intervals:
565   n_tau = casadi_qp_dual_breakpoints(d, d->w, d->iw, d->tau);
566   // No dual blocking yet
567   du_index = -1;
568   // How long step can we take without exceeding e?
569   tau_k = 0.;
570   for (j=0; j<n_tau; ++j) {
571     // Distance to the next tau (may be zero)
572     dtau = d->w[j] - tau_k;
573     // Check if maximum dual infeasibilty gets exceeded
574     for (k=0; k<p->nx; ++k) {
575       // Get infeasibility and infeasibility tangent
576       infeas  = d->infeas[k];
577       tinfeas  = d->tinfeas[k];
578       // Make sure tinfeas>0
579       if (fabs(tinfeas)<1e-14) {
580         // Skip
581         continue;
582       } else if (tinfeas<0) {
583         // Switch signs
584         infeas *= -1;
585         tinfeas *= -1;
586       }
587       // Tentative new infeasibility
588       new_infeas = infeas + dtau*tinfeas;
589       // Does infeasibility get exceeded
590       if (new_infeas > d->edu) {
591         // Sign change and exceeded
592         tau1 = fmax(tau_k, tau_k + (d->edu - infeas)/tinfeas);
593         if (tau1 < d->tau) {
594           // Enforce dual blocking constraint
595           d->tau = tau1;
596           du_index = k;
597         }
598       }
599     }
600     // Update infeasibility
601     casadi_axpy(p->nx, fmin(d->tau - tau_k, dtau), d->tinfeas, d->infeas);
602     // Stop here if dual blocking constraint
603     if (du_index>=0) return du_index;
604     // Continue to the next tau
605     tau_k = d->w[j];
606     // Get component, break if last
607     i = d->iw[j];
608     if (i<0) break;
609     // Update sign or tinfeas
610     if (!d->neverzero[i]) {
611       // lam becomes zero, update the infeasibility tangent
612       if (i<p->nx) {
613         // Set a lam_x to zero
614         d->tinfeas[i] -= d->dlam[i];
615       } else {
616         // Set a lam_a to zero
617         for (k=at_colind[i-p->nx]; k<at_colind[i-p->nx+1]; ++k) {
618           d->tinfeas[at_row[k]] -= d->nz_at[k]*d->dlam[i];
619         }
620       }
621     }
622   }
623   return du_index;
624 }
625 
626 // SYMBOL "qp_take_step"
627 template<typename T1>
casadi_qp_take_step(casadi_qp_data<T1> * d)628 void casadi_qp_take_step(casadi_qp_data<T1>* d) {
629   // Local variables
630   casadi_int i;
631   const casadi_qp_prob<T1>* p = d->prob;
632   // Get current sign
633   for (i=0; i<p->nz; ++i) d->iw[i] = d->lam[i]>0. ? 1 : d->lam[i]<0 ? -1 : 0;
634   // Take primal-dual step
635   casadi_axpy(p->nz, d->tau, d->dz, d->z);
636   casadi_axpy(p->nz, d->tau, d->dlam, d->lam);
637   // Update sign
638   for (i=0; i<p->nz; ++i) {
639     // Allow sign changes for certain components
640     if (d->neverzero[i] && (d->iw[i]<0 ? d->lam[i]>0 : d->lam[i]<0)) {
641       d->iw[i]=-d->iw[i];
642     }
643     // Ensure correct sign
644     switch (d->iw[i]) {
645       case -1: d->lam[i] = fmin(d->lam[i], -p->dmin); break;
646       case  1: d->lam[i] = fmax(d->lam[i],  p->dmin); break;
647       case  0: d->lam[i] = 0.; break;
648     }
649   }
650 }
651 
652 // SYMBOL "qp_flip_check"
653 template<typename T1>
casadi_qp_flip_check(casadi_qp_data<T1> * d)654 int casadi_qp_flip_check(casadi_qp_data<T1>* d) {
655   const casadi_qp_prob<T1>* p = d->prob;
656   // Calculate the difference between unenforced and enforced column index
657   casadi_qp_kkt_vector(d, d->dlam, d->index);
658   // Calculate the difference between old and new column index
659   if (d->sign == 0) casadi_scal(p->nz, -1., d->dlam);
660   // Try to find a linear combination of the new columns
661   casadi_qr_solve(d->dlam, 1, 0, p->sp_v, d->nz_v, p->sp_r, d->nz_r, d->beta,
662     p->prinv, p->pc, d->w);
663   // If dlam[index]!=1, new columns must be linearly independent
664   if (fabs(d->dlam[d->index]-1.) >= 1e-12) return 0;
665   // Next, find a linear combination of the new rows
666   casadi_clear(d->dz, p->nz);
667   d->dz[d->index] = 1;
668   casadi_qr_solve(d->dz, 1, 1, p->sp_v, d->nz_v, p->sp_r, d->nz_r, d->beta,
669     p->prinv, p->pc, d->w);
670   // Normalize dlam, dz
671   casadi_scal(p->nz, 1./sqrt(casadi_dot(p->nz, d->dlam, d->dlam)), d->dlam);
672   casadi_scal(p->nz, 1./sqrt(casadi_dot(p->nz, d->dz, d->dz)), d->dz);
673   // KKT system will be singular
674   return 1;
675 }
676 
677 // SYMBOL "qp_factorize"
678 template<typename T1>
casadi_qp_factorize(casadi_qp_data<T1> * d)679 void casadi_qp_factorize(casadi_qp_data<T1>* d) {
680   const casadi_qp_prob<T1>* p = d->prob;
681   // Do we already have a search direction due to lost singularity?
682   if (d->has_search_dir) {
683     d->sing = 1;
684     return;
685   }
686   // Construct the KKT matrix
687   casadi_qp_kkt(d);
688   // QR factorization
689   casadi_qr(p->sp_kkt, d->nz_kkt, d->w, p->sp_v, d->nz_v, p->sp_r,
690             d->nz_r, d->beta, p->prinv, p->pc);
691   // Check singularity
692   d->sing = casadi_qr_singular(&d->mina, &d->imina, d->nz_r, p->sp_r, p->pc, 1e-12);
693 }
694 
695 // SYMBOL "qp_expand_step"
696 template<typename T1>
casadi_qp_expand_step(casadi_qp_data<T1> * d)697 void casadi_qp_expand_step(casadi_qp_data<T1>* d) {
698   // Local variables
699   casadi_int i;
700   const casadi_qp_prob<T1>* p = d->prob;
701   // Calculate change in Lagrangian gradient
702   casadi_clear(d->dlam, p->nx);
703   casadi_mv(d->nz_h, p->sp_h, d->dz, d->dlam, 0); // gradient of the objective
704   casadi_mv(d->nz_a, p->sp_a, d->dz + p->nx, d->dlam, 1); // gradient of the Lagrangian
705   // Step in lam[:nx]
706   casadi_scal(p->nx, -1., d->dlam);
707   // For inactive constraints, lam(x) step is zero
708   for (i = 0; i < p->nx; ++i) if (d->lam[i] == 0.) d->dlam[i] = 0.;
709   // Step in lam[nx:]
710   casadi_copy(d->dz+p->nx, p->na, d->dlam + p->nx);
711   // Step in z[nx:]
712   casadi_clear(d->dz + p->nx, p->na);
713   casadi_mv(d->nz_a, p->sp_a, d->dz, d->dz + p->nx, 0);
714   // Avoid steps that are nonzero due to numerics
715   for (i = 0; i < p->nz; ++i) if (fabs(d->dz[i]) < 1e-14) d->dz[i] = 0.;
716   // Tangent of the dual infeasibility at tau=0
717   casadi_clear(d->tinfeas, p->nx);
718   casadi_mv(d->nz_h, p->sp_h, d->dz, d->tinfeas, 0);
719   casadi_mv(d->nz_a, p->sp_a, d->dlam + p->nx, d->tinfeas, 1);
720   casadi_axpy(p->nx, 1., d->dlam, d->tinfeas);
721 }
722 
723 // SYMBOL "casadi_qp_pr_direction"
724 template<typename T1>
casadi_qp_pr_direction(casadi_qp_data<T1> * d)725 int casadi_qp_pr_direction(casadi_qp_data<T1>* d) {
726   casadi_int i;
727   const casadi_qp_prob<T1>* p = d->prob;
728   for (i=0; i<p->nz; ++i) {
729     if (d->lbz[i] - d->z[i] >= d->epr) {
730       // Prevent further violation of lower bound
731       if (d->dz[i] < 0 || d->dlam[i] > 0) return 1;
732     } else if (d->z[i] - d->ubz[i] >= d->epr) {
733       // Prevent further violation of upper bound
734       if (d->dz[i] > 0 || d->dlam[i] < 0) return 1;
735     }
736   }
737   return 0;
738 }
739 
740 // SYMBOL "casadi_qp_du_direction"
741 template<typename T1>
casadi_qp_du_direction(casadi_qp_data<T1> * d)742 int casadi_qp_du_direction(casadi_qp_data<T1>* d) {
743   casadi_int i;
744   const casadi_qp_prob<T1>* p = d->prob;
745   for (i=0; i<p->nx; ++i) {
746     // Prevent further increase in dual infeasibility
747     if (d->infeas[i] <= -d->edu && d->tinfeas[i] < -1e-12) {
748       return 1;
749     } else if (d->infeas[i] >= d->edu && d->tinfeas[i] > 1e-12) {
750       return 1;
751     }
752   }
753   return 0;
754 }
755 
756 // SYMBOL "qp_enforceable"
757 template<typename T1>
casadi_qp_enforceable(casadi_qp_data<T1> * d,casadi_int i,casadi_int s)758 int casadi_qp_enforceable(casadi_qp_data<T1>* d, casadi_int i, casadi_int s) {
759   // Local variables
760   casadi_int k;
761   const casadi_int *at_colind, *at_row;
762   const casadi_qp_prob<T1>* p = d->prob;
763   // Can always enforce if not at bound
764   if (fabs(d->infeas[i]) < d->edu) return 1;
765   // AT sparsity
766   at_colind = p->sp_at + 2;
767   at_row = at_colind + p->na + 1;
768   // Can we set lam[i] := s*DMIN without exceeding edu?
769   if (i<p->nx) {
770     return (s < 0) == (d->infeas[i] > 0);
771   } else {
772     for (k=at_colind[i-p->nx]; k<at_colind[i-p->nx+1]; ++k) {
773       if (d->nz_at[k] > 0) {
774         if ((s > 0) == (d->infeas[at_row[k]] > 0)) return 0;
775       } else if (d->nz_at[k] < 0) {
776         if ((s < 0) == (d->infeas[at_row[k]] > 0)) return 0;
777       }
778     }
779     return 1;
780   }
781 }
782 
783 // SYMBOL "qp_singular_step"
784 // C-REPLACE "static_cast<T1*>(0)" "0"
785 template<typename T1>
casadi_qp_singular_step(casadi_qp_data<T1> * d)786 int casadi_qp_singular_step(casadi_qp_data<T1>* d) {
787   // Local variables
788   T1 tau_test, tau;
789   casadi_int nnz_kkt, nk, k, i, best_k, best_neg, neg;
790   const casadi_qp_prob<T1>* p = d->prob;
791   // Find the columns that take part in any linear combination
792   for (i = 0; i < p->nz; ++i) d->lincomb[i] = 0;
793   for (k = 0; k < d->sing; ++k) {
794     if (!d->has_search_dir) {
795       casadi_qr_colcomb(d->dlam, d->nz_r, p->sp_r, p->pc, 1e-12, k);
796     }
797     for (i = 0; i < p->nz; ++i) if (fabs(d->dlam[i]) >= 1e-12) d->lincomb[i]++;
798   }
799 
800   if (d->has_search_dir) {
801     // One, given search direction
802     nk = 1;
803   } else {
804     // QR factorization of the transpose
805     casadi_trans(d->nz_kkt, p->sp_kkt, d->nz_v, p->sp_kkt, d->iw);
806     nnz_kkt = p->sp_kkt[2+p->nz]; // kkt_colind[nz]
807     casadi_copy(d->nz_v, nnz_kkt, d->nz_kkt);
808     casadi_qr(p->sp_kkt, d->nz_kkt, d->w, p->sp_v, d->nz_v, p->sp_r, d->nz_r,
809               d->beta, p->prinv, p->pc);
810     // For all nullspace vectors
811     nk = casadi_qr_singular(static_cast<T1*>(0), 0, d->nz_r, p->sp_r, p->pc, 1e-12);
812   }
813   // Best flip
814   best_k = best_neg = -1;
815   tau = p->inf;
816   for (k=0; k<nk; ++k) {
817     if (!d->has_search_dir) {
818       // Get a linear combination of the rows in kkt
819       casadi_qr_colcomb(d->dz, d->nz_r, p->sp_r, p->pc, 1e-12, k);
820     }
821     // Which constraints can be flipped in order to increase rank?
822     for (i=0; i<p->nz; ++i) {
823       d->iw[i] = d->lincomb[i] && fabs(casadi_qp_kkt_dot(d, d->dz, i)) > 1e-12;
824     }
825     // Calculate step, dz and dlam
826     casadi_qp_expand_step(d);
827     // Try both positive and negative direction
828     for (neg = 0; neg < 2; ++neg) {
829       // Negate direction
830       if (neg) {
831         casadi_scal(p->nz, -1., d->dz);
832         casadi_scal(p->nz, -1., d->dlam);
833         casadi_scal(p->nx, -1., d->tinfeas);
834       }
835       // Make sure primal infeasibility doesn't exceed limits
836       if (casadi_qp_pr_direction(d)) continue;
837       // Make sure dual infeasibility doesn't exceed limits
838       if (casadi_qp_du_direction(d)) continue;
839       // Loop over potential active set changes
840       for (i=0; i<p->nz; ++i) {
841         // Skip if no rank increase
842         if (!d->iw[i]) continue;
843         // Enforced or not?
844         if (d->lam[i]==0.) {
845           if (d->z[i] <= d->ubz[i] && (d->z[i] >= d->lbz[i] ?
846               d->dz[i] < -1e-12 : d->dz[i] > 1e-12)) {
847             // Enforce lower bound?
848             if (!d->neverlower[i]
849                 && (tau_test = (d->lbz[i] - d->z[i]) / d->dz[i]) < tau
850                 && casadi_qp_enforceable(d, i, -1)) {
851               tau = tau_test;
852               d->r_index = i;
853               d->r_sign = -1;
854               best_k = k;
855               best_neg = neg;
856             }
857           } else if (d->z[i] >= d->lbz[i] && (d->z[i] <= d->ubz[i] ?
858               d->dz[i] > 1e-12 : d->dz[i] < -1e-12)) {
859             // Enforce upper bound?
860             if (!d->neverupper[i]
861                 && (tau_test = (d->ubz[i] - d->z[i]) / d->dz[i]) < tau
862                 && casadi_qp_enforceable(d, i, 1)) {
863               tau = tau_test;
864               d->r_index = i;
865               d->r_sign = 1;
866               best_k = k;
867               best_neg = neg;
868             }
869           }
870         } else if (!d->neverzero[i]) {
871           // Drop a constraint?
872           if (d->lam[i] > 0 ? d->dlam[i] < -1e-12 : d->dlam[i] > 1e-12) {
873             if ((tau_test = -d->lam[i] / d->dlam[i]) < tau) {
874               tau = tau_test;
875               d->r_index = i;
876               d->r_sign = 0;
877               best_k = k;
878               best_neg = neg;
879             }
880           }
881         }
882       }
883     }
884   }
885   // Can we restore feasibility?
886   if (d->r_index < 0) return 1;
887   // Recalculate direction, if needed
888   if (--k != best_k) {
889     // Need to recalculate direction
890     casadi_qr_colcomb(d->dz, d->nz_r, p->sp_r, p->pc, 1e-12, best_k);
891     casadi_qp_expand_step(d);
892     if (best_neg) tau *= -1;
893   } else if (--neg != best_neg) {
894     // No need to recalculate, but opposite direction
895     tau *= -1;
896   }
897   // Scale step so that that tau=1 corresponds to a full step
898   casadi_scal(p->nz, tau, d->dz);
899   casadi_scal(p->nz, tau, d->dlam);
900   casadi_scal(p->nx, tau, d->tinfeas);
901   return 0;
902 }
903 
904 // SYMBOL "qp_calc_step"
905 template<typename T1>
casadi_qp_calc_step(casadi_qp_data<T1> * d)906 int casadi_qp_calc_step(casadi_qp_data<T1>* d) {
907   // Local variables
908   const casadi_qp_prob<T1>* p = d->prob;
909   // Reset returns
910   d->r_index = -1;
911   d->r_sign = 0;
912   // Handle singularity
913   if (d->sing) return casadi_qp_singular_step(d);
914   // Negative KKT residual
915   casadi_qp_kkt_residual(d, d->dz);
916   // Solve to get step in z[:nx] and lam[nx:]
917   casadi_qr_solve(d->dz, 1, 1, p->sp_v, d->nz_v, p->sp_r, d->nz_r, d->beta,
918                   p->prinv, p->pc, d->w);
919   // Have step in dz[:nx] and dlam[nx:]. Calculate complete dz and dlam
920   casadi_qp_expand_step(d);
921   // Successful return
922   return 0;
923 }
924 
925 // SYMBOL "qp_calc_sens"
926 template<typename T1>
casadi_qp_calc_sens(casadi_qp_data<T1> * d,casadi_int i)927 void casadi_qp_calc_sens(casadi_qp_data<T1>* d, casadi_int i) {
928   // Local variables
929   const casadi_qp_prob<T1>* p = d->prob;
930   // Calculate sensitivities in decreasing dual infeasibility index i
931   casadi_clear(d->sens, p->nz);
932   if (i >= 0) {
933     d->sens[i] = d->infeas[i] > 0 ? -1. : 1.;
934     casadi_mv(d->nz_a, p->sp_a, d->sens, d->sens + p->nx, 0);
935   }
936 }
937 
938 // SYMBOL "qp_calc_dependent"
939 template<typename T1>
casadi_qp_calc_dependent(casadi_qp_data<T1> * d)940 void casadi_qp_calc_dependent(casadi_qp_data<T1>* d) {
941   // Local variables
942   casadi_int i;
943   T1 r;
944   const casadi_qp_prob<T1>* p = d->prob;
945   // Calculate f
946   d->f = casadi_bilin(d->nz_h, p->sp_h, d->z, d->z)/2.
947        + casadi_dot(p->nx, d->z, d->g);
948   // Calculate z[nx:]
949   casadi_clear(d->z+p->nx, p->na);
950   casadi_mv(d->nz_a, p->sp_a, d->z, d->z+p->nx, 0);
951   // Calculate gradient of the Lagrangian
952   casadi_copy(d->g, p->nx, d->infeas);
953   casadi_mv(d->nz_h, p->sp_h, d->z, d->infeas, 0);
954   casadi_mv(d->nz_a, p->sp_a, d->lam+p->nx, d->infeas, 1);
955   // Calculate lam[:nx] without changing the sign accidentally, dual infeasibility
956   for (i=0; i<p->nx; ++i) {
957     // No change if zero
958     if (d->lam[i]==0) continue;
959     // lam[i] with no sign restrictions
960     r = -d->infeas[i];
961     if (d->lam[i]>0) {
962       if (d->neverzero[i] && !d->neverlower[i]) {
963         d->lam[i] = r==0 ? p->dmin : r; // keep sign if r==0
964       } else {
965         d->lam[i] = fmax(r, p->dmin); // no sign change
966       }
967     } else {
968       if (d->neverzero[i] && !d->neverupper[i]) {
969         d->lam[i] = r==0 ? -p->dmin : r; // keep sign if r==0
970       } else {
971         d->lam[i] = fmin(r, -p->dmin); // no sign change
972       }
973     }
974     // Update dual infeasibility
975     d->infeas[i] += d->lam[i];
976   }
977   // Calculate primal and dual error
978   casadi_qp_pr(d);
979   casadi_qp_du(d);
980   // Acceptable primal and dual error
981   d->epr = fmax(d->pr, (0.5 * p->constr_viol_tol / p->dual_inf_tol) * d->du);
982   d->edu = fmax(d->du, (0.5 * p->dual_inf_tol / p->constr_viol_tol) * d->pr);
983   // Sensitivity in decreasing |du|
984   casadi_qp_calc_sens(d, d->idu);
985 }
986 
987 // SYMBOL "qp_linesearch"
988 template<typename T1>
casadi_qp_linesearch(casadi_qp_data<T1> * d)989 void casadi_qp_linesearch(casadi_qp_data<T1>* d) {
990   // Local variables
991   casadi_int du_index;
992   // Start with a full step and no active set change
993   d->sign = 0;
994   d->index = -1;
995   d->tau = 1.;
996   // Find largest possible step without exceeding acceptable |pr|
997   casadi_qp_primal_blocking(d);
998   // Find largest possible step without exceeding acceptable |du|
999   du_index = casadi_qp_dual_blocking(d);
1000   // Take primal-dual step, avoiding accidental sign changes for lam
1001   casadi_qp_take_step(d);
1002   // Handle dual blocking constraints
1003   if (du_index >= 0) {
1004     // Sensititivity in decreasing du_index
1005     casadi_qp_calc_sens(d, du_index);
1006     // Find corresponding index
1007     casadi_qp_du_index(d);
1008   }
1009 }
1010 
1011 // SYMBOL "qp_flip"
1012 template<typename T1>
casadi_qp_flip(casadi_qp_data<T1> * d)1013 void casadi_qp_flip(casadi_qp_data<T1>* d) {
1014   // Local variables
1015   const casadi_qp_prob<T1>* p = d->prob;
1016   // Try to restore regularity if possible
1017   if (d->index == -1 && d->r_index >= 0) {
1018     if (d->r_sign != 0 || casadi_qp_du_check(d, d->r_index)) {
1019       d->index = d->r_index;
1020       d->sign = d->r_sign;
1021       if (d->sign > 0) {
1022         d->msg = "Enforced ubz for regularity";
1023       } else if (d->sign < 0) {
1024         d->msg = "Enforced lbz for regularity";
1025       } else if (d->lam[d->index] > 0) {
1026         d->msg = "Dropped ubz for regularity";
1027       } else {
1028         d->msg = "Dropped lbz for regularity";
1029       }
1030       d->msg_ind = d->index;
1031     }
1032   }
1033   // If nonsingular and nonzero error, try to flip a constraint
1034   if (!d->sing && d->index == -1) {
1035     if (d->pr * p->dual_inf_tol >= p->constr_viol_tol * d->du) {
1036       // Improve primal feasibility if dominating
1037       if (d->pr >= p->constr_viol_tol) casadi_qp_pr_index(d);
1038     } else {
1039       // Improve dual feasibility if dominating
1040       if (d->du >= p->dual_inf_tol) casadi_qp_du_index(d);
1041     }
1042   }
1043   // No search direction given by default
1044   d->has_search_dir = 0;
1045   // If a constraint was added
1046   if (d->index >= 0) {
1047     // Detect singularity before it happens and get nullspace vectors
1048     if (!d->sing) d->has_search_dir = casadi_qp_flip_check(d);
1049     // Perform the active-set change
1050     d->lam[d->index] = d->sign==0 ? 0 : d->sign > 0 ? p->dmin : -p->dmin;
1051     // Recalculate primal and dual infeasibility
1052     casadi_qp_calc_dependent(d);
1053   }
1054 }
1055 
1056 // SYMBOL "qp_prepare"
1057 template<typename T1>
casadi_qp_prepare(casadi_qp_data<T1> * d)1058 int casadi_qp_prepare(casadi_qp_data<T1>* d) {
1059   // Local variables
1060   const casadi_qp_prob<T1>* p = d->prob;
1061   // Calculate dependent quantities
1062   casadi_qp_calc_dependent(d);
1063   // Make an active set change
1064   casadi_qp_flip(d);
1065   // Form and factorize the KKT system
1066   casadi_qp_factorize(d);
1067   // Termination message
1068   if (!d->sing && d->index == -1) {
1069     d->status = QP_SUCCESS;
1070     d->msg = "Converged";
1071     d->msg_ind = -2;
1072     return 1;
1073   } else if (d->iter >= p->max_iter) {
1074     d->status = QP_MAX_ITER;
1075     d->msg = "Max iter";
1076     d->msg_ind = -2;
1077     return 1;
1078   } else if (!d->sing && d->ipr < 0 && d->idu < 0) {
1079     d->status = QP_SUCCESS;
1080     d->msg = "No primal or dual error";
1081     d->msg_ind = -2;
1082     return 1;
1083   } else {
1084     // Keep iterating
1085     return 0;
1086   }
1087 }
1088 
1089 // SYMBOL "qp_iterate"
1090 template<typename T1>
casadi_qp_iterate(casadi_qp_data<T1> * d)1091 int casadi_qp_iterate(casadi_qp_data<T1>* d) {
1092   // Reset message flag
1093   d->msg = 0;
1094   // Start a new iteration
1095   d->iter++;
1096   // Calculate search direction
1097   if (casadi_qp_calc_step(d)) {
1098     d->status = QP_NO_SEARCH_DIR;
1099     return 1;
1100   }
1101   // Line search in the calculated direction
1102   casadi_qp_linesearch(d);
1103   // Keep iterating
1104   return 0;
1105 }
1106 
1107 // The following routines require stdio
1108 #ifndef CASADI_PRINTF
1109 
1110 // SYMBOL "qp_print_header"
1111 template<typename T1>
casadi_qp_print_header(casadi_qp_data<T1> * d,char * buf,size_t buf_sz)1112 int casadi_qp_print_header(casadi_qp_data<T1>* d, char* buf, size_t buf_sz) {
1113   int flag;
1114   // Print to string
1115   flag = snprintf(buf, buf_sz, "%5s %5s %9s %9s %5s %9s %5s %9s %5s %9s  %4s",
1116           "Iter", "Sing", "fk", "|pr|", "con", "|du|", "var",
1117           "min_R", "con", "last_tau", "Note");
1118   // Check if error
1119   if (flag < 0) {
1120     d->status = QP_PRINTING_ERROR;
1121     return 1;
1122   }
1123   // Successful return
1124   return 0;
1125 }
1126 
1127 // SYMBOL "qp_print_colcomb"
1128 template<typename T1>
casadi_qp_print_colcomb(casadi_qp_data<T1> * d,char * buf,size_t buf_sz,casadi_int j)1129 int casadi_qp_print_colcomb(casadi_qp_data<T1>* d, char* buf, size_t buf_sz, casadi_int j) {
1130   casadi_int num_size, n_print, i, k, buf_offset, val;
1131   size_t b;
1132   const casadi_qp_prob<T1>* p = d->prob;
1133   casadi_qr_colcomb(d->dlam, d->nz_r, p->sp_r, p->pc, 1e-12, j);
1134 
1135   // Determine max printing size
1136   num_size = 1;
1137   val = p->nz-1;
1138   while (val) {
1139     val/=10;
1140     num_size++;
1141   }
1142 
1143   if (buf_sz<=4) return 1;
1144 
1145   // How many numbers can be printed?
1146   // Need some extra space for '...'
1147   // and null
1148   n_print = (buf_sz-4)/num_size;
1149 
1150   // Clear buffer
1151   for (b=0;b<buf_sz;++b) buf[b]=' ';
1152 
1153   buf_offset = 0;
1154   for (i=0;i<p->nz;++i) {
1155     if (fabs(d->dlam[i]) >= 1e-12) {
1156       if (n_print==0) {
1157         buf[buf_sz-4] = '.';
1158         buf[buf_sz-3] = '.';
1159         buf[buf_sz-2] = '.';
1160         buf[buf_sz-1] = '\0';
1161         return 1;
1162       }
1163       n_print--;
1164       snprintf(buf+buf_offset, num_size, "%d", static_cast<int>(i));
1165       // Clear null chars
1166       for (k=0;k<num_size;++k) {
1167         if (buf[buf_offset+k]=='\0') buf[buf_offset+k] = ' ';
1168       }
1169       buf_offset += num_size;
1170     }
1171   }
1172   buf[buf_sz-1] = '\0';
1173 
1174   // Successful return
1175   return 0;
1176 }
1177 
1178 // SYMBOL "qp_print_iteration"
1179 template<typename T1>
casadi_qp_print_iteration(casadi_qp_data<T1> * d,char * buf,int buf_sz)1180 int casadi_qp_print_iteration(casadi_qp_data<T1>* d, char* buf, int buf_sz) {
1181   int flag;
1182   // Print iteration data without note to string
1183   flag = snprintf(buf, buf_sz,
1184     "%5d %5d %9.2g %9.2g %5d %9.2g %5d %9.2g %5d %9.2g  ",
1185     static_cast<int>(d->iter), static_cast<int>(d->sing), d->f, d->pr,
1186     static_cast<int>(d->ipr), d->du, static_cast<int>(d->idu),
1187     d->mina, static_cast<int>(d->imina), d->tau);
1188   // Check if error
1189   if (flag < 0) {
1190     d->status = QP_PRINTING_ERROR;
1191     return 1;
1192   }
1193   // Rest of buffer reserved for iteration note
1194   buf += flag;
1195   buf_sz -= flag;
1196   // Print iteration note, if any
1197   if (d->msg) {
1198     if (d->msg_ind > -2) {
1199       flag = snprintf(buf, buf_sz, "%s, i=%d", d->msg, static_cast<int>(d->msg_ind));
1200     } else {
1201       flag = snprintf(buf, buf_sz, "%s", d->msg);
1202     }
1203     // Check if error
1204     if (flag < 0) {
1205       d->status = QP_PRINTING_ERROR;
1206       return 1;
1207     }
1208   }
1209   // Successful return
1210   return 0;
1211 }
1212 
1213 #endif  // CASADI_PRINTF
1214