//Add SDP Constraint
int addSDPCone(SDPCone sdpcone, const mxArray *cone, int block, double *X)
{
int i;
double *SDP_pr = mxGetPr(cone);
mwIndex *SDP_ir = mxGetIr(cone);
mwIndex *SDP_jc = mxGetJc(cone);
int *iSDP_ir = NULL;
int SDP_M = (int)mxGetM(cone);
int SDP_N = (int)mxGetN(cone); //remember [C A0 A1 A2...] so non-square
int SDP_C_nnz = 0; //nnz in C
int SDP_A_nnz = 0; //nnz in current A
int SDP_A_index = 0; //start index of pr and ir within current A
int SDP_DIM = (int)(sqrt(2*(double)SDP_M + 0.25) - 0.5); //calculate dimension based on rows (lower tri)
#if DEBUG
mexPrintf("SDP_DIM: %d, M: %d, N: %d\n",SDP_DIM,SDP_M,SDP_N);
#endif
//Copy cast ir
iSDP_ir = (int*)mxCalloc(SDP_jc[SDP_N],sizeof(int));
for(i=0;i<(int)SDP_jc[SDP_N];i++)
iSDP_ir[i] = (int)SDP_ir[i];
//Set Cone Size
DSDP_ERR( SDPConeSetBlockSize(sdpcone,block,(int)SDP_DIM), "Error setting cone dimensions");
DSDP_ERR( SDPConeUsePackedFormat(sdpcone, block), "Error setting cone packed format");
DSDP_ERR( SDPConeSetSparsity(sdpcone,block,(int)SDP_jc[SDP_N]), "Error setting cone sparsity nnz");
//Add C
SDP_C_nnz = (int)(SDP_jc[1]-SDP_jc[0]);
#if DEBUG
mexPrintf("C nnz: %d\n",SDP_C_nnz);
#endif
DSDP_ERR( SDPConeSetASparseVecMat(sdpcone,block,0,SDP_DIM,1.0,0,iSDP_ir,SDP_pr,SDP_C_nnz), "Error setting cone C matrix");
//Add Each A
SDP_A_index = SDP_C_nnz; //set index to elements after C
for(i=1;i<SDP_N;i++) {
SDP_A_nnz = (int)(SDP_jc[i+1]-SDP_jc[i]);
#if DEBUG
mexPrintf("A[%d] nnz: %d, A index: %d, pr[0] = %f ir[0] = %d\n",i-1,SDP_A_nnz,SDP_A_index,SDP_pr[SDP_A_index],iSDP_ir[SDP_A_index]);
#endif
DSDP_ERR( SDPConeSetASparseVecMat(sdpcone,block,i,SDP_DIM,1.0,0,&iSDP_ir[SDP_A_index],&SDP_pr[SDP_A_index],SDP_A_nnz), "Error setting cone A matrix");
SDP_A_index += SDP_A_nnz; //shift index forward
}
//Add X Vector if supplied
if(X != NULL)
DSDP_ERR( SDPConeSetXArray(sdpcone,block, SDP_DIM, X, SDP_M), "Error Setting SDPCone X Array" );
return SDP_DIM;
//Do not free memory here, MATLAB will take care of it once MEX completes (DSDP will use it during solving) [See MATLAB/User's Guide/C/C++ and Fortran API Reference/mxFree]
}
void DSDPInternal::evaluate(int nfdir, int nadir) {
int info;
// Copy b vector
for (int i=0; i<m_; ++i) {
info = DSDPSetDualObjective(dsdp_, i+1, -input(SDP_B).at(i));
}
// Get Ai from supplied A
mapping_.setInput(input(SDP_C),0);
mapping_.setInput(input(SDP_A),1);
// Negate because the standard form in PSDP is different
std::transform(mapping_.input(0).begin(), mapping_.input(0).end(), mapping_.input(0).begin(), std::negate<double>());
std::transform(mapping_.input(1).begin(), mapping_.input(1).end(), mapping_.input(1).begin(), std::negate<double>());
mapping_.evaluate();
for (int i=0; i<m_+1; ++i) {
for (int j=0; j<nb_; ++j) {
mapping_.output(i*nb_+j).get(values_[i][j],SPARSESYM);
info = SDPConeSetASparseVecMat(sdpcone_, j, i, block_sizes_[j], 1, 0, &pattern_[i][j][0], &values_[i][j][0], pattern_[i][j].size() );
}
}
info = DSDPSetup(dsdp_);
info = DSDPSolve(dsdp_);
casadi_assert_message(info==0,"DSDPSolver failed");
DSDPTerminationReason reason;
DSDPStopReason(dsdp_, &reason);
std::cout << "Termination reason: " << (*terminationReason_.find(reason)).second << std::endl;
DSDPSolutionType pdfeasible;
DSDPGetSolutionType(dsdp_,&pdfeasible);
std::cout << "Solution type: " << (*solutionType_.find(pdfeasible)).second << std::endl;
info = DSDPGetY(dsdp_,&output(SDP_PRIMAL).at(0),m_);
double temp;
DSDPGetDDObjective(dsdp_, &temp);
output(SDP_PRIMAL_COST).set(-temp);
DSDPGetPPObjective(dsdp_, &temp);
output(SDP_DUAL_COST).set(-temp);
if (calc_dual_) {
for (int j=0; j<nb_; ++j) {
info = SDPConeComputeX(sdpcone_, j, block_sizes_[j], &store_X_[j][0], store_X_[j].size());
Pmapper_.input(j).set(store_X_[j],SPARSESYM);
}
Pmapper_.evaluate();
std::copy(Pmapper_.output().data().begin(),Pmapper_.output().data().end(),output(SDP_DUAL).data().begin());
}
if (calc_p_) {
for (int j=0; j<nb_; ++j) {
info = SDPConeComputeS(sdpcone_, j, 1.0, &output(SDP_PRIMAL).at(0), m_, 0, block_sizes_[j] , &store_P_[j][0], store_P_[j].size());
Pmapper_.input(j).set(store_P_[j],SPARSESYM);
}
Pmapper_.evaluate();
std::copy(Pmapper_.output().data().begin(),Pmapper_.output().data().end(),output(SDP_PRIMAL_P).data().begin());
}
}
