1 /**************************************************************************************************
2 * *
3 * This file is part of HPIPM. *
4 * *
5 * HPIPM -- High-Performance Interior Point Method. *
6 * Copyright (C) 2019 by Gianluca Frison. *
7 * Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
8 * All rights reserved. *
9 * *
10 * The 2-Clause BSD License *
11 * *
12 * Redistribution and use in source and binary forms, with or without *
13 * modification, are permitted provided that the following conditions are met: *
14 * *
15 * 1. Redistributions of source code must retain the above copyright notice, this *
16 * list of conditions and the following disclaimer. *
17 * 2. Redistributions in binary form must reproduce the above copyright notice, *
18 * this list of conditions and the following disclaimer in the documentation *
19 * and/or other materials provided with the distribution. *
20 * *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND *
22 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED *
23 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE *
24 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR *
25 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
26 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; *
27 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND *
28 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
29 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS *
30 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
31 * *
32 * Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
33 * *
34 **************************************************************************************************/
35
36
37
DENSE_QP_RES_MEMSIZE(struct DENSE_QP_DIM * dim)38 int DENSE_QP_RES_MEMSIZE(struct DENSE_QP_DIM *dim)
39 {
40
41 // loop index
42 int ii;
43
44 // extract ocp qp size
45 int nv = dim->nv;
46 int ne = dim->ne;
47 int nb = dim->nb;
48 int ng = dim->ng;
49 int ns = dim->ns;
50
51 int size = 0;
52
53 size += 4*sizeof(struct STRVEC); // res_g res_b res_d res_m
54
55 size += 1*SIZE_STRVEC(nv+2*ns); // res_g
56 size += 1*SIZE_STRVEC(ne); // res_b
57 size += 2*SIZE_STRVEC(2*nb+2*ng+2*ns); // res_d res_m
58
59 size = (size+63)/64*64; // make multiple of typical cache line size
60 size += 1*64; // align once to typical cache line size
61
62 return size;
63
64 }
65
66
67
DENSE_QP_RES_CREATE(struct DENSE_QP_DIM * dim,struct DENSE_QP_RES * res,void * mem)68 void DENSE_QP_RES_CREATE(struct DENSE_QP_DIM *dim, struct DENSE_QP_RES *res, void *mem)
69 {
70
71 // loop index
72 int ii;
73
74 // extract ocp qp size
75 int nv = dim->nv;
76 int ne = dim->ne;
77 int nb = dim->nb;
78 int ng = dim->ng;
79 int ns = dim->ns;
80
81
82 // vector struct
83 struct STRVEC *sv_ptr = (struct STRVEC *) mem;
84
85 res->res_g = sv_ptr;
86 sv_ptr += 1;
87 res->res_b = sv_ptr;
88 sv_ptr += 1;
89 res->res_d = sv_ptr;
90 sv_ptr += 1;
91 res->res_m = sv_ptr;
92 sv_ptr += 1;
93
94
95 // align to typicl cache line size
96 size_t s_ptr = (size_t) sv_ptr;
97 s_ptr = (s_ptr+63)/64*64;
98
99
100 // void stuf
101 char *c_ptr = (char *) s_ptr;
102
103 CREATE_STRVEC(nv+2*ns, res->res_g, c_ptr);
104 c_ptr += (res->res_g)->memsize;
105
106 CREATE_STRVEC(ne, res->res_b, c_ptr);
107 c_ptr += (res->res_b)->memsize;
108
109 CREATE_STRVEC(2*nb+2*ng+2*ns, res->res_d, c_ptr);
110 c_ptr += (res->res_d)->memsize;
111
112 CREATE_STRVEC(2*nb+2*ng+2*ns, res->res_m, c_ptr);
113 c_ptr += (res->res_m)->memsize;
114
115 res->dim = dim;
116
117 res->memsize = DENSE_QP_RES_MEMSIZE(dim);
118
119
120 #if defined(RUNTIME_CHECKS)
121 if(c_ptr > ((char *) mem) + res->memsize)
122 {
123 printf("\ncreate_dense_qp_res: outsize memory bounds!\n\n");
124 exit(1);
125 }
126 #endif
127
128
129 return;
130
131 }
132
133
134
DENSE_QP_RES_WS_MEMSIZE(struct DENSE_QP_DIM * dim)135 int DENSE_QP_RES_WS_MEMSIZE(struct DENSE_QP_DIM *dim)
136 {
137
138 // loop index
139 int ii;
140
141 // extract ocp qp size
142 int nv = dim->nv;
143 int ne = dim->ne;
144 int nb = dim->nb;
145 int ng = dim->ng;
146 int ns = dim->ns;
147
148 int size = 0;
149
150 size += 3*sizeof(struct STRVEC); // 2*tmp_nbg tmp_ns
151
152 size += 2*SIZE_STRVEC(nb+ng); // tmp_nbg
153 size += 1*SIZE_STRVEC(ns); // tmp_ns
154
155 size = (size+63)/64*64; // make multiple of typical cache line size
156 size += 1*64; // align once to typical cache line size
157
158 return size;
159
160 }
161
162
163
DENSE_QP_RES_WS_CREATE(struct DENSE_QP_DIM * dim,struct DENSE_QP_RES_WS * ws,void * mem)164 void DENSE_QP_RES_WS_CREATE(struct DENSE_QP_DIM *dim, struct DENSE_QP_RES_WS *ws, void *mem)
165 {
166
167 // loop index
168 int ii;
169
170 // extract ocp qp size
171 int nv = dim->nv;
172 int ne = dim->ne;
173 int nb = dim->nb;
174 int ng = dim->ng;
175 int ns = dim->ns;
176
177
178 // vector struct
179 struct STRVEC *sv_ptr = (struct STRVEC *) mem;
180
181 ws->tmp_nbg = sv_ptr;
182 sv_ptr += 2;
183 ws->tmp_ns = sv_ptr;
184 sv_ptr += 1;
185
186
187 // align to typicl cache line size
188 size_t s_ptr = (size_t) sv_ptr;
189 s_ptr = (s_ptr+63)/64*64;
190
191
192 // void stuf
193 char *c_ptr = (char *) s_ptr;
194
195
196 CREATE_STRVEC(nb+ng, ws->tmp_nbg+0, c_ptr);
197 c_ptr += (ws->tmp_nbg+0)->memsize;
198
199 CREATE_STRVEC(nb+ng, ws->tmp_nbg+1, c_ptr);
200 c_ptr += (ws->tmp_nbg+1)->memsize;
201
202 CREATE_STRVEC(ns, ws->tmp_ns+0, c_ptr);
203 c_ptr += (ws->tmp_ns+0)->memsize;
204
205 ws->memsize = DENSE_QP_RES_WS_MEMSIZE(dim);
206
207
208 #if defined(RUNTIME_CHECKS)
209 if(c_ptr > ((char *) mem) + ws->memsize)
210 {
211 printf("\ncreate_dense_qp_res_workspace: outsize memory bounds!\n\n");
212 exit(1);
213 }
214 #endif
215
216
217 return;
218
219 }
220
221
222
DENSE_QP_RES_COMPUTE(struct DENSE_QP * qp,struct DENSE_QP_SOL * qp_sol,struct DENSE_QP_RES * res,struct DENSE_QP_RES_WS * ws)223 void DENSE_QP_RES_COMPUTE(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct DENSE_QP_RES *res, struct DENSE_QP_RES_WS *ws)
224 {
225
226 int nv = qp->dim->nv;
227 int ne = qp->dim->ne;
228 int nb = qp->dim->nb;
229 int ng = qp->dim->ng;
230 int ns = qp->dim->ns;
231
232 int nct = 2*nb+2*ng+2*ns;
233
234 REAL nct_inv = 1.0/nct;
235
236 struct STRMAT *Hg = qp->Hv;
237 struct STRMAT *A = qp->A;
238 struct STRMAT *Ct = qp->Ct;
239 struct STRVEC *gz = qp->gz;
240 struct STRVEC *b = qp->b;
241 struct STRVEC *d = qp->d;
242 struct STRVEC *m = qp->m;
243 int *idxb = qp->idxb;
244 struct STRVEC *Z = qp->Z;
245 int *idxs = qp->idxs;
246
247 struct STRVEC *v = qp_sol->v;
248 struct STRVEC *pi = qp_sol->pi;
249 struct STRVEC *lam = qp_sol->lam;
250 struct STRVEC *t = qp_sol->t;
251
252 struct STRVEC *res_g = res->res_g;
253 struct STRVEC *res_b = res->res_b;
254 struct STRVEC *res_d = res->res_d;
255 struct STRVEC *res_m = res->res_m;
256
257 struct STRVEC *tmp_nbg = ws->tmp_nbg;
258 struct STRVEC *tmp_ns = ws->tmp_ns;
259
260 REAL mu;
261
262 // res g
263 SYMV_L(nv, nv, 1.0, Hg, 0, 0, v, 0, 1.0, gz, 0, res_g, 0);
264
265 if(nb+ng>0)
266 {
267 AXPY(nb+ng, -1.0, lam, 0, lam, nb+ng, tmp_nbg+0, 0);
268 // AXPY(nb+ng, 1.0, d, 0, t, 0, res_d, 0);
269 // AXPY(nb+ng, 1.0, d, nb+ng, t, nb+ng, res_d, nb+ng);
270 AXPY(2*nb+2*ng, 1.0, d, 0, t, 0, res_d, 0);
271 // box
272 if(nb>0)
273 {
274 VECAD_SP(nb, 1.0, tmp_nbg+0, 0, idxb, res_g, 0);
275 VECEX_SP(nb, 1.0, idxb, v, 0, tmp_nbg+1, 0);
276 }
277 // general
278 if(ng>0)
279 {
280 GEMV_NT(nv, ng, 1.0, 1.0, Ct, 0, 0, tmp_nbg+0, nb, v, 0, 1.0, 0.0, res_g, 0, tmp_nbg+1, nb, res_g, 0, tmp_nbg+1, nb);
281 }
282 AXPY(nb+ng, -1.0, tmp_nbg+1, 0, res_d, 0, res_d, 0);
283 AXPY(nb+ng, 1.0, tmp_nbg+1, 0, res_d, nb+ng, res_d, nb+ng);
284 }
285 if(ns>0)
286 {
287 // res_g
288 GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 1.0, gz, nv, res_g, nv);
289 AXPY(2*ns, -1.0, lam, 2*nb+2*ng, res_g, nv, res_g, nv);
290 VECEX_SP(ns, 1.0, idxs, lam, 0, tmp_ns, 0);
291 AXPY(ns, -1.0, tmp_ns, 0, res_g, nv, res_g, nv);
292 VECEX_SP(ns, 1.0, idxs, lam, nb+ng, tmp_ns, 0);
293 AXPY(ns, -1.0, tmp_ns, 0, res_g, nv+ns, res_g, nv+ns);
294 // res_d
295 VECAD_SP(ns, -1.0, v, nv, idxs, res_d, 0);
296 VECAD_SP(ns, -1.0, v, nv+ns, idxs, res_d, nb+ng);
297 AXPY(2*ns, -1.0, v, nv, t, 2*nb+2*ng, res_d, 2*nb+2*ng);
298 AXPY(2*ns, 1.0, d, 2*nb+2*ng, res_d, 2*nb+2*ng, res_d, 2*nb+2*ng);
299 }
300
301 // res b, res g
302 GEMV_NT(ne, nv, -1.0, -1.0, A, 0, 0, v, 0, pi, 0, 1.0, 1.0, b, 0, res_g, 0, res_b, 0, res_g, 0);
303
304 // res_m res_mu
305 mu = VECMULDOT(nct, lam, 0, t, 0, res_m, 0);
306 AXPY(nct, -1.0, m, 0, res_m, 0, res_m, 0);
307 res->res_mu = mu*nct_inv;
308
309
310 return;
311
312 }
313
314
DENSE_QP_RES_COMPUTE_LIN(struct DENSE_QP * qp,struct DENSE_QP_SOL * qp_sol,struct DENSE_QP_SOL * qp_step,struct DENSE_QP_RES * res,struct DENSE_QP_RES_WS * ws)315 void DENSE_QP_RES_COMPUTE_LIN(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct DENSE_QP_SOL *qp_step, struct DENSE_QP_RES *res, struct DENSE_QP_RES_WS *ws)
316 {
317
318 int nv = qp->dim->nv;
319 int ne = qp->dim->ne;
320 int nb = qp->dim->nb;
321 int ng = qp->dim->ng;
322 int ns = qp->dim->ns;
323
324 int nct = 2*nb+2*ng+2*ns;
325
326 REAL nct_inv = 1.0/nct;
327
328 struct STRMAT *Hg = qp->Hv;
329 struct STRMAT *A = qp->A;
330 struct STRMAT *Ct = qp->Ct;
331 struct STRVEC *gz = qp->gz;
332 struct STRVEC *b = qp->b;
333 struct STRVEC *d = qp->d;
334 struct STRVEC *m = qp->m;
335 int *idxb = qp->idxb;
336 struct STRVEC *Z = qp->Z;
337 int *idxs = qp->idxs;
338
339 struct STRVEC *v = qp_step->v;
340 struct STRVEC *pi = qp_step->pi;
341 struct STRVEC *lam = qp_step->lam;
342 struct STRVEC *t = qp_step->t;
343
344 struct STRVEC *Lam = qp_sol->lam;
345 struct STRVEC *T = qp_sol->t;
346
347 struct STRVEC *res_g = res->res_g;
348 struct STRVEC *res_b = res->res_b;
349 struct STRVEC *res_d = res->res_d;
350 struct STRVEC *res_m = res->res_m;
351
352 struct STRVEC *tmp_nbg = ws->tmp_nbg;
353 struct STRVEC *tmp_ns = ws->tmp_ns;
354
355 REAL mu;
356
357 // res g
358 SYMV_L(nv, nv, 1.0, Hg, 0, 0, v, 0, 1.0, gz, 0, res_g, 0);
359
360 if(nb+ng>0)
361 {
362 AXPY(nb+ng, -1.0, lam, 0, lam, nb+ng, tmp_nbg+0, 0);
363 // AXPY(nb+ng, 1.0, d, 0, t, 0, res_d, 0);
364 // AXPY(nb+ng, 1.0, d, nb+ng, t, nb+ng, res_d, nb+ng);
365 AXPY(2*nb+2*ng, 1.0, d, 0, t, 0, res_d, 0);
366 // box
367 if(nb>0)
368 {
369 VECAD_SP(nb, 1.0, tmp_nbg+0, 0, idxb, res_g, 0);
370 VECEX_SP(nb, 1.0, idxb, v, 0, tmp_nbg+1, 0);
371 }
372 // general
373 if(ng>0)
374 {
375 GEMV_NT(nv, ng, 1.0, 1.0, Ct, 0, 0, tmp_nbg+0, nb, v, 0, 1.0, 0.0, res_g, 0, tmp_nbg+1, nb, res_g, 0, tmp_nbg+1, nb);
376 }
377 AXPY(nb+ng, -1.0, tmp_nbg+1, 0, res_d, 0, res_d, 0);
378 AXPY(nb+ng, 1.0, tmp_nbg+1, 0, res_d, nb+ng, res_d, nb+ng);
379 }
380 if(ns>0)
381 {
382 // res_g
383 GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 1.0, gz, nv, res_g, nv);
384 AXPY(2*ns, -1.0, lam, 2*nb+2*ng, res_g, nv, res_g, nv);
385 VECEX_SP(ns, 1.0, idxs, lam, 0, tmp_ns, 0);
386 AXPY(ns, -1.0, tmp_ns, 0, res_g, nv, res_g, nv);
387 VECEX_SP(ns, 1.0, idxs, lam, nb+ng, tmp_ns, 0);
388 AXPY(ns, -1.0, tmp_ns, 0, res_g, nv+ns, res_g, nv+ns);
389 // res_d
390 VECAD_SP(ns, -1.0, v, nv, idxs, res_d, 0);
391 VECAD_SP(ns, -1.0, v, nv+ns, idxs, res_d, nb+ng);
392 AXPY(2*ns, -1.0, v, nv, t, 2*nb+2*ng, res_d, 2*nb+2*ng);
393 AXPY(2*ns, 1.0, d, 2*nb+2*ng, res_d, 2*nb+2*ng, res_d, 2*nb+2*ng);
394 }
395
396 // res b, res g
397 GEMV_NT(ne, nv, -1.0, -1.0, A, 0, 0, v, 0, pi, 0, 1.0, 1.0, b, 0, res_g, 0, res_b, 0, res_g, 0);
398
399 // res_m res_mu
400 // VECCPSC(nct, 1.0, m, 0, res_m, 0);
401 VECCP(nct, m, 0, res_m, 0);
402 VECMULACC(nct, Lam, 0, t, 0, res_m, 0);
403 VECMULACC(nct, lam, 0, T, 0, res_m, 0);
404 // mu = VECMULDOT(nct, lam, 0, t, 0, res_m, 0);
405 // AXPY(nct, -1.0, m, 0, res_m, 0, res_m, 0);
406 // res->res_mu = mu*nct_inv;
407
408
409 return;
410
411 }
412
413
DENSE_QP_RES_GET_ALL(struct DENSE_QP_RES * res,REAL * res_g,REAL * res_ls,REAL * res_us,REAL * res_b,REAL * res_d_lb,REAL * res_d_ub,REAL * res_d_lg,REAL * res_d_ug,REAL * res_d_ls,REAL * res_d_us,REAL * res_m_lb,REAL * res_m_ub,REAL * res_m_lg,REAL * res_m_ug,REAL * res_m_ls,REAL * res_m_us)414 void DENSE_QP_RES_GET_ALL(struct DENSE_QP_RES *res, REAL *res_g, REAL *res_ls, REAL *res_us, REAL *res_b, REAL *res_d_lb, REAL *res_d_ub, REAL *res_d_lg, REAL *res_d_ug, REAL *res_d_ls, REAL *res_d_us, REAL *res_m_lb, REAL *res_m_ub, REAL *res_m_lg, REAL *res_m_ug, REAL *res_m_ls, REAL *res_m_us)
415 {
416
417 int nv = res->dim->nv;
418 int ne = res->dim->ne;
419 int nb = res->dim->nb;
420 int ng = res->dim->ng;
421 int ns = res->dim->ns;
422
423 CVT_STRVEC2VEC(nv, res->res_g, 0, res_g);
424 CVT_STRVEC2VEC(ns, res->res_g, nv, res_ls);
425 CVT_STRVEC2VEC(ns, res->res_g, nv+ns, res_us);
426
427 CVT_STRVEC2VEC(ne, res->res_b, 0, res_b);
428 CVT_STRVEC2VEC(nb, res->res_d, 0, res_d_lb);
429 CVT_STRVEC2VEC(ng, res->res_d, nb, res_d_lg);
430 CVT_STRVEC2VEC(nb, res->res_d, nb+ng, res_d_ub);
431 CVT_STRVEC2VEC(ng, res->res_d, 2*nb+ng, res_d_ug);
432 CVT_STRVEC2VEC(ns, res->res_d, 2*nb+2*ng, res_d_ls);
433 CVT_STRVEC2VEC(ns, res->res_d, 2*nb+2*ng+ns, res_d_us);
434 CVT_STRVEC2VEC(nb, res->res_m, 0, res_m_lb);
435 CVT_STRVEC2VEC(ng, res->res_m, nb, res_m_lg);
436 CVT_STRVEC2VEC(nb, res->res_m, nb+ng, res_m_ub);
437 CVT_STRVEC2VEC(ng, res->res_m, 2*nb+ng, res_m_ug);
438 CVT_STRVEC2VEC(ns, res->res_m, 2*nb+2*ng, res_m_ls);
439 CVT_STRVEC2VEC(ns, res->res_m, 2*nb+2*ng+ns, res_m_us);
440
441 return;
442
443 }
444
445