diff --git a/include/types.h b/include/types.h index 68d6602..2ba5056 100644 --- a/include/types.h +++ b/include/types.h @@ -29,6 +29,9 @@ typedef struct{ // sense define the state of the constraints // (active, immutable, upper/lower, soft, binary). + // if is_factored != 0, field H contains the Cholesky factor R, where H=R'R + // H has dimension n*n if !is_factored, n*(n+1)/2 if is_factored + int n; int m; int ms; @@ -42,6 +45,8 @@ typedef struct{ int* sense; + int is_factored; + }DAQPProblem; typedef struct{ diff --git a/src/utils.c b/src/utils.c index 78ae0c6..3d8ade4 100644 --- a/src/utils.c +++ b/src/utils.c @@ -87,9 +87,10 @@ int update_ldp(const int mask, DAQPWorkspace *work){ int update_Rinv(DAQPWorkspace *work){ int i,j,k,disp,disp2,disp3; const int n = NX; + int is_factored = work->qp->is_factored; // Check if diagonal int is_diagonal = 1; - for (i=0,disp=1; iqp->H[disp] > 1e-12 || work->qp->H[disp] < -1e-12){ is_diagonal=0; @@ -107,21 +108,41 @@ int update_Rinv(DAQPWorkspace *work){ } c_float Hi; i=0; disp=0; - if(work->scaling != NULL){ - for(;iqp->H[disp++]+work->settings->eps_prox; + if (is_factored) { + if(work->scaling != NULL){ + for(;iqp->H[disp]; + if (work->settings->eps_prox != 0.) { + Hi = sqrt(Hi*Hi+work->settings->eps_prox); + } + work->RinvD[i] = 1/Hi; + work->scaling[i] = Hi; + } + } + for(;iqp->H[disp]; + if (work->settings->eps_prox != 0.) { + Hi = sqrt(Hi*Hi+work->settings->eps_prox); + } + work->RinvD[i] = 1/Hi; + } + } else { + if(work->scaling != NULL){ + for(;iqp->H[disp++]+work->settings->eps_prox; + if (Hi <= 0) return EXIT_NONCONVEX; + Hi = sqrt(Hi); + work->RinvD[i] = 1/Hi; + work->scaling[i] = Hi; + } + } + for(;iqp->H[disp++] + work->settings->eps_prox; if (Hi <= 0) return EXIT_NONCONVEX; Hi = sqrt(Hi); work->RinvD[i] = 1/Hi; - work->scaling[i] = Hi; } } - for(;iqp->H[disp++] + work->settings->eps_prox; - if (Hi <= 0) return EXIT_NONCONVEX; - Hi = sqrt(Hi); - work->RinvD[i] = 1/Hi; - } return 1; } // Make sure Rinv can be assinged if not diagonal @@ -131,27 +152,40 @@ int update_Rinv(DAQPWorkspace *work){ work->RinvD = NULL; } + if (is_factored) { + for(i=0,disp=0;iRinv[disp] = 1/work->qp->H[disp]; + disp++; + // Off-diagonal elements + for (j=i+1; jRinv[disp] = work->qp->H[disp]; + } + } + } else { + // Cholesky + for (i=0,disp=0,disp3=0; iRinv[disp] = work->qp->H[disp3++]+work->settings->eps_prox;// Add regularization + for (k=0,disp2=i; kRinv[disp] -= work->Rinv[disp2]*work->Rinv[disp2]; + if (work->Rinv[disp] <= 0) return EXIT_NONCONVEX; // Not positive definite - // Cholesky - for (i=0,disp=0,disp3=0; iRinv[disp] = work->qp->H[disp3++]+work->settings->eps_prox;// Add regularization - for (k=0,disp2=i; kRinv[disp] -= work->Rinv[disp2]*work->Rinv[disp2]; - if (work->Rinv[disp] <= 0) return EXIT_NONCONVEX; // Not positive definite - - work->Rinv[disp] = sqrt(work->Rinv[disp]); + work->Rinv[disp] = sqrt(work->Rinv[disp]); - // Off-diagonal elements - for (j=1; jRinv[disp+j]=work->qp->H[disp3++]; - for (k=0,disp2=i; kRinv[disp+j] -= work->Rinv[disp2]*work->Rinv[disp2+j]; - work->Rinv[disp+j] /= work->Rinv[disp]; + // Off-diagonal elements + for (j=1; jRinv[disp+j]=work->qp->H[disp3++]; + for (k=0,disp2=i; kRinv[disp+j] -= work->Rinv[disp2]*work->Rinv[disp2+j]; + work->Rinv[disp+j] /= work->Rinv[disp]; + } + // Store 1/r_ii instead of r_ii + // to get multiplication instead division when forward/backward substituting + work->Rinv[disp] = 1/work->Rinv[disp]; } - // Store 1/r_ii instead of r_ii - // to get multiplication instead division when forward/backward substituting - work->Rinv[disp] = 1/work->Rinv[disp]; } // Compute Rinv (store in R) by Rinv = R\I