Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 16ce3c78abe894f77f0a95bbb88530f3f100c509 / . / dense_qp / x_dense_qp_sol.c
blob: 95297292c69039d41e0be8438da01ae17117738b [file] [log] [blame]
Austin Schuh16ce3c72018-01-28 16:17:08 -0800[diff] [blame^]1/**************************************************************************************************
2* *
3* This file is part of HPIPM. *
4* *
5* HPIPM -- High Performance Interior Point Method. *
6* Copyright (C) 2017 by Gianluca Frison. *
7* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
8* All rights reserved. *
9* *
10* HPMPC is free software; you can redistribute it and/or *
11* modify it under the terms of the GNU Lesser General Public *
12* License as published by the Free Software Foundation; either *
13* version 2.1 of the License, or (at your option) any later version. *
14* *
15* HPMPC is distributed in the hope that it will be useful, *
16* but WITHOUT ANY WARRANTY; without even the implied warranty of *
17* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
18* See the GNU Lesser General Public License for more details. *
19* *
20* You should have received a copy of the GNU Lesser General Public *
21* License along with HPMPC; if not, write to the Free Software *
22* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
23* *
24* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
25* *
26**************************************************************************************************/
27
28
29
30int MEMSIZE_DENSE_QP_SOL(int nv, int ne, int nb, int ng)
31 {
32
33 int size = 0;
34
35 size += (10)*sizeof(struct STRVEC); // v pi lam_lb lam_ub lam_lg lam_ug t_lb t_ub t_lg t_ug
36
37 size += 1*SIZE_STRVEC(nv); // ux
38 size += 1*SIZE_STRVEC(ne); // pi
39 size += 8*SIZE_STRVEC(2*nb+2*ng); // lam_lb lam_ub lam_lg lam_ug t_lb t_ub t_lg t_ug
40
41 size = (size+63)/64*64; // make multiple of typical cache line size
42 size += 64; // align to typical cache line size
43
44 return size;
45
46 }
47
48
49
50void CREATE_DENSE_QP_SOL(int nv, int ne, int nb, int ng, struct DENSE_QP_SOL *qp_sol, void *memory)
51 {
52
53 qp_sol->memsize = MEMSIZE_DENSE_QP_SOL(nv, ne, nb, ng);
54
55
56 // vector struct stuff
57 struct STRVEC *sv_ptr = (struct STRVEC *) memory;
58
59 qp_sol->v = sv_ptr;
60 sv_ptr += 1;
61 qp_sol->pi = sv_ptr;
62 sv_ptr += 1;
63 qp_sol->lam_lb = sv_ptr;
64 sv_ptr += 1;
65 qp_sol->lam_ub = sv_ptr;
66 sv_ptr += 1;
67 qp_sol->lam_lg = sv_ptr;
68 sv_ptr += 1;
69 qp_sol->lam_ug = sv_ptr;
70 sv_ptr += 1;
71 qp_sol->t_lb = sv_ptr;
72 sv_ptr += 1;
73 qp_sol->t_ub = sv_ptr;
74 sv_ptr += 1;
75 qp_sol->t_lg = sv_ptr;
76 sv_ptr += 1;
77 qp_sol->t_ug = sv_ptr;
78 sv_ptr += 1;
79
80
81 // align to typical cache line size
82 long long l_ptr = (long long) sv_ptr;
83 l_ptr = (l_ptr+63)/64*64;
84
85
86 // double stuff
87 void *v_ptr;
88 v_ptr = (void *) l_ptr;
89
90 void *tmp_ptr;
91
92 // v
93 CREATE_STRVEC(nv, qp_sol->v, v_ptr);
94 v_ptr += qp_sol->v->memory_size;
95 // pi
96 CREATE_STRVEC(ne, qp_sol->pi, v_ptr);
97 v_ptr += qp_sol->pi->memory_size;
98 // lam
99 tmp_ptr = v_ptr;
100 v_ptr += SIZE_STRVEC(2*nb+2*ng);
101 // lam_lb
102 CREATE_STRVEC(nb, qp_sol->lam_lb, tmp_ptr);
103 tmp_ptr += (nb)*sizeof(REAL);
104 // lam_lg
105 CREATE_STRVEC(ng, qp_sol->lam_lg, tmp_ptr);
106 tmp_ptr += (ng)*sizeof(REAL);
107 // lam_ub
108 CREATE_STRVEC(nb, qp_sol->lam_ub, tmp_ptr);
109 tmp_ptr += (nb)*sizeof(REAL);
110 // lam_ug
111 CREATE_STRVEC(ng, qp_sol->lam_ug, tmp_ptr);
112 tmp_ptr += (ng)*sizeof(REAL);
113 // t_lb
114 CREATE_STRVEC(nb, qp_sol->t_lb, tmp_ptr);
115 tmp_ptr += (nb)*sizeof(REAL);
116 // t_lg
117 CREATE_STRVEC(ng, qp_sol->t_lg, tmp_ptr);
118 tmp_ptr += (ng)*sizeof(REAL);
119 // t_ub
120 CREATE_STRVEC(nb, qp_sol->t_ub, tmp_ptr);
121 tmp_ptr += (nb)*sizeof(REAL);
122 // t_ug
123 CREATE_STRVEC(ng, qp_sol->t_ug, tmp_ptr);
124 tmp_ptr += (ng)*sizeof(REAL);
125
126 return;
127
128 }
129
130
131
132void CVT_DENSE_QP_SOL_TO_COLMAJ(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, REAL *v, REAL *pi, REAL *lam_lb, REAL *lam_ub, REAL *lam_lg, REAL *lam_ug)
133 {
134
135 int nv = qp->nv;
136 int ne = qp->ne;
137 int nb = qp->nb;
138 int ng = qp->ng;
139
140 CVT_STRVEC2VEC(nv, qp_sol->v, 0, v);
141 CVT_STRVEC2VEC(ne, qp_sol->pi, 0, pi);
142 CVT_STRVEC2VEC(nb, qp_sol->lam_lb, 0, lam_lb);
143 CVT_STRVEC2VEC(nb, qp_sol->lam_ub, 0, lam_ub);
144 CVT_STRVEC2VEC(ng, qp_sol->lam_lg, 0, lam_lg);
145 CVT_STRVEC2VEC(ng, qp_sol->lam_ug, 0, lam_ug);
146
147 return;
148
149 }
150
151
152
153void CVT_DENSE_QP_SOL_TO_ROWMAJ(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, REAL *v, REAL *pi, REAL *lam_lb, REAL *lam_ub, REAL *lam_lg, REAL *lam_ug)
154 {
155
156 int nv = qp->nv;
157 int ne = qp->ne;
158 int nb = qp->nb;
159 int ng = qp->ng;
160
161 CVT_STRVEC2VEC(nv, qp_sol->v, 0, v);
162 CVT_STRVEC2VEC(ne, qp_sol->pi, 0, pi);
163 CVT_STRVEC2VEC(nb, qp_sol->lam_lb, 0, lam_lb);
164 CVT_STRVEC2VEC(nb, qp_sol->lam_ub, 0, lam_ub);
165 CVT_STRVEC2VEC(ng, qp_sol->lam_lg, 0, lam_lg);
166 CVT_STRVEC2VEC(ng, qp_sol->lam_ug, 0, lam_ug);
167
168 return;
169
170 }
171
172
173
174void CVT_DENSE_QP_SOL_TO_LIBSTR(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct STRVEC *v, struct STRVEC *pi, struct STRVEC *lam_lb, struct STRVEC *lam_ub, struct STRVEC *lam_lg, struct STRVEC *lam_ug)
175 {
176
177 int nv = qp->nv;
178 int ne = qp->ne;
179 int nb = qp->nb;
180 int ng = qp->ng;
181
182 VECCP_LIBSTR(nv, qp_sol->v, 0, v, 0);
183 VECCP_LIBSTR(ne, qp_sol->pi, 0, pi, 0);
184 VECCP_LIBSTR(nb, qp_sol->lam_lb, 0, lam_lb, 0);
185 VECCP_LIBSTR(nb, qp_sol->lam_ub, 0, lam_ub, 0);
186 VECCP_LIBSTR(ng, qp_sol->lam_lg, 0, lam_lg, 0);
187 VECCP_LIBSTR(ng, qp_sol->lam_ug, 0, lam_ug, 0);
188
189 return;
190
191 }