Merge branch 'master' of https://github.com/giaf/hpipm into stable

FucaiChen · Oct 19, 2017 · 87e9982 · 87e9982
2 parents 670046b + d6c196f
commit 87e9982
Show file tree

Hide file tree

Showing 51 changed files with 1,401 additions and 393 deletions.
diff --git a/cond/d_cond.c b/cond/d_cond.c
@@ -45,7 +45,10 @@
 
 
 #define COND_DCTD d_cond_DCtd
+#define COND_D d_cond_d
+#define COND_B d_cond_b
 #define COND_BABT d_cond_BAbt
+#define COND_RQ_N2NX3 d_cond_rq_N2nx3
 #define COND_RSQRQ_N2NX3 d_cond_RSQrq_N2nx3
 #define COND_QP_OCP2DENSE_WORKSPACE d_cond_qp_ocp2dense_workspace
 #define CREATE_STRMAT d_create_strmat
@@ -64,6 +67,7 @@
 #define MEMSIZE_COND_QP_OCP2DENSE d_memsize_cond_qp_ocp2dense
 #define CREATE_COND_QP_OCP2DENSE d_create_cond_qp_ocp2dense
 #define COND_QP_OCP2DENSE d_cond_qp_ocp2dense
+#define COND_RHS_QP_OCP2DENSE d_cond_rhs_qp_ocp2dense
 #define EXPAND_SOL_DENSE2OCP d_expand_sol_dense2ocp
 
 

diff --git a/cond/d_cond_aux.c b/cond/d_cond_aux.c
@@ -65,10 +65,12 @@
 #define TRMM_RLNN_LIBSTR dtrmm_rlnn_libstr
 #define VECCP_LIBSTR dveccp_libstr
 
-#define COMPUTE_GAMMA d_compute_Gamma
 #define COND_BABT d_cond_BAbt
+#define COND_B d_cond_b
 #define COND_RSQRQ_N2NX3 d_cond_RSQrq_N2nx3
+#define COND_RQ_N2NX3 d_cond_rq_N2nx3
 #define COND_DCTD d_cond_DCtd
+#define COND_D d_cond_d
 #define EXPAND_SOL d_expand_sol
 
 

diff --git a/cond/d_part_cond.c b/cond/d_part_cond.c
@@ -45,8 +45,11 @@
 
 
 
+#define COND_B d_cond_b
 #define COND_BABT d_cond_BAbt
+#define COND_D d_cond_d
 #define COND_DCTD d_cond_DCtd
+#define COND_RQ_N2NX3 d_cond_rq_N2nx3
 #define COND_RSQRQ_N2NX3 d_cond_RSQrq_N2nx3
 #define COND_QP_OCP2DENSE_WORKSPACE d_cond_qp_ocp2dense_workspace
 #define COND_QP_OCP2OCP_WORKSPACE d_cond_qp_ocp2ocp_workspace
@@ -66,6 +69,7 @@
 #define MEMSIZE_COND_QP_OCP2OCP d_memsize_cond_qp_ocp2ocp
 #define CREATE_COND_QP_OCP2OCP d_create_cond_qp_ocp2ocp
 #define COND_QP_OCP2OCP d_cond_qp_ocp2ocp
+#define COND_RHS_QP_OCP2OCP d_cond_rhs_qp_ocp2ocp
 #define EXPAND_SOL_OCP2OCP d_expand_sol_ocp2ocp
 
 

diff --git a/cond/s_cond.c b/cond/s_cond.c
@@ -45,7 +45,10 @@
 
 
 #define COND_DCTD s_cond_DCtd
+#define COND_D s_cond_d
+#define COND_B s_cond_b
 #define COND_BABT s_cond_BAbt
+#define COND_RQ_N2NX3 s_cond_rq_N2nx3
 #define COND_RSQRQ_N2NX3 s_cond_RSQrq_N2nx3
 #define COND_QP_OCP2DENSE_WORKSPACE s_cond_qp_ocp2dense_workspace
 #define CREATE_STRMAT s_create_strmat
@@ -64,6 +67,7 @@
 #define MEMSIZE_COND_QP_OCP2DENSE s_memsize_cond_qp_ocp2dense
 #define CREATE_COND_QP_OCP2DENSE s_create_cond_qp_ocp2dense
 #define COND_QP_OCP2DENSE s_cond_qp_ocp2dense
+#define COND_RHS_QP_OCP2DENSE s_cond_rhs_qp_ocp2dense
 #define EXPAND_SOL_DENSE2OCP s_expand_sol_dense2ocp
 
 

diff --git a/cond/s_cond_aux.c b/cond/s_cond_aux.c
@@ -65,10 +65,12 @@
 #define TRMM_RLNN_LIBSTR strmm_rlnn_libstr
 #define VECCP_LIBSTR sveccp_libstr
 
-#define COMPUTE_GAMMA s_compute_Gamma
 #define COND_BABT s_cond_BAbt
+#define COND_B s_cond_b
 #define COND_RSQRQ_N2NX3 s_cond_RSQrq_N2nx3
+#define COND_RQ_N2NX3 s_cond_rq_N2nx3
 #define COND_DCTD s_cond_DCtd
+#define COND_D s_cond_d
 #define EXPAND_SOL s_expand_sol
 
 

diff --git a/cond/s_part_cond.c b/cond/s_part_cond.c
@@ -45,8 +45,11 @@
 
 
 
+#define COND_B s_cond_b
 #define COND_BABT s_cond_BAbt
+#define COND_D s_cond_d
 #define COND_DCTD s_cond_DCtd
+#define COND_RQ_N2NX3 s_cond_rq_N2nx3
 #define COND_RSQRQ_N2NX3 s_cond_RSQrq_N2nx3
 #define COND_QP_OCP2DENSE_WORKSPACE s_cond_qp_ocp2dense_workspace
 #define COND_QP_OCP2OCP_WORKSPACE s_cond_qp_ocp2ocp_workspace
@@ -66,6 +69,7 @@
 #define MEMSIZE_COND_QP_OCP2OCP s_memsize_cond_qp_ocp2ocp
 #define CREATE_COND_QP_OCP2OCP s_create_cond_qp_ocp2ocp
 #define COND_QP_OCP2OCP s_cond_qp_ocp2ocp
+#define COND_RHS_QP_OCP2OCP s_cond_rhs_qp_ocp2ocp
 #define EXPAND_SOL_OCP2OCP s_expand_sol_ocp2ocp
 
 

diff --git a/cond/x_cond.c b/cond/x_cond.c
@@ -21,7 +21,7 @@
 * License along with HPIPM; 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                             *                          
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
 *                                                                                                 *
 **************************************************************************************************/
 
@@ -109,8 +109,10 @@ int MEMSIZE_COND_QP_OCP2DENSE(struct OCP_QP *ocp_qp, struct DENSE_QP *dense_qp)
 
 	int size = 0;
 
-	size += (2+2*(N+1))*sizeof(struct STRMAT); // Gamma L Lx AL
-	size += (2+1*(N+1))*sizeof(struct STRVEC); // Gammab tmp_ngM tmp_nuxM
+	size += 2*(N+1)*sizeof(struct STRMAT); // Gamma L
+	size += 2*sizeof(struct STRMAT); // Lx AL
+	size += 2*(N+1)*sizeof(struct STRVEC); // Gammab l
+	size += 2*sizeof(struct STRVEC); // tmp_ngM tmp_nuxM
 
 	int nu_tmp = 0;
 	for(ii=0; ii<N; ii++)
@@ -124,6 +126,8 @@ int MEMSIZE_COND_QP_OCP2DENSE(struct OCP_QP *ocp_qp, struct DENSE_QP *dense_qp)
 	size += SIZE_STRMAT(nuM+nxM+1, nxM); // AL
 	for(ii=0; ii<N; ii++) 
 		size += 1*SIZE_STRVEC(nx[ii+1]); // Gammab
+	for(ii=0; ii<=N; ii++) 
+		size += SIZE_STRVEC(nu[ii]+nx[ii]); // l
 	size += SIZE_STRVEC(ngM); // tmp_ngM
 	size += 1*SIZE_STRVEC(nuM+nxM); // tmp_nuxM
 	size += 1*SIZE_STRVEC(ngM); // tmp_ngM
@@ -199,6 +203,8 @@ void CREATE_COND_QP_OCP2DENSE(struct OCP_QP *ocp_qp, struct DENSE_QP *dense_qp,
 
 	cond_ws->Gammab = sv_ptr;
 	sv_ptr += N+1;
+	cond_ws->l = sv_ptr;
+	sv_ptr += N+1;
 	cond_ws->tmp_ngM = sv_ptr;
 	sv_ptr += 1;
 	cond_ws->tmp_nuxM = sv_ptr;
@@ -244,6 +250,11 @@ void CREATE_COND_QP_OCP2DENSE(struct OCP_QP *ocp_qp, struct DENSE_QP *dense_qp,
 		CREATE_STRVEC(nx[ii+1], cond_ws->Gammab+ii, c_ptr);
 		c_ptr += (cond_ws->Gammab+ii)->memory_size;
 		}
+	for(ii=0; ii<=N; ii++)
+		{
+		CREATE_STRVEC(nu[ii]+nx[ii], cond_ws->l+ii, c_ptr);
+		c_ptr += (cond_ws->l+ii)->memory_size;
+		}
 	CREATE_STRVEC(ngM, cond_ws->tmp_ngM, c_ptr);
 	c_ptr += cond_ws->tmp_ngM->memory_size;
 	c_tmp = c_ptr;
@@ -283,6 +294,21 @@ void COND_QP_OCP2DENSE(struct OCP_QP *ocp_qp, struct DENSE_QP *dense_qp, struct
 
 
 
+void COND_RHS_QP_OCP2DENSE(struct OCP_QP *ocp_qp, struct DENSE_QP *dense_qp, struct COND_QP_OCP2DENSE_WORKSPACE *cond_ws)
+	{
+
+	COND_B(ocp_qp, NULL, cond_ws);
+
+	COND_RQ_N2NX3(ocp_qp, dense_qp->g, cond_ws);
+
+	COND_D(ocp_qp, dense_qp->d, dense_qp->z, cond_ws);
+
+	return;
+
+	}
+
+
+
 void EXPAND_SOL_DENSE2OCP(struct OCP_QP *ocp_qp, struct DENSE_QP_SOL *dense_qp_sol, struct OCP_QP_SOL *ocp_qp_sol, struct COND_QP_OCP2DENSE_WORKSPACE *cond_ws)
 	{