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.c b/ocp_qp/x_ocp_qp.c
new file mode 100644
index 0000000..bc5605a
--- /dev/null
+++ b/ocp_qp/x_ocp_qp.c
@@ -0,0 +1,448 @@
+/**************************************************************************************************
+* *
+* 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(int N, int *nx, int *nu, int *nb, int *ng)
+ {
+
+ int ii;
+
+ int size = 0;
+
+ size += 4*(N+1)*sizeof(int); // nx nu nb ng
+ size += (N+1)*sizeof(int *); // idxb
+ size += (1*N+2*(N+1))*sizeof(struct STRMAT); // BAbt ; RSqrq DCt
+ size += (1*N+5*(N+1))*sizeof(struct STRVEC); // b ; rq d_lb d_ub d_lg d_ug
+
+ for(ii=0; ii<N; ii++)
+ {
+ size += nb[ii]*sizeof(int); // idxb
+ size += SIZE_STRMAT(nu[ii]+nx[ii]+1, nx[ii+1]); // BAbt
+ size += SIZE_STRVEC(nx[ii+1]); // b
+ size += SIZE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // RSQrq
+ size += SIZE_STRVEC(nu[ii]+nx[ii]); // rq
+ size += SIZE_STRMAT(nu[ii]+nx[ii], ng[ii]); // DCt
+ size += 2*SIZE_STRVEC(nb[ii]); // d_lb d_ub
+ size += 2*SIZE_STRVEC(ng[ii]); // d_lg d_ug
+ }
+ ii = N;
+ size += nb[ii]*sizeof(int); // idxb
+ size += SIZE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // RSQrq
+ size += SIZE_STRVEC(nu[ii]+nx[ii]); // rq
+ size += SIZE_STRMAT(nu[ii]+nx[ii], ng[ii]); // DCt
+ size += 2*SIZE_STRVEC(nb[ii]); // d_lb d_ub
+ size += 2*SIZE_STRVEC(ng[ii]); // d_lg d_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(int N, int *nx, int *nu, int *nb, int *ng, struct OCP_QP *qp, void *memory)
+ {
+
+ int ii;
+
+
+ // memsize
+ qp->memsize = MEMSIZE_OCP_QP(N, nx, nu, nb, ng);
+
+
+ // horizon length
+ qp->N = N;
+
+
+ // int pointer stuff
+ int **ip_ptr;
+ ip_ptr = (int **) memory;
+
+ // idxb
+ qp->idxb = ip_ptr;
+ ip_ptr += N+1;
+
+
+ // matrix struct stuff
+ struct STRMAT *sm_ptr = (struct STRMAT *) ip_ptr;
+
+ // BAbt
+ qp->BAbt = sm_ptr;
+ sm_ptr += N;
+
+ // RSQrq
+ qp->RSQrq = sm_ptr;
+ sm_ptr += N+1;
+
+ // DCt
+ qp->DCt = sm_ptr;
+ sm_ptr += N+1;
+
+
+ // vector struct stuff
+ struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr;
+
+ // b
+ qp->b = sv_ptr;
+ sv_ptr += N;
+
+ // rq
+ qp->rq = sv_ptr;
+ sv_ptr += N+1;
+
+ // d_lb
+ qp->d_lb = sv_ptr;
+ sv_ptr += N+1;
+
+ // d_ub
+ qp->d_ub = sv_ptr;
+ sv_ptr += N+1;
+
+ // d_lg
+ qp->d_lg = sv_ptr;
+ sv_ptr += N+1;
+
+ // d_ug
+ qp->d_ug = sv_ptr;
+ sv_ptr += N+1;
+
+
+ // integer stuff
+ int *i_ptr;
+ i_ptr = (int *) sv_ptr;
+
+ // nx
+ qp->nx = i_ptr;
+ for(ii=0; ii<=N; ii++)
+ {
+ i_ptr[ii] = nx[ii];
+ }
+ i_ptr += N+1;
+
+ // nu
+ qp->nu = i_ptr;
+ for(ii=0; ii<=N; ii++)
+ {
+ i_ptr[ii] = nu[ii];
+ }
+ i_ptr += N+1;
+
+ // nb
+ qp->nb = i_ptr;
+ for(ii=0; ii<=N; ii++)
+ {
+ i_ptr[ii] = nb[ii];
+ }
+ i_ptr += N+1;
+
+ // ng
+ qp->ng = i_ptr;
+ for(ii=0; ii<=N; ii++)
+ {
+ i_ptr[ii] = ng[ii];
+ }
+ i_ptr += N+1;
+
+ // idxb
+ for(ii=0; ii<=N; ii++)
+ {
+ (qp->idxb)[ii] = i_ptr;
+ i_ptr += nb[ii];
+ }
+
+
+ // align to typical cache line size
+ long long l_ptr = (long long) i_ptr;
+ l_ptr = (l_ptr+63)/64*64;
+
+
+ // double stuff
+ char *c_ptr;
+ c_ptr = (char *) l_ptr;
+
+ // BAbt
+ for(ii=0; ii<N; ii++)
+ {
+ CREATE_STRMAT(nu[ii]+nx[ii]+1, nx[ii+1], qp->BAbt+ii, c_ptr);
+ c_ptr += (qp->BAbt+ii)->memory_size;
+ }
+
+ // RSQrq
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], qp->RSQrq+ii, c_ptr);
+ c_ptr += (qp->RSQrq+ii)->memory_size;
+ }
+
+ // DCt
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRMAT(nu[ii]+nx[ii], ng[ii], qp->DCt+ii, c_ptr);
+ c_ptr += (qp->DCt+ii)->memory_size;
+ }
+
+ // b
+ for(ii=0; ii<N; ii++)
+ {
+ CREATE_STRVEC(nx[ii+1], qp->b+ii, c_ptr);
+ c_ptr += (qp->b+ii)->memory_size;
+ }
+
+ // rq
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nu[ii]+nx[ii], qp->rq+ii, c_ptr);
+ c_ptr += (qp->rq+ii)->memory_size;
+ }
+
+ // d_lb
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp->d_lb+ii, c_ptr);
+ c_ptr += (qp->d_lb+ii)->memory_size;
+ }
+
+ // d_ub
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp->d_ub+ii, c_ptr);
+ c_ptr += (qp->d_ub+ii)->memory_size;
+ }
+
+ // d_lg
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp->d_lg+ii, c_ptr);
+ c_ptr += (qp->d_lg+ii)->memory_size;
+ }
+
+ // d_ug
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp->d_ug+ii, c_ptr);
+ c_ptr += (qp->d_ug+ii)->memory_size;
+ }
+
+ return;
+
+ }
+
+
+
+void CAST_OCP_QP(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, struct STRMAT *BAbt, struct STRVEC *b, struct STRMAT *RSQrq, struct STRVEC *rq, struct STRMAT *DCt, struct STRVEC *d_lb, struct STRVEC *d_ub, struct STRVEC *d_lg, struct STRVEC *d_ug, struct OCP_QP *qp)
+ {
+
+ qp->N = N;
+ qp->nx = nx;
+ qp->nu = nu;
+ qp->nb = nb;
+ qp->idxb = idxb;
+ qp->ng = ng;
+ qp->BAbt = BAbt;
+ qp->b = b;
+ qp->RSQrq = RSQrq;
+ qp->rq = rq;
+ qp->DCt = DCt;
+ qp->d_lb = d_lb;
+ qp->d_ub = d_ub;
+ qp->d_lg = d_lg;
+ qp->d_ug = d_ug;
+
+ return;
+
+ }
+
+
+
+void CVT_COLMAJ_TO_OCP_QP(REAL **A, REAL **B, REAL **b, REAL **Q, REAL **S, REAL **R, REAL **q, REAL **r, int **idxb, REAL **d_lb, REAL **d_ub, REAL **C, REAL **D, REAL **d_lg, REAL **d_ug, struct OCP_QP *qp)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int ii, jj;
+
+ for(ii=0; ii<N; ii++)
+ {
+ CVT_TRAN_MAT2STRMAT(nx[ii+1], nu[ii], B[ii], nx[ii+1], qp->BAbt+ii, 0, 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii+1], nx[ii], A[ii], nx[ii+1], qp->BAbt+ii, nu[ii], 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii+1], 1, b[ii], nx[ii+1], qp->BAbt+ii, nu[ii]+nx[ii], 0);
+ CVT_VEC2STRVEC(nx[ii+1], b[ii], qp->b+ii, 0);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ CVT_MAT2STRMAT(nu[ii], nu[ii], R[ii], nu[ii], qp->RSQrq+ii, 0, 0);
+ CVT_TRAN_MAT2STRMAT(nu[ii], nx[ii], S[ii], nu[ii], qp->RSQrq+ii, nu[ii], 0);
+ CVT_MAT2STRMAT(nx[ii], nx[ii], Q[ii], nx[ii], qp->RSQrq+ii, nu[ii], nu[ii]);
+ CVT_TRAN_MAT2STRMAT(nu[ii], 1, r[ii], nu[ii], qp->RSQrq+ii, nu[ii]+nx[ii], 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii], 1, q[ii], nx[ii], qp->RSQrq+ii, nu[ii]+nx[ii], nu[ii]);
+ CVT_VEC2STRVEC(nu[ii], r[ii], qp->rq+ii, 0);
+ CVT_VEC2STRVEC(nx[ii], q[ii], qp->rq+ii, nu[ii]);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ for(jj=0; jj<nb[ii]; jj++)
+ qp->idxb[ii][jj] = idxb[ii][jj];
+ CVT_VEC2STRVEC(nb[ii], d_lb[ii], qp->d_lb+ii, 0);
+ CVT_VEC2STRVEC(nb[ii], d_ub[ii], qp->d_ub+ii, 0);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ CVT_TRAN_MAT2STRMAT(ng[ii], nu[ii], D[ii], ng[ii], qp->DCt+ii, 0, 0);
+ CVT_TRAN_MAT2STRMAT(ng[ii], nx[ii], C[ii], ng[ii], qp->DCt+ii, nu[ii], 0);
+ CVT_VEC2STRVEC(ng[ii], d_lg[ii], qp->d_lg+ii, 0);
+ CVT_VEC2STRVEC(ng[ii], d_ug[ii], qp->d_ug+ii, 0);
+ }
+
+ return;
+
+ }
+
+
+
+void CVT_ROWMAJ_TO_OCP_QP(REAL **A, REAL **B, REAL **b, REAL **Q, REAL **S, REAL **R, REAL **q, REAL **r, int **idxb, REAL **d_lb, REAL **d_ub, REAL **C, REAL **D, REAL **d_lg, REAL **d_ug, struct OCP_QP *qp)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int ii, jj;
+
+ for(ii=0; ii<N; ii++)
+ {
+ CVT_MAT2STRMAT(nu[ii], nx[ii+1], B[ii], nu[ii], qp->BAbt+ii, 0, 0);
+ CVT_MAT2STRMAT(nx[ii], nx[ii+1], A[ii], nx[ii], qp->BAbt+ii, nu[ii], 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii+1], 1, b[ii], nx[ii+1], qp->BAbt+ii, nu[ii]+nx[ii], 0);
+ CVT_VEC2STRVEC(nx[ii+1], b[ii], qp->b+ii, 0);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ CVT_TRAN_MAT2STRMAT(nu[ii], nu[ii], R[ii], nu[ii], qp->RSQrq+ii, 0, 0);
+ CVT_MAT2STRMAT(nx[ii], nu[ii], S[ii], nx[ii], qp->RSQrq+ii, nu[ii], 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii], nx[ii], Q[ii], nx[ii], qp->RSQrq+ii, nu[ii], nu[ii]);
+ CVT_TRAN_MAT2STRMAT(nu[ii], 1, r[ii], nu[ii], qp->RSQrq+ii, nu[ii]+nx[ii], 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii], 1, q[ii], nx[ii], qp->RSQrq+ii, nu[ii]+nx[ii], nu[ii]);
+ CVT_VEC2STRVEC(nu[ii], r[ii], qp->rq+ii, 0);
+ CVT_VEC2STRVEC(nx[ii], q[ii], qp->rq+ii, nu[ii]);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ for(jj=0; jj<nb[ii]; jj++)
+ qp->idxb[ii][jj] = idxb[ii][jj];
+ CVT_VEC2STRVEC(nb[ii], d_lb[ii], qp->d_lb+ii, 0);
+ CVT_VEC2STRVEC(nb[ii], d_ub[ii], qp->d_ub+ii, 0);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ CVT_MAT2STRMAT(nu[ii], ng[ii], D[ii], nu[ii], qp->DCt+ii, 0, 0);
+ CVT_MAT2STRMAT(nx[ii], ng[ii], C[ii], nx[ii], qp->DCt+ii, nu[ii], 0);
+ CVT_VEC2STRVEC(ng[ii], d_lg[ii], qp->d_lg+ii, 0);
+ CVT_VEC2STRVEC(ng[ii], d_ug[ii], qp->d_ug+ii, 0);
+ }
+
+ return;
+
+ }
+
+
+
+void COPY_OCP_QP(struct OCP_QP *qp_in, struct OCP_QP *qp_out)
+ {
+
+ int N = qp_in->N;
+ int *nx = qp_in->nx;
+ int *nu = qp_in->nu;
+ int *nb = qp_in->nb;
+ int *ng = qp_in->ng;
+
+ int ii, jj;
+
+#if defined(RUNTIME_CHECKS)
+ if(qp_out->N != qp_in->N)
+ {
+ printf("\nError : x_copy_ocp_qp : qp_out->N != qp_out->N : %d != %d\n\n", qp_out->N, qp_in->N);
+ exit(1);
+ }
+ for(ii=0; ii<=N; ii++)
+ {
+ if(qp_out->nx[ii] != qp_in->nx[ii])
+ {
+ printf("\nError : x_copy_ocp_qp : qp_out->nx[%d] != qp_out->nx[%d] : %d != %d\n\n", ii, ii, qp_out->nx[ii], qp_in->nx[ii]);
+ exit(1);
+ }
+ if(qp_out->nu[ii] != qp_in->nu[ii])
+ {
+ printf("\nError : x_copy_ocp_qp : qp_out->nu[%d] != qp_out->nu[%d] : %d != %d\n\n", ii, ii, qp_out->nu[ii], qp_in->nu[ii]);
+ exit(1);
+ }
+ if(qp_out->nb[ii] != qp_in->nb[ii])
+ {
+ printf("\nError : x_copy_ocp_qp : qp_out->nb[%d] != qp_out->nb[%d] : %d != %d\n\n", ii, ii, qp_out->nb[ii], qp_in->nb[ii]);
+ exit(1);
+ }
+ if(qp_out->ng[ii] != qp_in->ng[ii])
+ {
+ printf("\nError : x_copy_ocp_qp : qp_out->ng[%d] != qp_out->ng[%d] : %d != %d\n\n", ii, ii, qp_out->ng[ii], qp_in->ng[ii]);
+ exit(1);
+ }
+ }
+#endif
+
+ for(ii=0; ii<N; ii++)
+ {
+ for(jj=0; jj<qp_in->nb[ii]; jj++) qp_out->idxb[ii][jj] = qp_in->idxb[ii][jj];
+ GECP_LIBSTR(nx[ii]+nu[ii]+1, nx[ii+1], qp_in->BAbt+ii, 0, 0, qp_out->BAbt+ii, 0, 0);
+ VECCP_LIBSTR(nx[ii+1], qp_in->b+ii, 0, qp_out->b+ii, 0);
+ GECP_LIBSTR(nx[ii]+nu[ii]+1, nu[ii]+nx[ii], qp_in->RSQrq+ii, 0, 0, qp_out->RSQrq+ii, 0, 0);
+ VECCP_LIBSTR(nu[ii]+nx[ii], qp_in->rq+ii, 0, qp_out->rq+ii, 0);
+ GECP_LIBSTR(nx[ii]+nu[ii], ng[ii], qp_in->DCt+ii, 0, 0, qp_out->DCt+ii, 0, 0);
+ VECCP_LIBSTR(nb[ii], qp_in->d_lb+ii, 0, qp_out->d_lb+ii, 0);
+ VECCP_LIBSTR(nb[ii], qp_in->d_ub+ii, 0, qp_out->d_ub+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_in->d_lg+ii, 0, qp_out->d_lg+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_in->d_ug+ii, 0, qp_out->d_ug+ii, 0);
+ }
+
+ return;
+
+ }
+
+