Squashed 'third_party/hpipm/' content from commit c2c0261
Change-Id: I05e186a6e1e1f075aa629092e7ad86e6116ca711
git-subtree-dir: third_party/hpipm
git-subtree-split: c2c0261e8ded36f636d9c4390a2899bdc4c999cc
diff --git a/dense_qp/Makefile b/dense_qp/Makefile
new file mode 100644
index 0000000..3e629ea
--- /dev/null
+++ b/dense_qp/Makefile
@@ -0,0 +1,52 @@
+###################################################################################################
+# #
+# 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 #
+# #
+###################################################################################################
+
+include ../Makefile.rule
+
+OBJS =
+
+ifeq ($(TARGET), GENERIC)
+OBJS +=
+endif
+
+OBJS += d_dense_qp.o d_dense_qp_sol.o d_dense_qp_kkt.o d_dense_qp_ipm_hard.o
+OBJS += s_dense_qp.o s_dense_qp_sol.o s_dense_qp_kkt.o s_dense_qp_ipm_hard.o
+
+obj: $(OBJS)
+
+clean:
+ rm -f *.o
+ rm -f *.s
+
+d_dense_qp.o: d_dense_qp.c x_dense_qp.c
+s_dense_qp.o: s_dense_qp.c x_dense_qp.c
+d_dense_qp_sol.o: d_dense_qp_sol.c x_dense_qp_sol.c
+s_dense_qp_sol.o: s_dense_qp_sol.c x_dense_qp_sol.c
+d_dense_qp_kkt.o: d_dense_qp_kkt.c x_dense_qp_kkt.c
+s_dense_qp_kkt.o: s_dense_qp_kkt.c x_dense_qp_kkt.c
+d_dense_qp_ipm_hard.o: d_dense_qp_ipm_hard.c x_dense_qp_ipm_hard.c
+s_dense_qp_ipm_hard.o: s_dense_qp_ipm_hard.c x_dense_qp_ipm_hard.c
diff --git a/dense_qp/d_dense_qp.c b/dense_qp/d_dense_qp.c
new file mode 100644
index 0000000..42b4681
--- /dev/null
+++ b/dense_qp/d_dense_qp.c
@@ -0,0 +1,77 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#if defined(RUNTIME_CHECKS)
+#include <stdlib.h>
+#endif
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_aux.h>
+
+#include "../include/hpipm_d_dense_qp.h"
+
+
+#define CREATE_STRMAT d_create_strmat
+#define CREATE_STRVEC d_create_strvec
+#define CVT_MAT2STRMAT d_cvt_mat2strmat
+#define CVT_TRAN_MAT2STRMAT d_cvt_tran_mat2strmat
+#define CVT_TRAN_STRMAT2MAT d_cvt_tran_strmat2mat
+#define CVT_VEC2STRVEC d_cvt_vec2strvec
+#define CVT_STRMAT2MAT d_cvt_strmat2mat
+#define CVT_STRVEC2VEC d_cvt_strvec2vec
+#define DENSE_QP_DIM d_dense_qp_dim
+#define DENSE_QP_VEC d_dense_qp_vec
+#define DENSE_QP_MAT d_dense_qp_mat
+#define DENSE_QP d_dense_qp
+#define GECP_LIBSTR dgecp_libstr
+#define GETR_LIBSTR dgetr_libstr
+#define REAL double
+#define ROWIN_LIBSTR drowin_libstr
+#define SIZE_STRMAT d_size_strmat
+#define SIZE_STRVEC d_size_strvec
+#define STRMAT d_strmat
+#define STRVEC d_strvec
+#define VECCP_LIBSTR dveccp_libstr
+
+#define MEMSIZE_DENSE_QP d_memsize_dense_qp
+#define CREATE_DENSE_QP d_create_dense_qp
+#define CVT_COLMAJ_TO_DENSE_QP d_cvt_colmaj_to_dense_qp
+#define CVT_DENSE_QP_TO_COLMAJ d_cvt_dense_qp_to_colmaj
+#define CVT_ROWMAJ_TO_DENSE_QP d_cvt_rowmaj_to_dense_qp
+#define CVT_DENSE_QP_TO_ROWMAJ d_cvt_dense_qp_to_rowmaj
+#define CVT_LIBSTR_TO_DENSE_QP d_cvt_libstr_to_dense_qp
+#define CVT_DENSE_QP_TO_LIBSTR d_cvt_dense_qp_to_libstr
+#define CAST_DENSE_QP_DIM d_cast_dense_qp_dim
+//#define CREATE_DENSE_QP d_create_dense_qp
+//#define COPY_DENSE_QP d_copy_dense_qp
+
+
+
+#include "x_dense_qp.c"
diff --git a/dense_qp/d_dense_qp_ipm_hard.c b/dense_qp/d_dense_qp_ipm_hard.c
new file mode 100644
index 0000000..8a70f6f
--- /dev/null
+++ b/dense_qp/d_dense_qp_ipm_hard.c
@@ -0,0 +1,76 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_aux.h>
+
+#include "../include/hpipm_d_dense_qp.h"
+#include "../include/hpipm_d_dense_qp_sol.h"
+#include "../include/hpipm_d_dense_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard_aux.h"
+#include "../include/hpipm_d_dense_qp_kkt.h"
+
+
+
+#define COMPUTE_ALPHA_HARD_QP d_compute_alpha_hard_qp
+#define COMPUTE_CENTERING_CORRECTION_HARD_QP d_compute_centering_correction_hard_qp
+#define COMPUTE_MU_AFF_HARD_QP d_compute_mu_aff_hard_qp
+#define COMPUTE_RES_HARD_DENSE_QP d_compute_res_hard_dense_qp
+#define CREATE_IPM_HARD_CORE_QP d_create_ipm_hard_core_qp
+#define CREATE_STRMAT d_create_strmat
+#define CREATE_STRVEC d_create_strvec
+#define DENSE_QP d_dense_qp
+#define DENSE_QP_SOL d_dense_qp_sol
+#define FACT_SOLVE_KKT_STEP_HARD_DENSE_QP d_fact_solve_kkt_step_hard_dense_qp
+#define FACT_SOLVE_KKT_UNCONSTR_DENSE_QP d_fact_solve_kkt_unconstr_dense_qp
+#define INIT_VAR_HARD_DENSE_QP d_init_var_hard_dense_qp
+#define IPM_HARD_CORE_QP_WORKSPACE d_ipm_hard_core_qp_workspace
+#define IPM_HARD_DENSE_QP_ARG d_ipm_hard_dense_qp_arg
+#define IPM_HARD_DENSE_QP_WORKSPACE d_ipm_hard_dense_qp_workspace
+#define MEMSIZE_IPM_HARD_CORE_QP d_memsize_ipm_hard_core_qp
+#define REAL double
+#define SIZE_STRMAT d_size_strmat
+#define SIZE_STRVEC d_size_strvec
+#define SOLVE_KKT_STEP_HARD_DENSE_QP d_solve_kkt_step_hard_dense_qp
+#define STRMAT d_strmat
+#define STRVEC d_strvec
+#define UPDATE_VAR_HARD_QP d_update_var_hard_qp
+
+
+
+#define MEMSIZE_IPM_HARD_DENSE_QP d_memsize_ipm_hard_dense_qp
+#define CREATE_IPM_HARD_DENSE_QP d_create_ipm_hard_dense_qp
+#define SOLVE_IPM_HARD_DENSE_QP d_solve_ipm_hard_dense_qp
+#define SOLVE_IPM2_HARD_DENSE_QP d_solve_ipm2_hard_dense_qp
+
+
+
+#include "x_dense_qp_ipm_hard.c"
diff --git a/dense_qp/d_dense_qp_kkt.c b/dense_qp/d_dense_qp_kkt.c
new file mode 100644
index 0000000..2d3968c
--- /dev/null
+++ b/dense_qp/d_dense_qp_kkt.c
@@ -0,0 +1,88 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#include <math.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_aux.h>
+#include <blasfeo_d_blas.h>
+
+#include "../include/hpipm_d_dense_qp.h"
+#include "../include/hpipm_d_dense_qp_sol.h"
+#include "../include/hpipm_d_dense_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard_aux.h"
+
+
+
+#define AXPY_LIBSTR daxpy_libstr
+#define COMPUTE_LAM_T_HARD_QP d_compute_lam_t_hard_qp
+#define COMPUTE_QX_HARD_QP d_compute_qx_hard_qp
+#define COMPUTE_QX_QX_HARD_QP d_compute_Qx_qx_hard_qp
+#define DENSE_QP d_dense_qp
+#define DENSE_QP_SOL d_dense_qp_sol
+#define DIAAD_SP_LIBSTR ddiaad_sp_libstr
+#define GECP_LIBSTR dgecp_libstr
+#define GEMM_R_DIAG_LIBSTR dgemm_r_diag_libstr
+#define GEMV_N_LIBSTR dgemv_n_libstr
+#define GEMV_NT_LIBSTR dgemv_nt_libstr
+#define GEMV_T_LIBSTR dgemv_t_libstr
+#define GESE_LIBSTR dgese_libstr
+#define IPM_HARD_CORE_QP_WORKSPACE d_ipm_hard_core_qp_workspace
+#define IPM_HARD_DENSE_QP_WORKSPACE d_ipm_hard_dense_qp_workspace
+#define POTRF_L_LIBSTR dpotrf_l_libstr
+#define POTRF_L_MN_LIBSTR dpotrf_l_mn_libstr
+#define REAL double
+#define ROWAD_SP_LIBSTR drowad_sp_libstr
+#define ROWEX_LIBSTR drowex_libstr
+#define ROWIN_LIBSTR drowin_libstr
+#define STRMAT d_strmat
+#define STRVEC d_strvec
+#define SYMV_L_LIBSTR dsymv_l_libstr
+#define SYRK_POTRF_LN_LIBSTR dsyrk_dpotrf_ln_libstr
+#define TRCP_L_LIBSTR dtrcp_l_libstr
+#define TRSM_RLTN_LIBSTR dtrsm_rltn_libstr
+#define TRSV_LNN_LIBSTR dtrsv_lnn_libstr
+#define TRSV_LTN_LIBSTR dtrsv_ltn_libstr
+#define VECAD_SP_LIBSTR dvecad_sp_libstr
+#define VECCP_LIBSTR dveccp_libstr
+#define VECEX_SP_LIBSTR dvecex_sp_libstr
+#define VECMULDOT_LIBSTR dvecmuldot_libstr
+#define VECSC_LIBSTR dvecsc_libstr
+
+#define INIT_VAR_HARD_DENSE_QP d_init_var_hard_dense_qp
+#define COMPUTE_RES_HARD_DENSE_QP d_compute_res_hard_dense_qp
+#define FACT_SOLVE_KKT_UNCONSTR_DENSE_QP d_fact_solve_kkt_unconstr_dense_qp
+#define FACT_SOLVE_KKT_STEP_HARD_DENSE_QP d_fact_solve_kkt_step_hard_dense_qp
+#define SOLVE_KKT_STEP_HARD_DENSE_QP d_solve_kkt_step_hard_dense_qp
+
+
+
+#include "x_dense_qp_kkt.c"
diff --git a/dense_qp/d_dense_qp_sol.c b/dense_qp/d_dense_qp_sol.c
new file mode 100644
index 0000000..2d59048
--- /dev/null
+++ b/dense_qp/d_dense_qp_sol.c
@@ -0,0 +1,61 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#if defined(RUNTIME_CHECKS)
+#include <stdlib.h>
+#endif
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_aux.h>
+
+#include "../include/hpipm_d_dense_qp.h"
+#include "../include/hpipm_d_dense_qp_sol.h"
+
+
+
+#define CREATE_STRVEC d_create_strvec
+#define CVT_STRVEC2VEC d_cvt_strvec2vec
+#define DENSE_QP d_dense_qp
+#define DENSE_QP_SOL d_dense_qp_sol
+#define REAL double
+#define STRVEC d_strvec
+#define SIZE_STRVEC d_size_strvec
+#define VECCP_LIBSTR dveccp_libstr
+
+#define CREATE_DENSE_QP_SOL d_create_dense_qp_sol
+#define MEMSIZE_DENSE_QP_SOL d_memsize_dense_qp_sol
+#define CVT_DENSE_QP_SOL_TO_COLMAJ d_cvt_dense_qp_sol_to_colmaj
+#define CVT_DENSE_QP_SOL_TO_ROWMAJ d_cvt_dense_qp_sol_to_rowmaj
+#define CVT_DENSE_QP_SOL_TO_LIBSTR d_cvt_dense_qp_sol_to_libstr
+
+
+
+#include "x_dense_qp_sol.c"
+
diff --git a/dense_qp/s_dense_qp.c b/dense_qp/s_dense_qp.c
new file mode 100644
index 0000000..dfd2329
--- /dev/null
+++ b/dense_qp/s_dense_qp.c
@@ -0,0 +1,79 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+
+#if defined(RUNTIME_CHECKS)
+#include <stdlib.h>
+#endif
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_s_aux.h>
+
+#include "../include/hpipm_s_dense_qp.h"
+
+
+#define CREATE_STRMAT s_create_strmat
+#define CREATE_STRVEC s_create_strvec
+#define CVT_MAT2STRMAT s_cvt_mat2strmat
+#define CVT_TRAN_MAT2STRMAT s_cvt_tran_mat2strmat
+#define CVT_TRAN_STRMAT2MAT s_cvt_tran_strmat2mat
+#define CVT_VEC2STRVEC s_cvt_vec2strvec
+#define CVT_STRMAT2MAT s_cvt_strmat2mat
+#define CVT_STRVEC2VEC s_cvt_strvec2vec
+#define DENSE_QP_DIM s_dense_qp_dim
+#define DENSE_QP_VEC s_dense_qp_vec
+#define DENSE_QP_MAT s_dense_qp_mat
+#define DENSE_QP s_dense_qp
+#define GECP_LIBSTR sgecp_libstr
+#define GETR_LIBSTR sgetr_libstr
+#define REAL float
+#define ROWIN_LIBSTR srowin_libstr
+#define SIZE_STRMAT s_size_strmat
+#define SIZE_STRVEC s_size_strvec
+#define STRMAT s_strmat
+#define STRVEC s_strvec
+#define VECCP_LIBSTR sveccp_libstr
+
+#define MEMSIZE_DENSE_QP s_memsize_dense_qp
+#define CREATE_DENSE_QP s_create_dense_qp
+#define CVT_COLMAJ_TO_DENSE_QP s_cvt_colmaj_to_dense_qp
+#define CVT_DENSE_QP_TO_COLMAJ s_cvt_dense_qp_to_colmaj
+#define CVT_ROWMAJ_TO_DENSE_QP s_cvt_rowmaj_to_dense_qp
+#define CVT_DENSE_QP_TO_ROWMAJ s_cvt_dense_qp_to_rowmaj
+#define CVT_LIBSTR_TO_DENSE_QP s_cvt_libstr_to_dense_qp
+#define CVT_DENSE_QP_TO_LIBSTR s_cvt_dense_qp_to_libstr
+#define CAST_DENSE_QP_DIM s_cast_dense_qp_dim
+//#define CREATE_DENSE_QP s_create_dense_qp
+//#define COPY_DENSE_QP s_copy_dense_qp
+
+
+
+#include "x_dense_qp.c"
+
diff --git a/dense_qp/s_dense_qp_ipm_hard.c b/dense_qp/s_dense_qp_ipm_hard.c
new file mode 100644
index 0000000..e415f5b
--- /dev/null
+++ b/dense_qp/s_dense_qp_ipm_hard.c
@@ -0,0 +1,77 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_s_aux.h>
+
+#include "../include/hpipm_s_dense_qp.h"
+#include "../include/hpipm_s_dense_qp_sol.h"
+#include "../include/hpipm_s_dense_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard_aux.h"
+#include "../include/hpipm_s_dense_qp_kkt.h"
+
+
+
+#define COMPUTE_ALPHA_HARD_QP s_compute_alpha_hard_qp
+#define COMPUTE_CENTERING_CORRECTION_HARD_QP s_compute_centering_correction_hard_qp
+#define COMPUTE_MU_AFF_HARD_QP s_compute_mu_aff_hard_qp
+#define COMPUTE_RES_HARD_DENSE_QP s_compute_res_hard_dense_qp
+#define CREATE_IPM_HARD_CORE_QP s_create_ipm_hard_core_qp
+#define CREATE_STRMAT s_create_strmat
+#define CREATE_STRVEC s_create_strvec
+#define DENSE_QP s_dense_qp
+#define DENSE_QP_SOL s_dense_qp_sol
+#define FACT_SOLVE_KKT_STEP_HARD_DENSE_QP s_fact_solve_kkt_step_hard_dense_qp
+#define FACT_SOLVE_KKT_UNCONSTR_DENSE_QP s_fact_solve_kkt_unconstr_dense_qp
+#define INIT_VAR_HARD_DENSE_QP s_init_var_hard_dense_qp
+#define IPM_HARD_CORE_QP_WORKSPACE s_ipm_hard_core_qp_workspace
+#define IPM_HARD_DENSE_QP_ARG s_ipm_hard_dense_qp_arg
+#define IPM_HARD_DENSE_QP_WORKSPACE s_ipm_hard_dense_qp_workspace
+#define MEMSIZE_IPM_HARD_CORE_QP s_memsize_ipm_hard_core_qp
+#define REAL float
+#define SIZE_STRMAT s_size_strmat
+#define SIZE_STRVEC s_size_strvec
+#define SOLVE_KKT_STEP_HARD_DENSE_QP s_solve_kkt_step_hard_dense_qp
+#define STRMAT s_strmat
+#define STRVEC s_strvec
+#define UPDATE_VAR_HARD_QP s_update_var_hard_qp
+
+
+
+#define MEMSIZE_IPM_HARD_DENSE_QP s_memsize_ipm_hard_dense_qp
+#define CREATE_IPM_HARD_DENSE_QP s_create_ipm_hard_dense_qp
+#define SOLVE_IPM_HARD_DENSE_QP s_solve_ipm_hard_dense_qp
+#define SOLVE_IPM2_HARD_DENSE_QP s_solve_ipm2_hard_dense_qp
+
+
+
+#include "x_dense_qp_ipm_hard.c"
+
diff --git a/dense_qp/s_dense_qp_kkt.c b/dense_qp/s_dense_qp_kkt.c
new file mode 100644
index 0000000..f57b8ff
--- /dev/null
+++ b/dense_qp/s_dense_qp_kkt.c
@@ -0,0 +1,89 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#include <math.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_s_aux.h>
+#include <blasfeo_s_blas.h>
+
+#include "../include/hpipm_s_dense_qp.h"
+#include "../include/hpipm_s_dense_qp_sol.h"
+#include "../include/hpipm_s_dense_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard_aux.h"
+
+
+
+#define AXPY_LIBSTR saxpy_libstr
+#define COMPUTE_LAM_T_HARD_QP s_compute_lam_t_hard_qp
+#define COMPUTE_QX_HARD_QP s_compute_qx_hard_qp
+#define COMPUTE_QX_QX_HARD_QP s_compute_Qx_qx_hard_qp
+#define DENSE_QP s_dense_qp
+#define DENSE_QP_SOL s_dense_qp_sol
+#define DIAAD_SP_LIBSTR sdiaad_sp_libstr
+#define GECP_LIBSTR sgecp_libstr
+#define GEMM_R_DIAG_LIBSTR sgemm_r_diag_libstr
+#define GEMV_N_LIBSTR sgemv_n_libstr
+#define GEMV_NT_LIBSTR sgemv_nt_libstr
+#define GEMV_T_LIBSTR sgemv_t_libstr
+#define GESE_LIBSTR sgese_libstr
+#define IPM_HARD_CORE_QP_WORKSPACE s_ipm_hard_core_qp_workspace
+#define IPM_HARD_DENSE_QP_WORKSPACE s_ipm_hard_dense_qp_workspace
+#define POTRF_L_LIBSTR spotrf_l_libstr
+#define POTRF_L_MN_LIBSTR spotrf_l_mn_libstr
+#define REAL float
+#define ROWAD_SP_LIBSTR srowad_sp_libstr
+#define ROWEX_LIBSTR srowex_libstr
+#define ROWIN_LIBSTR srowin_libstr
+#define STRMAT s_strmat
+#define STRVEC s_strvec
+#define SYMV_L_LIBSTR ssymv_l_libstr
+#define SYRK_POTRF_LN_LIBSTR ssyrk_spotrf_ln_libstr
+#define TRCP_L_LIBSTR strcp_l_libstr
+#define TRSM_RLTN_LIBSTR strsm_rltn_libstr
+#define TRSV_LNN_LIBSTR strsv_lnn_libstr
+#define TRSV_LTN_LIBSTR strsv_ltn_libstr
+#define VECAD_SP_LIBSTR svecad_sp_libstr
+#define VECCP_LIBSTR sveccp_libstr
+#define VECEX_SP_LIBSTR svecex_sp_libstr
+#define VECMULDOT_LIBSTR svecmuldot_libstr
+#define VECSC_LIBSTR svecsc_libstr
+
+#define INIT_VAR_HARD_DENSE_QP s_init_var_hard_dense_qp
+#define COMPUTE_RES_HARD_DENSE_QP s_compute_res_hard_dense_qp
+#define FACT_SOLVE_KKT_UNCONSTR_DENSE_QP s_fact_solve_kkt_unconstr_dense_qp
+#define FACT_SOLVE_KKT_STEP_HARD_DENSE_QP s_fact_solve_kkt_step_hard_dense_qp
+#define SOLVE_KKT_STEP_HARD_DENSE_QP s_solve_kkt_step_hard_dense_qp
+
+
+
+#include "x_dense_qp_kkt.c"
+
diff --git a/dense_qp/s_dense_qp_sol.c b/dense_qp/s_dense_qp_sol.c
new file mode 100644
index 0000000..2a61ccc
--- /dev/null
+++ b/dense_qp/s_dense_qp_sol.c
@@ -0,0 +1,62 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#if defined(RUNTIME_CHECKS)
+#include <stdlib.h>
+#endif
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_s_aux.h>
+
+#include "../include/hpipm_s_dense_qp.h"
+#include "../include/hpipm_s_dense_qp_sol.h"
+
+
+
+#define CREATE_STRVEC s_create_strvec
+#define CVT_STRVEC2VEC s_cvt_strvec2vec
+#define DENSE_QP s_dense_qp
+#define DENSE_QP_SOL s_dense_qp_sol
+#define REAL float
+#define STRVEC s_strvec
+#define SIZE_STRVEC s_size_strvec
+#define VECCP_LIBSTR sveccp_libstr
+
+#define CREATE_DENSE_QP_SOL s_create_dense_qp_sol
+#define MEMSIZE_DENSE_QP_SOL s_memsize_dense_qp_sol
+#define CVT_DENSE_QP_SOL_TO_COLMAJ s_cvt_dense_qp_sol_to_colmaj
+#define CVT_DENSE_QP_SOL_TO_ROWMAJ s_cvt_dense_qp_sol_to_rowmaj
+#define CVT_DENSE_QP_SOL_TO_LIBSTR s_cvt_dense_qp_sol_to_libstr
+
+
+
+#include "x_dense_qp_sol.c"
+
+
diff --git a/dense_qp/x_dense_qp.c b/dense_qp/x_dense_qp.c
new file mode 100644
index 0000000..34ad1bb
--- /dev/null
+++ b/dense_qp/x_dense_qp.c
@@ -0,0 +1,363 @@
+/**************************************************************************************************
+* *
+* 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_DENSE_QP(int nv, int ne, int nb, int ng)
+ {
+
+ int size = 0;
+
+ size += 7*sizeof(struct STRVEC); // g b d d_lb d_ub d_lg d_ug
+ size += 3*sizeof(struct STRMAT); // Hg A Ct
+
+ size += 1*SIZE_STRVEC(nv); // g
+ size += 1*SIZE_STRVEC(ne); // b
+ size += 1*SIZE_STRVEC(2*nb+2*ng); // d
+ size += 1*nb*sizeof(int); // idxb
+
+ size += 1*SIZE_STRMAT(nv+1, nv); // Hg
+ size += 1*SIZE_STRMAT(ne, nv); // A
+ size += 1*SIZE_STRMAT(nv, ng); // Ct
+
+ size = (size+63)/64*64; // make multiple of typical cache line size
+ size += 1*64; // align once to typical cache line size
+
+ return size;
+
+ }
+
+
+
+void CREATE_DENSE_QP(int nv, int ne, int nb, int ng, struct DENSE_QP *qp, void *memory)
+ {
+
+ qp->memsize = MEMSIZE_DENSE_QP(nv, ne, nb, ng);
+
+
+ // problem size
+ qp->nv = nv;
+ qp->ne = ne;
+ qp->nb = nb;
+ qp->ng = ng;
+
+
+ // matrix struct stuff
+ struct STRMAT *sm_ptr = (struct STRMAT *) memory;
+
+ qp->Hg = sm_ptr;
+ sm_ptr += 1;
+
+ qp->A = sm_ptr;
+ sm_ptr += 1;
+
+ qp->Ct = sm_ptr;
+ sm_ptr += 1;
+
+
+ // vector struct stuff
+ struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr;
+
+ qp->g = sv_ptr;
+ sv_ptr += 1;
+
+ qp->b = sv_ptr;
+ sv_ptr += 1;
+
+ qp->d = sv_ptr;
+ sv_ptr += 1;
+
+ qp->d_lb = sv_ptr;
+ sv_ptr += 1;
+
+ qp->d_ub = sv_ptr;
+ sv_ptr += 1;
+
+ qp->d_lg = sv_ptr;
+ sv_ptr += 1;
+
+ qp->d_ug = sv_ptr;
+ sv_ptr += 1;
+
+
+ // int stuff
+ int *i_ptr;
+ i_ptr = (int *) sv_ptr;
+
+ // idxb
+ qp->idxb = i_ptr;
+ i_ptr += nb;
+
+
+ // align to typical cache line size
+ size_t s_ptr = (size_t) i_ptr;
+ s_ptr = (s_ptr+63)/64*64;
+
+
+ // stuff
+ char *c_ptr;
+ c_ptr = (char *) s_ptr;
+
+ CREATE_STRMAT(nv+1, nv, qp->Hg, c_ptr);
+ c_ptr += qp->Hg->memory_size;
+
+ CREATE_STRMAT(ne, nv, qp->A, c_ptr);
+ c_ptr += qp->A->memory_size;
+
+ CREATE_STRMAT(nv, ng, qp->Ct, c_ptr);
+ c_ptr += qp->Ct->memory_size;
+
+ CREATE_STRVEC(nv, qp->g, c_ptr);
+ c_ptr += qp->g->memory_size;
+
+ CREATE_STRVEC(ne, qp->b, c_ptr);
+ c_ptr += qp->b->memory_size;
+
+ CREATE_STRVEC(2*nb+2*ng, qp->d, c_ptr);
+ CREATE_STRVEC(nb, qp->d_lb, c_ptr+0*sizeof(REAL));
+ CREATE_STRVEC(ng, qp->d_lg, c_ptr+(nb)*sizeof(REAL));
+ CREATE_STRVEC(nb, qp->d_ub, c_ptr+(nb+ng)*sizeof(REAL));
+ CREATE_STRVEC(ng, qp->d_ug, c_ptr+(2*nb+ng)*sizeof(REAL));
+ c_ptr += qp->d->memory_size;
+
+ return;
+
+ }
+
+
+
+void CVT_COLMAJ_TO_DENSE_QP(REAL *H, REAL *g, REAL *A, REAL *b, int *idxb, REAL *d_lb, REAL *d_ub, REAL *C, REAL *d_lg, REAL *d_ug, struct DENSE_QP *qp)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_MAT2STRMAT(nv, nv, H, nv, qp->Hg, 0, 0);
+ CVT_TRAN_MAT2STRMAT(nv, 1, g, nv, qp->Hg, nv, 0);
+ CVT_MAT2STRMAT(ne, nv, A, ne, qp->A, 0, 0);
+ CVT_TRAN_MAT2STRMAT(ng, nv, C, ng, qp->Ct, 0, 0);
+ CVT_VEC2STRVEC(nv, g, qp->g, 0);
+ CVT_VEC2STRVEC(ne, b, qp->b, 0);
+ CVT_VEC2STRVEC(nb, d_lb, qp->d_lb, 0);
+ CVT_VEC2STRVEC(nb, d_ub, qp->d_ub, 0);
+ CVT_VEC2STRVEC(ng, d_lg, qp->d_lg, 0);
+ CVT_VEC2STRVEC(ng, d_ug, qp->d_ug, 0);
+ for(ii=0; ii<nb; ii++) qp->idxb[ii] = idxb[ii];
+
+ return;
+
+ }
+
+
+
+void CVT_DENSE_QP_TO_COLMAJ(struct DENSE_QP *qp, REAL *H, REAL *g, REAL *A, REAL *b, int *idxb, REAL *d_lb, REAL *d_ub, REAL *C, REAL *d_lg, REAL *d_ug)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_STRMAT2MAT(nv, nv, qp->Hg, 0, 0, H, nv);
+ CVT_STRMAT2MAT(ne, nv, qp->A, 0, 0, A, ne);
+ CVT_TRAN_STRMAT2MAT(nv, ng, qp->Ct, 0, 0, C, ng);
+ CVT_STRVEC2VEC(nv, qp->g, 0, g);
+ CVT_STRVEC2VEC(ne, qp->b, 0, b);
+ CVT_STRVEC2VEC(nb, qp->d_lb, 0, d_lb);
+ CVT_STRVEC2VEC(nb, qp->d_ub, 0, d_ub);
+ CVT_STRVEC2VEC(ng, qp->d_lg, 0, d_lg);
+ CVT_STRVEC2VEC(ng, qp->d_ug, 0, d_ug);
+ for(ii=0; ii<nb; ii++) idxb[ii] = qp->idxb[ii];
+
+ return;
+
+ }
+
+
+
+void CVT_ROWMAJ_TO_DENSE_QP(REAL *H, REAL *g, REAL *A, REAL *b, int *idxb, REAL *d_lb, REAL *d_ub, REAL *C, REAL *d_lg, REAL *d_ug, struct DENSE_QP *qp)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_TRAN_MAT2STRMAT(nv, nv, H, nv, qp->Hg, 0, 0);
+ CVT_TRAN_MAT2STRMAT(nv, 1, g, nv, qp->Hg, nv, 0);
+ CVT_TRAN_MAT2STRMAT(nv, ne, A, nv, qp->A, 0, 0);
+ CVT_MAT2STRMAT(nv, ng, C, nv, qp->Ct, 0, 0);
+ CVT_VEC2STRVEC(nv, g, qp->g, 0);
+ CVT_VEC2STRVEC(ne, b, qp->b, 0);
+ CVT_VEC2STRVEC(nb, d_lb, qp->d_lb, 0);
+ CVT_VEC2STRVEC(nb, d_ub, qp->d_ub, 0);
+ CVT_VEC2STRVEC(ng, d_lg, qp->d_lg, 0);
+ CVT_VEC2STRVEC(ng, d_ug, qp->d_ug, 0);
+ for(ii=0; ii<nb; ii++) qp->idxb[ii] = idxb[ii];
+
+ return;
+
+ }
+
+
+
+void CVT_DENSE_QP_TO_ROWMAJ(struct DENSE_QP *qp, REAL *H, REAL *g, REAL *A, REAL *b, int *idxb, REAL *d_lb, REAL *d_ub, REAL *C, REAL *d_lg, REAL *d_ug)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_TRAN_STRMAT2MAT(nv, nv, qp->Hg, 0, 0, H, nv);
+ CVT_TRAN_STRMAT2MAT(ne, nv, qp->A, 0, 0, A, nv);
+ CVT_STRMAT2MAT(nv, ng, qp->Ct, 0, 0, C, nv);
+ CVT_STRVEC2VEC(nv, qp->g, 0, g);
+ CVT_STRVEC2VEC(ne, qp->b, 0, b);
+ CVT_STRVEC2VEC(nb, qp->d_lb, 0, d_lb);
+ CVT_STRVEC2VEC(nb, qp->d_ub, 0, d_ub);
+ CVT_STRVEC2VEC(ng, qp->d_lg, 0, d_lg);
+ CVT_STRVEC2VEC(ng, qp->d_ug, 0, d_ug);
+ for(ii=0; ii<nb; ii++) idxb[ii] = qp->idxb[ii];
+
+ return;
+
+ }
+
+
+
+void CVT_LIBSTR_TO_DENSE_QP(struct STRMAT *H, struct STRMAT *A, struct STRMAT *C, struct STRVEC *g, struct STRVEC *b, struct STRVEC *d_lb, struct STRVEC *d_ub, struct STRVEC *d_lg, struct STRVEC *d_ug, int *idxb, struct DENSE_QP *qp)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ GECP_LIBSTR(nv, nv, H, 0, 0, qp->Hg, 0, 0);
+ ROWIN_LIBSTR(nv, 1.0, g, 0, qp->Hg, nv, 0);
+ GECP_LIBSTR(ne, nv, A, 0, 0, qp->A, 0, 0);
+ GETR_LIBSTR(ng, nv, C, 0, 0, qp->Ct, 0, 0);
+ VECCP_LIBSTR(nv, g, 0, qp->g, 0);
+ VECCP_LIBSTR(ne, b, 0, qp->b, 0);
+ VECCP_LIBSTR(nb, d_lb, 0, qp->d_lb, 0);
+ VECCP_LIBSTR(nb, d_ub, 0, qp->d_ub, 0);
+ VECCP_LIBSTR(ng, d_lg, 0, qp->d_lg, 0);
+ VECCP_LIBSTR(ng, d_ug, 0, qp->d_ug, 0);
+ for(ii=0; ii<nb; ii++) qp->idxb[ii] = idxb[ii];
+
+ return;
+
+ }
+
+
+
+void CVT_DENSE_QP_TO_LIBSTR(struct DENSE_QP *qp, struct STRMAT *H, struct STRMAT *A, struct STRMAT *C, struct STRVEC *g, struct STRVEC *b, struct STRVEC *d_lb, struct STRVEC *d_ub, struct STRVEC *d_lg, struct STRVEC *d_ug, int *idxb)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ GECP_LIBSTR(nv, nv, qp->Hg, 0, 0, H, 0, 0);
+ GECP_LIBSTR(ne, nv, qp->A, 0, 0, A, 0, 0);
+ GETR_LIBSTR(nv, ng, qp->Ct, 0, 0, C, 0, 0);
+ VECCP_LIBSTR(nv, qp->g, 0, g, 0);
+ VECCP_LIBSTR(ne, qp->b, 0, b, 0);
+ VECCP_LIBSTR(nb, qp->d_lb, 0, d_lb, 0);
+ VECCP_LIBSTR(nb, qp->d_ub, 0, d_ub, 0);
+ VECCP_LIBSTR(ng, qp->d_lg, 0, d_lg, 0);
+ VECCP_LIBSTR(ng, qp->d_ug, 0, d_ug, 0);
+ for(ii=0; ii<nb; ii++) idxb[ii] = qp->idxb[ii];
+
+ return;
+
+ }
+
+
+
+#if 0
+void COPY_DENSE_QP(struct DENSE_QP *str_in, struct DENSE_QP *str_out)
+ {
+
+ int ii;
+
+#if defined(RUNTIME_CHECKS)
+ if(str_out->nv != str_in->nv)
+ {
+ printf("\nError : d_copy_dense_qp : str_out->nv != str_out->nv : %d != %d\n\n", str_out->nv, str_in->nv);
+ exit(1);
+ }
+ if(str_out->ne != str_in->ne)
+ {
+ printf("\nError : d_copy_dense_qp : str_out->ne != str_out->ne : %d != %d\n\n", str_out->ne, str_in->ne);
+ exit(1);
+ }
+ if(str_out->nb != str_in->nb)
+ {
+ printf("\nError : d_copy_dense_qp : str_out->nb != str_out->nb : %d != %d\n\n", str_out->nb, str_in->nb);
+ exit(1);
+ }
+ if(str_out->ng != str_in->ng)
+ {
+ printf("\nError : d_copy_dense_qp : str_out->ng != str_out->ng : %d != %d\n\n", str_out->ng, str_in->ng);
+ exit(1);
+ }
+#endif
+
+ for(ii=0; ii<str_in->nb; ii++) str_out->idxb[ii] = str_in->idxb[ii];
+ GECP_LIBSTR(str_in->nv, str_in->nv, &(str_out->sQ), 0, 0, &(str_in->sQ), 0, 0);
+ VECCP_LIBSTR(str_in->nv, &(str_out->sq), 0, &(str_in->sq), 0);
+ GECP_LIBSTR(str_in->ne, str_in->nv, &(str_out->sA), 0, 0, &(str_in->sA), 0, 0);
+ VECCP_LIBSTR(str_in->ne, &(str_out->sb), 0, &(str_in->sb), 0);
+ GECP_LIBSTR(str_in->nv, str_in->ng, &(str_out->sCt), 0, 0, &(str_in->sCt), 0, 0);
+ VECCP_LIBSTR(str_in->nb, &(str_out->slb), 0, &(str_in->slb), 0);
+ VECCP_LIBSTR(str_in->nb, &(str_out->sub), 0, &(str_in->sub), 0);
+ VECCP_LIBSTR(str_in->ng, &(str_out->slg), 0, &(str_in->slg), 0);
+ VECCP_LIBSTR(str_in->ng, &(str_out->sug), 0, &(str_in->sug), 0);
+
+ return;
+
+ }
+#endif
+
+
diff --git a/dense_qp/x_dense_qp_ipm_hard.c b/dense_qp/x_dense_qp_ipm_hard.c
new file mode 100644
index 0000000..beae7a3
--- /dev/null
+++ b/dense_qp/x_dense_qp_ipm_hard.c
@@ -0,0 +1,377 @@
+/**************************************************************************************************
+* *
+* 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_IPM_HARD_DENSE_QP(struct DENSE_QP *qp, struct IPM_HARD_DENSE_QP_ARG *arg)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ int size = 0;
+
+ size += 22*sizeof(struct STRVEC); // dv dpi dlam dt dt_lb dt_ub dt_lg dt_ug res_g res_b res_d res_d_lb res_d_ub res_d_lg res_d_ug res_m Qx qx lv tmp_nb tmp_ng0 tmp_ng1
+ size += 4*sizeof(struct STRMAT); // Lv AL Le Ctx
+
+ size += 1*SIZE_STRVEC(nb); // tmp_nb
+ size += 2*SIZE_STRVEC(ng); // tmp_ng0 tmp_ng1
+ size += 1*SIZE_STRVEC(nv); // lv
+ size += 1*SIZE_STRMAT(nv+1, nv); // Lv
+ size += 1*SIZE_STRMAT(ne, nv); // AL
+ size += 1*SIZE_STRMAT(ne, ne); // Le
+ size += 1*SIZE_STRMAT(nv+1, ng); // Ctx
+
+ size += 1*sizeof(struct IPM_HARD_CORE_QP_WORKSPACE);
+ size += 1*MEMSIZE_IPM_HARD_CORE_QP(qp->nv, qp->ne, qp->nb, qp->ng, arg->iter_max);
+
+ size = (size+63)/64*64; // make multiple of typical cache line size
+ size += 1*64; // align once to typical cache line size
+
+ return size;
+
+ }
+
+
+
+void CREATE_IPM_HARD_DENSE_QP(struct DENSE_QP *qp, struct IPM_HARD_DENSE_QP_ARG *arg, struct IPM_HARD_DENSE_QP_WORKSPACE *workspace, void *mem)
+ {
+
+ workspace->memsize = MEMSIZE_IPM_HARD_DENSE_QP(qp, arg);
+
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+
+ // core struct
+ struct IPM_HARD_CORE_QP_WORKSPACE *sr_ptr = mem;
+
+ // core workspace
+ workspace->core_workspace = sr_ptr;
+ sr_ptr += 1;
+ struct IPM_HARD_CORE_QP_WORKSPACE *rwork = workspace->core_workspace;
+
+
+ // matrix struct
+ struct STRMAT *sm_ptr = (struct STRMAT *) sr_ptr;
+
+ workspace->Lv = sm_ptr;
+ sm_ptr += 1;
+ workspace->AL = sm_ptr;
+ sm_ptr += 1;
+ workspace->Le = sm_ptr;
+ sm_ptr += 1;
+ workspace->Ctx = sm_ptr;
+ sm_ptr += 1;
+
+
+ // vector struct
+ struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr;
+
+ workspace->dv = sv_ptr;
+ sv_ptr += 1;
+ workspace->dpi = sv_ptr;
+ sv_ptr += 1;
+ workspace->dlam = sv_ptr;
+ sv_ptr += 1;
+ workspace->dt = sv_ptr;
+ sv_ptr += 1;
+ workspace->dt_lb = sv_ptr;
+ sv_ptr += 1;
+ workspace->dt_ub = sv_ptr;
+ sv_ptr += 1;
+ workspace->dt_lg = sv_ptr;
+ sv_ptr += 1;
+ workspace->dt_ug = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_g = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_b = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d_lb = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d_ub = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d_lg = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d_ug = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_m = sv_ptr;
+ sv_ptr += 1;
+ workspace->Qx = sv_ptr;
+ sv_ptr += 1;
+ workspace->qx = sv_ptr;
+ sv_ptr += 1;
+ workspace->lv = sv_ptr;
+ sv_ptr += 1;
+ workspace->tmp_nb = sv_ptr;
+ sv_ptr += 1;
+ workspace->tmp_ng0 = sv_ptr;
+ sv_ptr += 1;
+ workspace->tmp_ng1 = sv_ptr;
+ sv_ptr += 1;
+
+
+ // align to typicl cache line size
+ size_t s_ptr = (size_t) sv_ptr;
+ s_ptr = (s_ptr+63)/64*64;
+
+
+ // void stuf
+ void *v_ptr = (void *) s_ptr;
+
+ CREATE_STRMAT(nv+1, nv, workspace->Lv, v_ptr);
+ v_ptr += workspace->Lv->memory_size;
+
+ CREATE_STRMAT(ne, nv, workspace->AL, v_ptr);
+ v_ptr += workspace->AL->memory_size;
+
+ CREATE_STRMAT(ne, ne, workspace->Le, v_ptr);
+ v_ptr += workspace->Le->memory_size;
+
+ CREATE_STRMAT(nv+1, ng, workspace->Ctx, v_ptr);
+ v_ptr += workspace->Ctx->memory_size;
+
+ CREATE_STRVEC(nv, workspace->lv, v_ptr);
+ v_ptr += workspace->lv->memory_size;
+
+ CREATE_STRVEC(nb, workspace->tmp_nb, v_ptr);
+ v_ptr += workspace->tmp_nb->memory_size;
+
+ CREATE_STRVEC(ng, workspace->tmp_ng0, v_ptr);
+ v_ptr += workspace->tmp_ng0->memory_size;
+
+ CREATE_STRVEC(ng, workspace->tmp_ng1, v_ptr);
+ v_ptr += workspace->tmp_ng1->memory_size;
+
+ rwork->nv = nv;
+ rwork->ne = ne;
+ rwork->nb = nb;
+ rwork->ng = ng;
+ rwork->iter_max = arg->iter_max;
+ CREATE_IPM_HARD_CORE_QP(rwork, v_ptr);
+ v_ptr += workspace->core_workspace->memsize;
+
+ rwork->alpha_min = arg->alpha_min;
+ rwork->mu_max = arg->mu_max;
+ rwork->mu0 = arg->mu0;
+ rwork->nt_inv = 1.0/(2*nb+2*ng);
+
+
+ // alias members of workspace and core_workspace
+ CREATE_STRVEC(nv, workspace->dv, rwork->dv);
+ CREATE_STRVEC(ne, workspace->dpi, rwork->dpi);
+ CREATE_STRVEC(2*nb+2*ng, workspace->dlam, rwork->dlam);
+ CREATE_STRVEC(2*nb+2*ng, workspace->dt, rwork->dt);
+ CREATE_STRVEC(nb, workspace->dt_lb, rwork->dt_lb);
+ CREATE_STRVEC(nb, workspace->dt_ub, rwork->dt_ub);
+ CREATE_STRVEC(ng, workspace->dt_lg, rwork->dt_lg);
+ CREATE_STRVEC(ng, workspace->dt_ug, rwork->dt_ug);
+ CREATE_STRVEC(nv, workspace->res_g, rwork->res_g);
+ CREATE_STRVEC(ne, workspace->res_b, rwork->res_b);
+ CREATE_STRVEC(2*nb+2*ng, workspace->res_d, rwork->res_d);
+ CREATE_STRVEC(nb, workspace->res_d_lb, rwork->res_d_lb);
+ CREATE_STRVEC(nb, workspace->res_d_ub, rwork->res_d_ub);
+ CREATE_STRVEC(ng, workspace->res_d_lg, rwork->res_d_lg);
+ CREATE_STRVEC(ng, workspace->res_d_ug, rwork->res_d_ug);
+ CREATE_STRVEC(2*nb+2*ng, workspace->res_m, rwork->res_m);
+ CREATE_STRVEC(nb+ng, workspace->Qx, rwork->Qx);
+ CREATE_STRVEC(nb+ng, workspace->qx, rwork->qx);
+ workspace->stat = rwork->stat;
+
+ return;
+
+ }
+
+
+
+void SOLVE_IPM_HARD_DENSE_QP(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+ // alias qp vectors into qp
+ cws->d = qp->d->pa; // TODO REMOVE
+ cws->d_lb = qp->d_lb->pa;
+ cws->d_ub = qp->d_ub->pa;
+ cws->d_lg = qp->d_lg->pa;
+ cws->d_ug = qp->d_ug->pa;
+
+ // alias qp vectors into qp_sol
+ cws->v = qp_sol->v->pa;
+ cws->pi = qp_sol->pi->pa;
+ cws->lam = qp_sol->lam_lb->pa;
+ cws->lam_lb = qp_sol->lam_lb->pa;
+ cws->lam_ub = qp_sol->lam_ub->pa;
+ cws->lam_lg = qp_sol->lam_lg->pa;
+ cws->lam_ug = qp_sol->lam_ug->pa;
+ cws->t = qp_sol->t_lb->pa;
+ cws->t_lb = qp_sol->t_lb->pa;
+ cws->t_ub = qp_sol->t_ub->pa;
+ cws->t_lg = qp_sol->t_lg->pa;
+ cws->t_ug = qp_sol->t_ug->pa;
+
+ if(cws->nb+cws->ng==0)
+ {
+ FACT_SOLVE_KKT_UNCONSTR_DENSE_QP(qp, qp_sol, ws);
+ COMPUTE_RES_HARD_DENSE_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ ws->iter = 0;
+ return;
+ }
+
+ // init solver
+ INIT_VAR_HARD_DENSE_QP(qp, qp_sol, ws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_DENSE_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+
+ int kk;
+ for(kk=0; kk<cws->iter_max & cws->mu>cws->mu_max; kk++)
+ {
+
+ // fact and solve kkt
+ FACT_SOLVE_KKT_STEP_HARD_DENSE_QP(qp, ws);
+
+ // alpha
+ COMPUTE_ALPHA_HARD_QP(cws);
+ cws->stat[5*kk+0] = cws->alpha;
+
+ //
+ UPDATE_VAR_HARD_QP(cws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_DENSE_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ cws->stat[5*kk+1] = ws->res_mu;
+
+ }
+
+ ws->iter = kk;
+
+ return;
+
+ }
+
+
+
+void SOLVE_IPM2_HARD_DENSE_QP(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+ // alias qp vectors into qp
+ cws->d = qp->d->pa; // TODO REMOVE
+ cws->d_lb = qp->d_lb->pa;
+ cws->d_ub = qp->d_ub->pa;
+ cws->d_lg = qp->d_lg->pa;
+ cws->d_ug = qp->d_ug->pa;
+
+ // alias qp vectors into qp_sol
+ cws->v = qp_sol->v->pa;
+ cws->pi = qp_sol->pi->pa;
+ cws->lam = qp_sol->lam_lb->pa;
+ cws->lam_lb = qp_sol->lam_lb->pa;
+ cws->lam_ub = qp_sol->lam_ub->pa;
+ cws->lam_lg = qp_sol->lam_lg->pa;
+ cws->lam_ug = qp_sol->lam_ug->pa;
+ cws->t = qp_sol->t_lb->pa;
+ cws->t_lb = qp_sol->t_lb->pa;
+ cws->t_ub = qp_sol->t_ub->pa;
+ cws->t_lg = qp_sol->t_lg->pa;
+ cws->t_ug = qp_sol->t_ug->pa;
+
+ REAL tmp;
+
+ if(cws->nb+cws->ng==0)
+ {
+ FACT_SOLVE_KKT_UNCONSTR_DENSE_QP(qp, qp_sol, ws);
+ COMPUTE_RES_HARD_DENSE_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ ws->iter = 0;
+ return;
+ }
+
+ // init solver
+ INIT_VAR_HARD_DENSE_QP(qp, qp_sol, ws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_DENSE_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+
+ int kk;
+ for(kk=0; kk<cws->iter_max & cws->mu>cws->mu_max; kk++)
+ {
+
+ // fact and solve kkt
+ FACT_SOLVE_KKT_STEP_HARD_DENSE_QP(qp, ws);
+
+ // alpha
+ COMPUTE_ALPHA_HARD_QP(cws);
+ cws->stat[5*kk+0] = cws->alpha;
+
+ // mu_aff
+ COMPUTE_MU_AFF_HARD_QP(cws);
+ cws->stat[5*kk+1] = cws->mu_aff;
+
+ tmp = cws->mu_aff/cws->mu;
+ cws->sigma = tmp*tmp*tmp;
+ cws->stat[5*kk+2] = cws->sigma;
+
+ COMPUTE_CENTERING_CORRECTION_HARD_QP(cws);
+
+ // fact and solve kkt
+ SOLVE_KKT_STEP_HARD_DENSE_QP(qp, ws);
+
+ // alpha
+ COMPUTE_ALPHA_HARD_QP(cws);
+ cws->stat[5*kk+3] = cws->alpha;
+
+ //
+ UPDATE_VAR_HARD_QP(cws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_DENSE_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ cws->stat[5*kk+4] = ws->res_mu;
+
+ }
+
+ ws->iter = kk;
+
+ return;
+
+ }
+
diff --git a/dense_qp/x_dense_qp_kkt.c b/dense_qp/x_dense_qp_kkt.c
new file mode 100644
index 0000000..994532d
--- /dev/null
+++ b/dense_qp/x_dense_qp_kkt.c
@@ -0,0 +1,523 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+void INIT_VAR_HARD_DENSE_QP(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *rws = ws->core_workspace;
+
+ // extract rws members
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ REAL *d_lb = qp->d_lb->pa;
+ REAL *d_ub = qp->d_ub->pa;
+ REAL *d_lg = qp->d_lg->pa;
+ REAL *d_ug = qp->d_ug->pa;
+ int *idxb = qp->idxb;
+
+ REAL *v = qp_sol->v->pa;
+ REAL *pi = qp_sol->pi->pa;
+ REAL *lam_lb = qp_sol->lam_lb->pa;
+ REAL *lam_ub = qp_sol->lam_ub->pa;
+ REAL *lam_lg = qp_sol->lam_lg->pa;
+ REAL *lam_ug = qp_sol->lam_ug->pa;
+ REAL *t_lb = qp_sol->t_lb->pa;
+ REAL *t_ub = qp_sol->t_ub->pa;
+ REAL *t_lg = qp_sol->t_lg->pa;
+ REAL *t_ug = qp_sol->t_ug->pa;
+
+ REAL mu0 = rws->mu0;
+
+ // local variables
+ int ii;
+ int idxb0;
+ REAL thr0 = 0.1;
+
+ // warm start TODO
+
+
+ // cold start
+
+ // primal variables
+ for(ii=0; ii<nv; ii++)
+ {
+ v[ii] = 0.0;
+ }
+
+ // equality constraints
+ for(ii=0; ii<ne; ii++)
+ {
+ pi[ii] = 0.0;
+ }
+
+ // box constraints
+ for(ii=0; ii<nb; ii++)
+ {
+ idxb0 = idxb[ii];
+ t_lb[ii] = - d_lb[ii] + v[idxb0];
+ t_ub[ii] = d_ub[ii] - v[idxb0];
+ if(t_lb[ii]<thr0)
+ {
+ if(t_ub[ii]<thr0)
+ {
+ v[idxb0] = 0.5*(d_lb[ii] - d_ub[ii]);
+ t_lb[ii] = thr0;
+ t_ub[ii] = thr0;
+ }
+ else
+ {
+ t_lb[ii] = thr0;
+ v[idxb0] = d_lb[ii] + thr0;
+ }
+ }
+ else if(t_ub[ii]<thr0)
+ {
+ t_ub[ii] = thr0;
+ v[idxb0] = d_ub[ii] - thr0;
+ }
+ lam_lb[ii] = mu0/t_lb[ii];
+ lam_ub[ii] = mu0/t_ub[ii];
+ }
+
+ // inequality constraints
+ GEMV_T_LIBSTR(nv, ng, 1.0, qp->Ct, 0, 0, qp_sol->v, 0, 0.0, qp_sol->t_lg, 0, qp_sol->t_lg, 0);
+ for(ii=0; ii<ng; ii++)
+ {
+ t_ug[ii] = t_lg[ii];
+ t_lg[ii] -= d_lg[ii];
+ t_ug[ii] += d_ug[ii];
+ t_lg[ii] = fmax( thr0, t_lg[ii] );
+ t_ug[ii] = fmax( thr0, t_ug[ii] );
+// t_lg[ii] = thr0>t_lg[ii] ? thr0 : t_lg[ii];
+// t_ug[ii] = thr0>t_ug[ii] ? thr0 : t_ug[ii];
+ lam_lg[ii] = mu0/t_lg[ii];
+ lam_ug[ii] = mu0/t_ug[ii];
+ }
+
+ return;
+
+ }
+
+
+
+void COMPUTE_RES_HARD_DENSE_QP(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ // TODO extract qp arguments !!!!!
+
+ struct STRVEC *v = qp_sol->v;
+ struct STRVEC *pi = qp_sol->pi;
+ struct STRVEC *lam_lb = qp_sol->lam_lb;
+ struct STRVEC *lam_ub = qp_sol->lam_ub;
+ struct STRVEC *lam_lg = qp_sol->lam_lg;
+ struct STRVEC *lam_ug = qp_sol->lam_ug;
+ struct STRVEC *t_lb = qp_sol->t_lb;
+ struct STRVEC *t_ub = qp_sol->t_ub;
+ struct STRVEC *t_lg = qp_sol->t_lg;
+ struct STRVEC *t_ug = qp_sol->t_ug;
+
+ REAL mu;
+
+ // res g
+ SYMV_L_LIBSTR(nv, nv, 1.0, qp->Hg, 0, 0, v, 0, 1.0, qp->g, 0, ws->res_g, 0);
+
+ if(nb>0)
+ {
+ // res_g
+ AXPY_LIBSTR(nb, -1.0, lam_lb, 0, lam_ub, 0, ws->tmp_nb, 0);
+ VECAD_SP_LIBSTR(nb, 1.0, ws->tmp_nb, 0, qp->idxb, ws->res_g, 0);
+ // res_d
+ VECEX_SP_LIBSTR(nb, -1.0, qp->idxb, v, 0, ws->res_d_lb, 0);
+ VECCP_LIBSTR(nb, ws->res_d_lb, 0, ws->res_d_ub, 0);
+ AXPY_LIBSTR(nb, 1.0, qp->d_lb, 0, ws->res_d_lb, 0, ws->res_d_lb, 0);
+ AXPY_LIBSTR(nb, 1.0, qp->d_ub, 0, ws->res_d_ub, 0, ws->res_d_ub, 0);
+ AXPY_LIBSTR(nb, 1.0, t_lb, 0, ws->res_d_lb, 0, ws->res_d_lb, 0);
+ AXPY_LIBSTR(nb, -1.0, t_ub, 0, ws->res_d_ub, 0, ws->res_d_ub, 0);
+ }
+
+ if(ng>0)
+ {
+ AXPY_LIBSTR(ng, -1.0, lam_lg, 0, lam_ug, 0, ws->tmp_ng0, 0);
+ AXPY_LIBSTR(ng, 1.0, t_lg, 0, qp->d_lg, 0, ws->res_d_lg, 0);
+ AXPY_LIBSTR(ng, -1.0, t_ug, 0, qp->d_ug, 0, ws->res_d_ug, 0);
+ GEMV_NT_LIBSTR(nv, ng, 1.0, 1.0, qp->Ct, 0, 0, ws->tmp_ng0, 0, v, 0, 1.0, 0.0, ws->res_g, 0, ws->tmp_ng1, 0, ws->res_g, 0, ws->tmp_ng1, 0);
+ AXPY_LIBSTR(ng, -1.0, ws->tmp_ng1, 0, ws->res_d_lg, 0, ws->res_d_lg, 0);
+ AXPY_LIBSTR(ng, -1.0, ws->tmp_ng1, 0, ws->res_d_ug, 0, ws->res_d_ug, 0);
+ }
+
+ // res b, res g
+ GEMV_NT_LIBSTR(ne, nv, -1.0, -1.0, qp->A, 0, 0, v, 0, pi, 0, 1.0, 1.0, qp->b, 0, ws->res_g, 0, ws->res_b, 0, ws->res_g, 0);
+
+ // res_mu
+ mu = VECMULDOT_LIBSTR(2*nb+2*ng, lam_lb, 0, t_lb, 0, ws->res_m, 0);
+
+ if(cws->nb+cws->ng>0)
+ ws->res_mu = mu*cws->nt_inv;
+ else
+ ws->res_mu = 0.0;
+
+
+ return;
+
+ }
+
+
+// range-space (Schur complement) method
+void FACT_SOLVE_KKT_UNCONSTR_DENSE_QP(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+ struct STRMAT *Hg = qp->Hg;
+ struct STRMAT *A = qp->A;
+ struct STRVEC *g = qp->g;
+ struct STRVEC *b = qp->b;
+
+ struct STRVEC *v = qp_sol->v;
+ struct STRVEC *pi = qp_sol->pi;
+
+ struct STRMAT *Lv = ws->Lv;
+ struct STRMAT *Le = ws->Le;
+ struct STRMAT *Ctx = ws->Ctx;
+ struct STRMAT *AL = ws->AL;
+ struct STRVEC *lv = ws->lv;
+
+ if(ne>0)
+ {
+ POTRF_L_LIBSTR(nv, Hg, 0, 0, Lv, 0, 0);
+
+ GECP_LIBSTR(ne, nv, A, 0, 0, AL, 0, 0);
+ TRSM_RLTN_LIBSTR(ne, nv, 1.0, Lv, 0, 0, A, 0, 0, AL, 0, 0);
+
+ GESE_LIBSTR(ne, ne, 0.0, Le, 0, 0);
+ SYRK_POTRF_LN_LIBSTR(ne, ne, nv, AL, 0, 0, AL, 0, 0, Le, 0, 0, Le, 0, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, g, 0, lv, 0);
+
+ GEMV_N_LIBSTR(ne, nv, 1.0, AL, 0, 0, lv, 0, 1.0, b, 0, pi, 0);
+
+ TRSV_LNN_LIBSTR(ne, Le, 0, 0, pi, 0, pi, 0);
+ TRSV_LTN_LIBSTR(ne, Le, 0, 0, pi, 0, pi, 0);
+
+ GEMV_T_LIBSTR(ne, nv, 1.0, A, 0, 0, pi, 0, -1.0, g, 0, v, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, v, 0, v, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, v, 0, v, 0);
+ }
+ else
+ {
+#if 0
+ POTRF_L_LIBSTR(nv, Hg, 0, 0, Lv, 0, 0);
+
+ VECCP_LIBSTR(nv, g, 0, v, 0);
+ VECSC_LIBSTR(nv, -1.0, v, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, v, 0, v, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, v, 0, v, 0);
+#else
+ POTRF_L_MN_LIBSTR(nv+1, nv, Hg, 0, 0, Lv, 0, 0);
+
+ ROWEX_LIBSTR(nv, -1.0, Lv, nv, 0, v, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, v, 0, v, 0);
+#endif
+ }
+
+ return;
+
+ }
+
+
+
+// range-space (Schur complement) method
+void FACT_SOLVE_KKT_STEP_HARD_DENSE_QP(struct DENSE_QP *qp, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+ struct STRMAT *Hg = qp->Hg;
+ struct STRMAT *A = qp->A;
+ struct STRMAT *Ct = qp->Ct;
+ int *idxb = qp->idxb;
+
+ struct STRMAT *Lv = ws->Lv;
+ struct STRMAT *Le = ws->Le;
+ struct STRMAT *Ctx = ws->Ctx;
+ struct STRMAT *AL = ws->AL;
+ struct STRVEC *lv = ws->lv;
+ struct STRVEC *dv = ws->dv;
+ struct STRVEC *dpi = ws->dpi;
+ struct STRVEC *dt_lb = ws->dt_lb;
+ struct STRVEC *dt_lg = ws->dt_lg;
+ struct STRVEC *res_g = ws->res_g;
+ struct STRVEC *res_b = ws->res_b;
+ struct STRVEC *Qx = ws->Qx;
+ struct STRVEC *qx = ws->qx;
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *rws = ws->core_workspace;
+
+ if(nb>0 | ng>0)
+ {
+ COMPUTE_QX_QX_HARD_QP(rws);
+ }
+
+ if(ne>0)
+ {
+// TRCP_L_LIBSTR(nv, Hg, 0, 0, Lv, 0, 0);
+ GECP_LIBSTR(nv, nv, Hg, 0, 0, Lv, 0, 0);
+
+ VECCP_LIBSTR(nv, res_g, 0, lv, 0);
+
+ if(nb>0)
+ {
+ DIAAD_SP_LIBSTR(nb, 1.0, Qx, 0, idxb, Lv, 0, 0);
+ VECAD_SP_LIBSTR(nb, 1.0, qx, 0, idxb, lv, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMV_N_LIBSTR(nv, ng, 1.0, Ct, 0, 0, qx, nb, 1.0, lv, 0, lv, 0);
+ GEMM_R_DIAG_LIBSTR(nv, ng, 1.0, Ct, 0, 0, Qx, nb, 0.0, Ctx, 0, 0, Ctx, 0, 0);
+ SYRK_POTRF_LN_LIBSTR(nv, nv, ng, Ctx, 0, 0, Ct, 0, 0, Lv, 0, 0, Lv, 0, 0);
+ }
+ else
+ {
+ POTRF_L_LIBSTR(nv, Lv, 0, 0, Lv, 0, 0);
+ }
+
+ VECCP_LIBSTR(nv, lv, 0, dv, 0);
+
+ GECP_LIBSTR(ne, nv, A, 0, 0, AL, 0, 0);
+ TRSM_RLTN_LIBSTR(ne, nv, 1.0, Lv, 0, 0, A, 0, 0, AL, 0, 0);
+
+ GESE_LIBSTR(ne, ne, 0.0, Le, 0, 0);
+ SYRK_POTRF_LN_LIBSTR(ne, ne, nv, AL, 0, 0, AL, 0, 0, Le, 0, 0, Le, 0, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, lv, 0, lv, 0);
+
+ GEMV_N_LIBSTR(ne, nv, 1.0, AL, 0, 0, lv, 0, 1.0, res_b, 0, dpi, 0);
+
+ TRSV_LNN_LIBSTR(ne, Le, 0, 0, dpi, 0, dpi, 0);
+ TRSV_LTN_LIBSTR(ne, Le, 0, 0, dpi, 0, dpi, 0);
+
+ GEMV_T_LIBSTR(ne, nv, 1.0, A, 0, 0, dpi, 0, -1.0, dv, 0, dv, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ }
+ else
+ {
+#if 0
+ TRCP_L_LIBSTR(nv, Hg, 0, 0, Lv, 0, 0);
+ VECCP_LIBSTR(nv, res_g, 0, lv, 0);
+
+ if(nb>0)
+ {
+ DIAAD_SP_LIBSTR(nb, 1.0, Qx, 0, idxb, Lv, 0, 0);
+ VECAD_SP_LIBSTR(nb, 1.0, qx, 0, idxb, lv, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMM_R_DIAG_LIBSTR(nv, ng, 1.0, Ct, 0, 0, Qx, nb, 0.0, Ctx, 0, 0, Ctx, 0, 0);
+ GEMV_N_LIBSTR(nv, ng, 1.0, Ct, 0, 0, qx, nb, 1.0, lv, 0, lv, 0);
+ SYRK_POTRF_LN_LIBSTR(nv, nv, ng, Ctx, 0, 0, Ct, 0, 0, Lv, 0, 0, Lv, 0, 0); // TODO _mn_ routine in BLASFEO !!!
+ }
+ else
+ {
+ POTRF_L_LIBSTR(nv, Lv, 0, 0, Lv, 0, 0);
+ }
+
+ VECCP_LIBSTR(nv, lv, 0, dv, 0);
+ VECSC_LIBSTR(nv, -1.0, dv, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+#else
+// TRCP_L_LIBSTR(nv, Hg, 0, 0, Lv, 0, 0);
+ GECP_LIBSTR(nv, nv, Hg, 0, 0, Lv, 0, 0);
+ ROWIN_LIBSTR(nv, 1.0, res_g, 0, Lv, nv, 0);
+
+ if(nb>0)
+ {
+ DIAAD_SP_LIBSTR(nb, 1.0, Qx, 0, idxb, Lv, 0, 0);
+ ROWAD_SP_LIBSTR(nb, 1.0, qx, 0, idxb, Lv, nv, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMM_R_DIAG_LIBSTR(nv, ng, 1.0, Ct, 0, 0, Qx, nb, 0.0, Ctx, 0, 0, Ctx, 0, 0);
+ ROWIN_LIBSTR(ng, 1.0, qx, nb, Ctx, nv, 0);
+ SYRK_POTRF_LN_LIBSTR(nv+1, nv, ng, Ctx, 0, 0, Ct, 0, 0, Lv, 0, 0, Lv, 0, 0); // TODO _mn_ routine in BLASFEO !!!
+ }
+ else
+ {
+ POTRF_L_MN_LIBSTR(nv+1, nv, Lv, 0, 0, Lv, 0, 0);
+ }
+
+ ROWEX_LIBSTR(nv, -1.0, Lv, nv, 0, dv, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+#endif
+ }
+
+ if(nb>0)
+ {
+ VECEX_SP_LIBSTR(nb, 1.0, idxb, dv, 0, dt_lb, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMV_T_LIBSTR(nv, ng, 1.0, Ct, 0, 0, dv, 0, 0.0, dt_lg, 0, dt_lg, 0);
+ }
+
+ if(nb>0 | ng>0)
+ {
+ COMPUTE_LAM_T_HARD_QP(rws);
+ }
+
+ return;
+
+ }
+
+
+
+// range-space (Schur complement) method
+void SOLVE_KKT_STEP_HARD_DENSE_QP(struct DENSE_QP *qp, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+ struct STRMAT *A = qp->A;
+ struct STRMAT *Ct = qp->Ct;
+ int *idxb = qp->idxb;
+
+ struct STRMAT *Lv = ws->Lv;
+ struct STRMAT *Le = ws->Le;
+ struct STRMAT *Ctx = ws->Ctx;
+ struct STRMAT *AL = ws->AL;
+ struct STRVEC *lv = ws->lv;
+ struct STRVEC *dv = ws->dv;
+ struct STRVEC *dpi = ws->dpi;
+ struct STRVEC *dt_lb = ws->dt_lb;
+ struct STRVEC *dt_lg = ws->dt_lg;
+ struct STRVEC *res_g = ws->res_g;
+ struct STRVEC *res_b = ws->res_b;
+ struct STRVEC *qx = ws->qx;
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *rws = ws->core_workspace;
+
+ if(nb>0 | ng>0)
+ {
+ COMPUTE_QX_HARD_QP(rws);
+ }
+
+ if(ne>0)
+ {
+ VECCP_LIBSTR(nv, res_g, 0, lv, 0);
+
+ if(nb>0)
+ {
+ VECAD_SP_LIBSTR(nb, 1.0, qx, 0, idxb, lv, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMV_N_LIBSTR(nv, ng, 1.0, Ct, 0, 0, qx, nb, 1.0, lv, 0, lv, 0);
+ }
+
+ VECCP_LIBSTR(nv, lv, 0, dv, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, lv, 0, lv, 0);
+
+ GEMV_N_LIBSTR(ne, nv, 1.0, AL, 0, 0, lv, 0, 1.0, res_b, 0, dpi, 0);
+
+ TRSV_LNN_LIBSTR(ne, Le, 0, 0, dpi, 0, dpi, 0);
+ TRSV_LTN_LIBSTR(ne, Le, 0, 0, dpi, 0, dpi, 0);
+
+ GEMV_T_LIBSTR(ne, nv, 1.0, A, 0, 0, dpi, 0, -1.0, dv, 0, dv, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ }
+ else
+ {
+ VECCP_LIBSTR(nv, res_g, 0, lv, 0);
+
+ if(nb>0)
+ {
+ VECAD_SP_LIBSTR(nb, 1.0, qx, 0, idxb, lv, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMV_N_LIBSTR(nv, ng, 1.0, Ct, 0, 0, qx, nb, 1.0, lv, 0, lv, 0);
+ }
+
+ VECCP_LIBSTR(nv, lv, 0, dv, 0);
+ VECSC_LIBSTR(nv, -1.0, dv, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ }
+
+ if(nb>0)
+ {
+ VECEX_SP_LIBSTR(nb, 1.0, idxb, dv, 0, dt_lb, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMV_T_LIBSTR(nv, ng, 1.0, Ct, 0, 0, dv, 0, 0.0, dt_lg, 0, dt_lg, 0);
+ }
+
+ if(nb>0 | ng>0)
+ {
+ COMPUTE_LAM_T_HARD_QP(rws);
+ }
+
+ return;
+
+ }
+
+
diff --git a/dense_qp/x_dense_qp_sol.c b/dense_qp/x_dense_qp_sol.c
new file mode 100644
index 0000000..9529729
--- /dev/null
+++ b/dense_qp/x_dense_qp_sol.c
@@ -0,0 +1,191 @@
+/**************************************************************************************************
+* *
+* 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_DENSE_QP_SOL(int nv, int ne, int nb, int ng)
+ {
+
+ int size = 0;
+
+ size += (10)*sizeof(struct STRVEC); // v pi lam_lb lam_ub lam_lg lam_ug t_lb t_ub t_lg t_ug
+
+ size += 1*SIZE_STRVEC(nv); // ux
+ size += 1*SIZE_STRVEC(ne); // pi
+ size += 8*SIZE_STRVEC(2*nb+2*ng); // 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_DENSE_QP_SOL(int nv, int ne, int nb, int ng, struct DENSE_QP_SOL *qp_sol, void *memory)
+ {
+
+ qp_sol->memsize = MEMSIZE_DENSE_QP_SOL(nv, ne, nb, ng);
+
+
+ // vector struct stuff
+ struct STRVEC *sv_ptr = (struct STRVEC *) memory;
+
+ qp_sol->v = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->pi = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->lam_lb = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->lam_ub = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->lam_lg = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->lam_ug = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->t_lb = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->t_ub = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->t_lg = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->t_ug = sv_ptr;
+ sv_ptr += 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;
+
+ // v
+ CREATE_STRVEC(nv, qp_sol->v, v_ptr);
+ v_ptr += qp_sol->v->memory_size;
+ // pi
+ CREATE_STRVEC(ne, qp_sol->pi, v_ptr);
+ v_ptr += qp_sol->pi->memory_size;
+ // lam
+ tmp_ptr = v_ptr;
+ v_ptr += SIZE_STRVEC(2*nb+2*ng);
+ // lam_lb
+ CREATE_STRVEC(nb, qp_sol->lam_lb, tmp_ptr);
+ tmp_ptr += (nb)*sizeof(REAL);
+ // lam_lg
+ CREATE_STRVEC(ng, qp_sol->lam_lg, tmp_ptr);
+ tmp_ptr += (ng)*sizeof(REAL);
+ // lam_ub
+ CREATE_STRVEC(nb, qp_sol->lam_ub, tmp_ptr);
+ tmp_ptr += (nb)*sizeof(REAL);
+ // lam_ug
+ CREATE_STRVEC(ng, qp_sol->lam_ug, tmp_ptr);
+ tmp_ptr += (ng)*sizeof(REAL);
+ // t_lb
+ CREATE_STRVEC(nb, qp_sol->t_lb, tmp_ptr);
+ tmp_ptr += (nb)*sizeof(REAL);
+ // t_lg
+ CREATE_STRVEC(ng, qp_sol->t_lg, tmp_ptr);
+ tmp_ptr += (ng)*sizeof(REAL);
+ // t_ub
+ CREATE_STRVEC(nb, qp_sol->t_ub, tmp_ptr);
+ tmp_ptr += (nb)*sizeof(REAL);
+ // t_ug
+ CREATE_STRVEC(ng, qp_sol->t_ug, tmp_ptr);
+ tmp_ptr += (ng)*sizeof(REAL);
+
+ return;
+
+ }
+
+
+
+void 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)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_STRVEC2VEC(nv, qp_sol->v, 0, v);
+ CVT_STRVEC2VEC(ne, qp_sol->pi, 0, pi);
+ CVT_STRVEC2VEC(nb, qp_sol->lam_lb, 0, lam_lb);
+ CVT_STRVEC2VEC(nb, qp_sol->lam_ub, 0, lam_ub);
+ CVT_STRVEC2VEC(ng, qp_sol->lam_lg, 0, lam_lg);
+ CVT_STRVEC2VEC(ng, qp_sol->lam_ug, 0, lam_ug);
+
+ return;
+
+ }
+
+
+
+void 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)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_STRVEC2VEC(nv, qp_sol->v, 0, v);
+ CVT_STRVEC2VEC(ne, qp_sol->pi, 0, pi);
+ CVT_STRVEC2VEC(nb, qp_sol->lam_lb, 0, lam_lb);
+ CVT_STRVEC2VEC(nb, qp_sol->lam_ub, 0, lam_ub);
+ CVT_STRVEC2VEC(ng, qp_sol->lam_lg, 0, lam_lg);
+ CVT_STRVEC2VEC(ng, qp_sol->lam_ug, 0, lam_ug);
+
+ return;
+
+ }
+
+
+
+void 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)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ VECCP_LIBSTR(nv, qp_sol->v, 0, v, 0);
+ VECCP_LIBSTR(ne, qp_sol->pi, 0, pi, 0);
+ VECCP_LIBSTR(nb, qp_sol->lam_lb, 0, lam_lb, 0);
+ VECCP_LIBSTR(nb, qp_sol->lam_ub, 0, lam_ub, 0);
+ VECCP_LIBSTR(ng, qp_sol->lam_lg, 0, lam_lg, 0);
+ VECCP_LIBSTR(ng, qp_sol->lam_ug, 0, lam_ug, 0);
+
+ return;
+
+ }