Squashed 'third_party/hpipm/' content from commit c2c0261
Change-Id: I05e186a6e1e1f075aa629092e7ad86e6116ca711
git-subtree-dir: third_party/hpipm
git-subtree-split: c2c0261e8ded36f636d9c4390a2899bdc4c999cc
diff --git a/ocp_qp/x_ocp_qp_sol.c b/ocp_qp/x_ocp_qp_sol.c
new file mode 100644
index 0000000..274f0f0
--- /dev/null
+++ b/ocp_qp/x_ocp_qp_sol.c
@@ -0,0 +1,304 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+int MEMSIZE_OCP_QP_SOL(int N, int *nx, int *nu, int *nb, int *ng)
+ {
+
+ int ii;
+
+ int nvt = 0;
+ int net = 0;
+ int nbt = 0;
+ int ngt = 0;
+ for(ii=0; ii<N; ii++)
+ {
+ nvt += nu[ii]+nx[ii];
+ net += nx[ii+1];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ }
+ nvt += nu[ii]+nx[ii];
+ nbt += nb[ii];
+ ngt += ng[ii];
+
+ int size = 0;
+
+ size += (0+10*(N+1))*sizeof(struct STRVEC); // ux pi lam_lb lam_ub lam_lg lam_ug t_lb t_ub t_lg t_ug
+
+ size += 1*SIZE_STRVEC(nvt); // ux
+ size += 1*SIZE_STRVEC(net); // pi
+ size += 8*SIZE_STRVEC(2*nbt+2*ngt); // lam_lb lam_ub lam_lg lam_ug t_lb t_ub t_lg t_ug
+
+ size = (size+63)/64*64; // make multiple of typical cache line size
+ size += 64; // align to typical cache line size
+
+ return size;
+
+ }
+
+
+
+void CREATE_OCP_QP_SOL(int N, int *nx, int *nu, int *nb, int *ng, struct OCP_QP_SOL *qp_sol, void *memory)
+ {
+
+ int ii;
+
+
+ // memsize
+ qp_sol->memsize = MEMSIZE_OCP_QP_SOL(N, nx, nu, nb, ng);
+
+
+ int nvt = 0;
+ int net = 0;
+ int nbt = 0;
+ int ngt = 0;
+ for(ii=0; ii<N; ii++)
+ {
+ nvt += nu[ii]+nx[ii];
+ net += nx[ii+1];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ }
+ nvt += nu[ii]+nx[ii];
+ nbt += nb[ii];
+ ngt += ng[ii];
+
+
+ // vector struct stuff
+ struct STRVEC *sv_ptr = (struct STRVEC *) memory;
+
+ qp_sol->ux = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->pi = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->lam_lb = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->lam_ub = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->lam_lg = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->lam_ug = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->t_lb = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->t_ub = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->t_lg = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->t_ug = sv_ptr;
+ sv_ptr += N+1;
+
+
+ // align to typical cache line size
+ long long l_ptr = (long long) sv_ptr;
+ l_ptr = (l_ptr+63)/64*64;
+
+
+ // double stuff
+ void *v_ptr;
+ v_ptr = (void *) l_ptr;
+
+ void *tmp_ptr;
+
+ // ux
+ tmp_ptr = v_ptr;
+ v_ptr += SIZE_STRVEC(nvt);
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nu[ii]+nx[ii], qp_sol->ux+ii, tmp_ptr);
+ tmp_ptr += (nu[ii]+nx[ii])*sizeof(REAL);
+ }
+ // pi
+ tmp_ptr = v_ptr;
+ v_ptr += SIZE_STRVEC(net);
+ for(ii=0; ii<N; ii++)
+ {
+ CREATE_STRVEC(nx[ii+1], qp_sol->pi+ii, tmp_ptr);
+ tmp_ptr += (nx[ii+1])*sizeof(REAL);
+ }
+ // lam
+ tmp_ptr = v_ptr;
+ v_ptr += SIZE_STRVEC(2*nbt+2*ngt);
+ // lam_lb
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp_sol->lam_lb+ii, tmp_ptr);
+ tmp_ptr += (nb[ii])*sizeof(REAL);
+ }
+ // lam_lg
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp_sol->lam_lg+ii, tmp_ptr);
+ tmp_ptr += (ng[ii])*sizeof(REAL);
+ }
+ // lam_ub
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp_sol->lam_ub+ii, tmp_ptr);
+ tmp_ptr += (nb[ii])*sizeof(REAL);
+ }
+ // lam_ug
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp_sol->lam_ug+ii, tmp_ptr);
+ tmp_ptr += (ng[ii])*sizeof(REAL);
+ }
+ // t
+ tmp_ptr = v_ptr;
+ v_ptr += SIZE_STRVEC(2*nbt+2*ngt);
+ // t_lb
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp_sol->t_lb+ii, tmp_ptr);
+ tmp_ptr += (nb[ii])*sizeof(REAL);
+ }
+ // t_lg
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp_sol->t_lg+ii, tmp_ptr);
+ tmp_ptr += (ng[ii])*sizeof(REAL);
+ }
+ // t_ub
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp_sol->t_ub+ii, tmp_ptr);
+ tmp_ptr += (nb[ii])*sizeof(REAL);
+ }
+ // t_ug
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp_sol->t_ug+ii, tmp_ptr);
+ tmp_ptr += (ng[ii])*sizeof(REAL);
+ }
+
+ return;
+
+ }
+
+
+
+void CVT_OCP_QP_SOL_TO_COLMAJ(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, REAL **u, REAL **x, REAL **pi, REAL **lam_lb, REAL **lam_ub, REAL **lam_lg, REAL **lam_ug)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int ii;
+
+ for(ii=0; ii<N; ii++)
+ {
+ CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
+ CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
+ CVT_STRVEC2VEC(nx[ii+1], qp_sol->pi+ii, 0, pi[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
+ }
+ CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
+ CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
+
+ return;
+
+ }
+
+
+
+void CVT_OCP_QP_SOL_TO_ROWMAJ(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, REAL **u, REAL **x, REAL **pi, REAL **lam_lb, REAL **lam_ub, REAL **lam_lg, REAL **lam_ug)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int ii;
+
+ for(ii=0; ii<N; ii++)
+ {
+ CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
+ CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
+ CVT_STRVEC2VEC(nx[ii+1], qp_sol->pi+ii, 0, pi[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
+ }
+ CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
+ CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
+
+ return;
+
+ }
+
+
+
+void CVT_OCP_QP_SOL_TO_LIBSTR(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct STRVEC *u, struct STRVEC *x, struct STRVEC *pi, struct STRVEC *lam_lb, struct STRVEC *lam_ub, struct STRVEC *lam_lg, struct STRVEC *lam_ug)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int ii;
+
+ for(ii=0; ii<N; ii++)
+ {
+ VECCP_LIBSTR(nu[ii], qp_sol->ux+ii, 0, u+ii, 0);
+ VECCP_LIBSTR(nx[ii], qp_sol->ux+ii, nu[ii], x+ii, 0);
+ VECCP_LIBSTR(nx[ii+1], qp_sol->pi+ii, 0, pi+ii, 0);
+ VECCP_LIBSTR(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb+ii, 0);
+ VECCP_LIBSTR(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug+ii, 0);
+ }
+ VECCP_LIBSTR(nu[ii], qp_sol->ux+ii, 0, u+ii, 0);
+ VECCP_LIBSTR(nx[ii], qp_sol->ux+ii, nu[ii], x+ii, 0);
+ VECCP_LIBSTR(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb+ii, 0);
+ VECCP_LIBSTR(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug+ii, 0);
+
+ return;
+
+ }