From cd7858633a8ba9883704ae72eed55ef3735ea210 Mon Sep 17 00:00:00 2001
From: giaf <gianfri2008@hotmail.it>
Date: Wed, 1 Jun 2022 16:49:13 +0200
Subject: [PATCH] add obj computation dense_{qp,qcqp_}res

---
 dense_qp/x_dense_qcqp_res.c      | 12 ++++++++++--
 dense_qp/x_dense_qp_res.c        | 12 ++++++++++--
 include/hpipm_d_dense_qcqp_res.h |  1 +
 include/hpipm_d_dense_qp_res.h   |  1 +
 include/hpipm_s_dense_qcqp_res.h |  1 +
 include/hpipm_s_dense_qp_res.h   |  1 +
 6 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/dense_qp/x_dense_qcqp_res.c b/dense_qp/x_dense_qcqp_res.c
index 1db4266f..4b7893b5 100644
--- a/dense_qp/x_dense_qcqp_res.c
+++ b/dense_qp/x_dense_qcqp_res.c
@@ -301,9 +301,13 @@ void DENSE_QCQP_RES_COMPUTE(struct DENSE_QCQP *qp, struct DENSE_QCQP_SOL *qp_sol
 	struct STRVEC *tmp_ns = ws->tmp_ns;
 
 	REAL mu, tmp;
+	res->obj = 0.0;
 
 	// res g
-	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 1.0, gz, 0, res_g, 0);
+//	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 1.0, gz, 0, res_g, 0);
+	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 2.0, gz, 0, res_g, 0);
+	res->obj += 0.5*DOT(nv, res_g, 0, v, 0);
+	AXPY(nv, -1.0, gz, 0, res_g, 0, res_g, 0);
 
 	if(nb+ng+nq>0)
 		{
@@ -360,7 +364,11 @@ void DENSE_QCQP_RES_COMPUTE(struct DENSE_QCQP *qp, struct DENSE_QCQP_SOL *qp_sol
 	if(ns>0)
 		{
 		// res_g
-		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 1.0, gz, nv, res_g, nv);
+//		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 1.0, gz, nv, res_g, nv);
+		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 2.0, gz, nv, res_g, nv);
+		res->obj += 0.5*DOT(2*ns, res_g, nv, v, nv);
+		AXPY(2*ns, -1.0, gz, nv, res_g, nv, res_g, nv);
+
 		AXPY(2*ns, -1.0, lam, 2*nb+2*ng+2*nq, res_g, nv, res_g, nv);
 		for(ii=0; ii<nb+ng+nq; ii++)
 			{
diff --git a/dense_qp/x_dense_qp_res.c b/dense_qp/x_dense_qp_res.c
index 4e2a463a..ea60a0b6 100644
--- a/dense_qp/x_dense_qp_res.c
+++ b/dense_qp/x_dense_qp_res.c
@@ -295,9 +295,13 @@ void DENSE_QP_RES_COMPUTE(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, stru
 	struct STRVEC *tmp_ns = ws->tmp_ns;
 
 	REAL mu, tmp;
+	res->obj = 0.0;
 
 	// res g
-	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 1.0, gz, 0, res_g, 0);
+//	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 1.0, gz, 0, res_g, 0);
+	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 2.0, gz, 0, res_g, 0);
+	res->obj += 0.5*DOT(nv, res_g, 0, v, 0);
+	AXPY(nv, -1.0, gz, 0, res_g, 0, res_g, 0);
 
 	if(nb+ng>0)
 		{
@@ -322,7 +326,11 @@ void DENSE_QP_RES_COMPUTE(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, stru
 	if(ns>0)
 		{
 		// res_g
-		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 1.0, gz, nv, res_g, nv);
+//		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 1.0, gz, nv, res_g, nv);
+		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 2.0, gz, nv, res_g, nv);
+		res->obj += 0.5*DOT(2*ns, res_g, nv, v, nv);
+		AXPY(2*ns, -1.0, gz, nv, res_g, nv, res_g, nv);
+
 		AXPY(2*ns, -1.0, lam, 2*nb+2*ng, res_g, nv, res_g, nv);
 		for(ii=0; ii<nb+ng; ii++)
 			{
diff --git a/include/hpipm_d_dense_qcqp_res.h b/include/hpipm_d_dense_qcqp_res.h
index a76f1621..d20b6947 100644
--- a/include/hpipm_d_dense_qcqp_res.h
+++ b/include/hpipm_d_dense_qcqp_res.h
@@ -62,6 +62,7 @@ struct d_dense_qcqp_res
 	struct blasfeo_dvec *res_m; // m-residuals
 	double res_max[4]; // infinity norm of residuals
 	double res_mu; // mu-residual
+	double obj; // (primal) objective
 	hpipm_size_t memsize;
 	};
 
diff --git a/include/hpipm_d_dense_qp_res.h b/include/hpipm_d_dense_qp_res.h
index 7c202325..5635441e 100644
--- a/include/hpipm_d_dense_qp_res.h
+++ b/include/hpipm_d_dense_qp_res.h
@@ -62,6 +62,7 @@ struct d_dense_qp_res
 	struct blasfeo_dvec *res_m; // m-residuals
 	double res_max[4]; // max of residuals
 	double res_mu; // mu-residual
+	double obj; // (primal) objective
 	hpipm_size_t memsize;
 	};
 
diff --git a/include/hpipm_s_dense_qcqp_res.h b/include/hpipm_s_dense_qcqp_res.h
index 779658cc..1cfaba97 100644
--- a/include/hpipm_s_dense_qcqp_res.h
+++ b/include/hpipm_s_dense_qcqp_res.h
@@ -62,6 +62,7 @@ struct s_dense_qcqp_res
 	struct blasfeo_svec *res_m; // m-residuals
 	float res_max[4]; // infinity norm of residuals
 	float res_mu; // mu-residual
+	float obj; // (primal) objective
 	hpipm_size_t memsize;
 	};
 
diff --git a/include/hpipm_s_dense_qp_res.h b/include/hpipm_s_dense_qp_res.h
index 06b609c5..2094fd01 100644
--- a/include/hpipm_s_dense_qp_res.h
+++ b/include/hpipm_s_dense_qp_res.h
@@ -62,6 +62,7 @@ struct s_dense_qp_res
 	struct blasfeo_svec *res_m; // m-residuals
 	float res_max[4]; // max of residuals
 	float res_mu; // mu-residual
+	float obj; // (primal) objective
 	hpipm_size_t memsize;
 	};