| Austin Schuh | 16ce3c7 | 2018-01-28 16:17:08 -0800 | [diff] [blame^] | 1 | /************************************************************************************************** |
| 2 | * * |
| 3 | * This file is part of HPIPM. * |
| 4 | * * |
| 5 | * HPIPM -- High Performance Interior Point Method. * |
| 6 | * Copyright (C) 2017 by Gianluca Frison. * |
| 7 | * Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * |
| 8 | * All rights reserved. * |
| 9 | * * |
| 10 | * HPMPC is free software; you can redistribute it and/or * |
| 11 | * modify it under the terms of the GNU Lesser General Public * |
| 12 | * License as published by the Free Software Foundation; either * |
| 13 | * version 2.1 of the License, or (at your option) any later version. * |
| 14 | * * |
| 15 | * HPMPC is distributed in the hope that it will be useful, * |
| 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
| 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. * |
| 18 | * See the GNU Lesser General Public License for more details. * |
| 19 | * * |
| 20 | * You should have received a copy of the GNU Lesser General Public * |
| 21 | * License along with HPMPC; if not, write to the Free Software * |
| 22 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA * |
| 23 | * * |
| 24 | * Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * |
| 25 | * * |
| 26 | **************************************************************************************************/ |
| 27 | |
| 28 | |
| 29 | |
| 30 | #if defined(RUNTIME_CHECKS) |
| 31 | #include <stdlib.h> |
| 32 | #endif |
| 33 | |
| 34 | #include <blasfeo_target.h> |
| 35 | #include <blasfeo_common.h> |
| 36 | #include <blasfeo_m_aux.h> |
| 37 | |
| 38 | #include "../include/hpipm_d_ocp_qp.h" |
| 39 | #include "../include/hpipm_s_ocp_qp.h" |
| 40 | |
| 41 | |
| 42 | |
| 43 | void m_cvt_d_ocp_qp_to_s_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp) |
| 44 | { |
| 45 | |
| 46 | // TODO check that they have equal sizes !!!!! |
| 47 | |
| 48 | int N = d_qp->N; |
| 49 | int *nx = d_qp->nx; |
| 50 | int *nu = d_qp->nu; |
| 51 | int *nb = d_qp->nb; |
| 52 | int *ng = d_qp->ng; |
| 53 | |
| 54 | int ii, jj; |
| 55 | |
| 56 | for(ii=0; ii<N; ii++) |
| 57 | { |
| 58 | m_cvt_d2s_strmat(nu[ii]+nx[ii]+1, nx[ii+1], d_qp->BAbt+ii, 0, 0, s_qp->BAbt+ii, 0, 0); |
| 59 | m_cvt_d2s_strmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], d_qp->RSQrq+ii, 0, 0, s_qp->RSQrq+ii, 0, 0); |
| 60 | m_cvt_d2s_strmat(nu[ii]+nx[ii], ng[ii], d_qp->DCt+ii, 0, 0, s_qp->DCt+ii, 0, 0); |
| 61 | m_cvt_d2s_strvec(nx[ii+1], d_qp->b+ii, 0, s_qp->b+ii, 0); |
| 62 | m_cvt_d2s_strvec(nu[ii]+nx[ii], d_qp->rq+ii, 0, s_qp->rq+ii, 0); |
| 63 | m_cvt_d2s_strvec(nb[ii], d_qp->d_lb+ii, 0, s_qp->d_lb+ii, 0); |
| 64 | m_cvt_d2s_strvec(nb[ii], d_qp->d_ub+ii, 0, s_qp->d_ub+ii, 0); |
| 65 | m_cvt_d2s_strvec(ng[ii], d_qp->d_lg+ii, 0, s_qp->d_lg+ii, 0); |
| 66 | m_cvt_d2s_strvec(ng[ii], d_qp->d_ug+ii, 0, s_qp->d_ug+ii, 0); |
| 67 | for(jj=0; jj<nb[ii]; jj++) s_qp->idxb[ii][jj] = d_qp->idxb[ii][jj]; |
| 68 | } |
| 69 | ii = N; |
| 70 | m_cvt_d2s_strmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], d_qp->RSQrq+ii, 0, 0, s_qp->RSQrq+ii, 0, 0); |
| 71 | m_cvt_d2s_strmat(nu[ii]+nx[ii], ng[ii], d_qp->DCt+ii, 0, 0, s_qp->DCt+ii, 0, 0); |
| 72 | m_cvt_d2s_strvec(nu[ii]+nx[ii], d_qp->rq+ii, 0, s_qp->rq+ii, 0); |
| 73 | m_cvt_d2s_strvec(nb[ii], d_qp->d_lb+ii, 0, s_qp->d_lb+ii, 0); |
| 74 | m_cvt_d2s_strvec(nb[ii], d_qp->d_ub+ii, 0, s_qp->d_ub+ii, 0); |
| 75 | m_cvt_d2s_strvec(ng[ii], d_qp->d_lg+ii, 0, s_qp->d_lg+ii, 0); |
| 76 | m_cvt_d2s_strvec(ng[ii], d_qp->d_ug+ii, 0, s_qp->d_ug+ii, 0); |
| 77 | for(jj=0; jj<nb[ii]; jj++) s_qp->idxb[ii][jj] = d_qp->idxb[ii][jj]; |
| 78 | |
| 79 | return; |
| 80 | |
| 81 | } |