/**
* Multiplica duas matrizes
* @param m1 [description]
* @param m2 [description]
* @return [description]
*/
SparseMatrix* multiply(SparseMatrix* m1, SparseMatrix* m2)
{
SparseMatrix* result = newSparseMatrix();
// Verifica se colunas1 == linhas2
if (m1->_maximumX != m2->_maximumY)
{
printf("As matrizes a serem multiplicadas devem ter o mesmo número de colunas e linhas, respectivamente.\n");
exit(1);
}
long x, y, k;
// Quantidade de elementos de cada linha/coluna
long max = m1->_maximumX;
for (x = 1; x <= m1->_maximumY; x++)
{
for (y = 1; y <= m2->_maximumX; y++)
{
long cellValue = 0;
// a11.b11 + a21.b12 + a31.b13
for (k = 1; k <= max; k++)
{
cellValue += getCellValue(m1, k, x) * getCellValue(m2, y, k);
}
setCell(result, y, x, cellValue);
}
}
return result;
}
/**
* Transpose a matrix; returns A = M'
*/
spmat* transposeSparseMatrix(spmat* M)
{
idxint j, i, k, q;
idxint* w = (idxint *)MALLOC(M->m*sizeof(idxint));
spmat* A = newSparseMatrix(M->n, M->m, M->nnz);
/* row count: how often does row k occur in M? */
for( i=0; i < M->m; i++ ) { w[i] = 0; }
for( k=0; k < M->nnz; k++ ) { w[M->ir[k]]++; }
/* row pointers: cumulative sum of w gives A->jc */
spla_cumsum(A->jc, w, M->m);
/* now walk through M and copy data to right places and set row counter */
for( j=0; j < M->n; j++ ){
for( k = M->jc[j]; k < M->jc[j+1]; k++ ){
q = w[M->ir[k]]++;
A->ir[q] = j;
A->pr[q] = M->pr[k];
}
}
FREE(w);
return A;
}
/**
* Subtrai duas matrizes, desde que com dimensões iguais
* @param m1 [description]
* @param m2 [description]
* @return [description]
*/
SparseMatrix* subtract(SparseMatrix* m1, SparseMatrix* m2)
{
if (m1->_maximumX != m2->_maximumX || m1->_maximumY != m2->_maximumY)
{
printf("As matrizes a serem somadas devem ter o mesmo número de linhas e colunas.\n");
exit(1);
}
SparseMatrix* matrix = newSparseMatrix();
long x, y, diff;
for (x = 1; x <= m1->_maximumX; x++)
{
for (y = 1; y <= m1->_maximumY; y++)
{
diff = getCellValue(m1, x, y) - getCellValue(m2, x, y);
if (diff != 0)
{
setCell(matrix, x, y, diff);
}
}
}
return matrix;
}
/*
* Returns a copy of a sparse matrix A.
*/
spmat* copySparseMatrix(spmat* A)
{
idxint i;
spmat* B = newSparseMatrix(A->m, A->n, A->nnz);
/* copy over */
for( i=0; i<=A->n; i++ ){ B->jc[i] = A->jc[i]; }
for( i=0; i<A->nnz; i++ ){ B->ir[i] = A->ir[i]; }
for( i=0; i<A->nnz; i++ ){ B->pr[i] = A->pr[i]; }
return B;
}
/**
* Cria a transposta de uma matriz
* @param matrix [description]
* @return [description]
*/
SparseMatrix* transpose(SparseMatrix* matrix)
{
SparseMatrix* m = newSparseMatrix();
long x, y, cellValue;
for (x = 1; x <= matrix->_maximumX; x++)
{
for (y = 1; y <= matrix->_maximumY; y++)
{
cellValue = getCellValue(matrix, x, y);
if (cellValue != 0)
{
setCell(m, y, x, cellValue);
}
}
}
return m;
}
/**
* Multiplica uma matriz por um escalar
* @param matrix [description]
* @param scalar [description]
* @return [description]
*/
SparseMatrix* multiplyByScalar(SparseMatrix* matrix, long scalar)
{
SparseMatrix* m = newSparseMatrix();
long x, y, cellValue;
for (x = 1; x <= matrix->_maximumX; x++)
{
for (y = 1; y <= matrix->_maximumY; y++)
{
cellValue = getCellValue(matrix, x, y);
if (cellValue != 0)
{
cellValue *= scalar;
setCell(m, x, y, cellValue);
}
}
}
return m;
}
/*
* Sets up all data structures needed.
* Replace by codegen
*/
pwork* ECOS_setup(idxint n, idxint m, idxint p, idxint l, idxint ncones, idxint* q,
pfloat* Gpr, idxint* Gjc, idxint* Gir,
pfloat* Apr, idxint* Ajc, idxint* Air,
pfloat* c, pfloat* h, pfloat* b)
{
idxint i, j, k, cidx, conesize, lnz, amd_result, nK, *Ljc, *Lir, *P, *Pinv, *Sign;
pwork* mywork;
double Control [AMD_CONTROL], Info [AMD_INFO];
pfloat rx, ry, rz, *Lpr;
spmat *At, *Gt, *KU;
#if PROFILING > 0
timer tsetup;
#endif
#if PROFILING > 1
timer tcreatekkt;
timer tmattranspose;
timer tordering;
#endif
#if PROFILING > 0
tic(&tsetup);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("\n");
PRINTTEXT(" *******************************************************************************\n");
PRINTTEXT(" * ECOS: Embedded Conic Solver - Sparse Interior Point method for SOCPs *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * NOTE: The solver is based on L. Vandenberghe's 'The CVXOPT linear and quad- *\n");
PRINTTEXT(" * ratic cone program solvers', March 20, 2010. Available online: *\n");
PRINTTEXT(" * [http://abel.ee.ucla.edu/cvxopt/documentation/coneprog.pdf] *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * This code uses T.A. Davis' sparse LDL package and AMD code. *\n");
PRINTTEXT(" * [http://www.cise.ufl.edu/research/sparse] *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * Written during a summer visit at Stanford University with S. Boyd. *\n");
PRINTTEXT(" * *\n");
PRINTTEXT(" * (C) Alexander Domahidi, Automatic Control Laboratory, ETH Zurich, 2012-13. *\n");
PRINTTEXT(" * Email: [email protected] *\n");
PRINTTEXT(" *******************************************************************************\n");
PRINTTEXT("\n\n");
PRINTTEXT("PROBLEM SUMMARY:\n");
PRINTTEXT(" Primal variables (n): %d\n", (int)n);
PRINTTEXT("Equality constraints (p): %d\n", (int)p);
PRINTTEXT(" Conic variables (m): %d\n", (int)m);
PRINTTEXT("- - - - - - - - - - - - - - -\n");
PRINTTEXT(" Size of LP cone: %d\n", (int)l);
PRINTTEXT(" Number of SOCs: %d\n", (int)ncones);
for( i=0; i<ncones; i++ ){
PRINTTEXT(" Size of SOC #%02d: %d\n", (int)(i+1), (int)q[i]);
}
#endif
/* get work data structure */
mywork = (pwork *)MALLOC(sizeof(pwork));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for WORK struct\n");
#endif
/* dimensions */
mywork->n = n;
mywork->m = m;
mywork->p = p;
mywork->D = l + ncones;
#if PRINTLEVEL > 2
PRINTTEXT("Set dimensions\n");
#endif
/* variables */
mywork->x = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->y = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->z = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->s = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->lambda = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->dsaff_by_W = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->dsaff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->dzaff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->saff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->zaff = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->W_times_dzaff = (pfloat *)MALLOC(m*sizeof(pfloat));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for variables\n");
#endif
/* best iterates so far */
mywork->best_x = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->best_y = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->best_z = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->best_s = (pfloat *)MALLOC(m*sizeof(pfloat));
mywork->best_info = (stats *)MALLOC(sizeof(stats));
/* cones */
mywork->C = (cone *)MALLOC(sizeof(cone));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for cone struct\n");
#endif
/* LP cone */
mywork->C->lpc = (lpcone *)MALLOC(sizeof(lpcone));
mywork->C->lpc->p = l;
if( l > 0 ){
mywork->C->lpc->w = (pfloat *)MALLOC(l*sizeof(pfloat));
mywork->C->lpc->v = (pfloat *)MALLOC(l*sizeof(pfloat));
mywork->C->lpc->kkt_idx = (idxint *)MALLOC(l*sizeof(idxint));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for LP cone\n");
#endif
} else {
mywork->C->lpc->w = NULL;
mywork->C->lpc->v = NULL;
mywork->C->lpc->kkt_idx = NULL;
#if PRINTLEVEL > 2
PRINTTEXT("No LP cone present, pointers filled with NULL\n");
#endif
}
/* Second-order cones */
mywork->C->soc = (socone *)MALLOC(ncones*sizeof(socone));
mywork->C->nsoc = ncones;
cidx = 0;
for( i=0; i<ncones; i++ ){
conesize = (idxint)q[i];
mywork->C->soc[i].p = conesize;
mywork->C->soc[i].a = 0;
mywork->C->soc[i].eta = 0;
mywork->C->soc[i].q = (pfloat *)MALLOC((conesize-1)*sizeof(pfloat));
mywork->C->soc[i].skbar = (pfloat *)MALLOC((conesize)*sizeof(pfloat));
mywork->C->soc[i].zkbar = (pfloat *)MALLOC((conesize)*sizeof(pfloat));
#if CONEMODE == 0
mywork->C->soc[i].Didx = (idxint *)MALLOC((conesize)*sizeof(idxint));
#endif
#if CONEMODE > 0
mywork->C->soc[i].colstart = (idxint *)MALLOC((conesize)*sizeof(idxint));
#endif
cidx += conesize;
}
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for second-order cones\n");
#endif
/* info struct */
mywork->info = (stats *)MALLOC(sizeof(stats));
#if PROFILING > 1
mywork->info->tfactor = 0;
mywork->info->tkktsolve = 0;
mywork->info->tfactor_t1 = 0;
mywork->info->tfactor_t2 = 0;
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for info struct\n");
#endif
#if defined EQUILIBRATE && EQUILIBRATE > 0
/* equilibration vector */
mywork->xequil = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->Aequil = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->Gequil = (pfloat *)MALLOC(m*sizeof(pfloat));
#if PRINTLEVEL > 2
PRINTTEXT("Memory allocated for equilibration vectors\n");
#endif
#endif
/* settings */
mywork->stgs = (settings *)MALLOC(sizeof(settings));
mywork->stgs->maxit = MAXIT;
mywork->stgs->gamma = GAMMA;
mywork->stgs->delta = DELTA;
mywork->stgs->eps = EPS;
mywork->stgs->nitref = NITREF;
mywork->stgs->abstol = ABSTOL;
mywork->stgs->feastol = FEASTOL;
mywork->stgs->reltol = RELTOL;
mywork->stgs->abstol_inacc = ATOL_INACC;
mywork->stgs->feastol_inacc = FTOL_INACC;
mywork->stgs->reltol_inacc = RTOL_INACC;
mywork->stgs->verbose = VERBOSE;
#if PRINTLEVEL > 2
PRINTTEXT("Written settings\n");
#endif
mywork->c = c;
mywork->h = h;
mywork->b = b;
#if PRINTLEVEL > 2
PRINTTEXT("Hung pointers for c, h and b into WORK struct\n");
#endif
/* Store problem data */
if(Apr && Ajc && Air) {
mywork->A = createSparseMatrix(p, n, Ajc[n], Ajc, Air, Apr);
} else {
mywork->A = NULL;
}
if (Gpr && Gjc && Gir) {
mywork->G = createSparseMatrix(m, n, Gjc[n], Gjc, Gir, Gpr);
} else {
/* create an empty sparse matrix */
mywork->G = createSparseMatrix(m, n, 0, Gjc, Gir, Gpr);
}
#if defined EQUILIBRATE && EQUILIBRATE > 0
set_equilibration(mywork);
#if PRINTLEVEL > 2
PRINTTEXT("Done equilibrating\n");
#endif
#endif
#if PROFILING > 1
mywork->info->ttranspose = 0;
tic(&tmattranspose);
#endif
if(mywork->A)
At = transposeSparseMatrix(mywork->A);
else
At = NULL;
#if PROFILING > 1
mywork->info->ttranspose += toc(&tmattranspose);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Transposed A\n");
#endif
#if PROFILING > 1
tic(&tmattranspose);
#endif
Gt = transposeSparseMatrix(mywork->G);
#if PROFILING > 1
mywork->info->ttranspose += toc(&tmattranspose);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Transposed G\n");
#endif
/* set up KKT system */
#if PROFILING > 1
tic(&tcreatekkt);
#endif
createKKT_U(Gt, At, mywork->C, &Sign, &KU);
#if PROFILING > 1
mywork->info->tkktcreate = toc(&tcreatekkt);
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Created upper part of KKT matrix K\n");
#endif
/*
* Set up KKT system related data
* (L comes later after symbolic factorization)
*/
nK = KU->n;
#if DEBUG > 0
dumpSparseMatrix(KU, "KU0.txt");
#endif
#if PRINTLEVEL > 2
PRINTTEXT("Dimension of KKT matrix: %d\n", (int)nK);
PRINTTEXT("Non-zeros in KKT matrix: %d\n", (int)KU->nnz);
#endif
/* allocate memory in KKT system */
mywork->KKT = (kkt *)MALLOC(sizeof(kkt));
mywork->KKT->D = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->Parent = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->Pinv = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->work1 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work2 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work3 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work4 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work5 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->work6 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->Flag = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->Pattern = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->Lnz = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->RHS1 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->RHS2 = (pfloat *)MALLOC(nK*sizeof(pfloat));
mywork->KKT->dx1 = (pfloat *)MALLOC(mywork->n*sizeof(pfloat));
mywork->KKT->dx2 = (pfloat *)MALLOC(mywork->n*sizeof(pfloat));
mywork->KKT->dy1 = (pfloat *)MALLOC(mywork->p*sizeof(pfloat));
mywork->KKT->dy2 = (pfloat *)MALLOC(mywork->p*sizeof(pfloat));
mywork->KKT->dz1 = (pfloat *)MALLOC(mywork->m*sizeof(pfloat));
mywork->KKT->dz2 = (pfloat *)MALLOC(mywork->m*sizeof(pfloat));
mywork->KKT->Sign = (idxint *)MALLOC(nK*sizeof(idxint));
mywork->KKT->PKPt = newSparseMatrix(nK, nK, KU->nnz);
mywork->KKT->PK = (idxint *)MALLOC(KU->nnz*sizeof(idxint));
#if PRINTLEVEL > 2
PRINTTEXT("Created memory for KKT-related data\n");
#endif
/* calculate ordering of KKT matrix using AMD */
P = (idxint *)MALLOC(nK*sizeof(idxint));
#if PROFILING > 1
tic(&tordering);
#endif
AMD_defaults(Control);
amd_result = AMD_order(nK, KU->jc, KU->ir, P, Control, Info);
#if PROFILING > 1
mywork->info->torder = toc(&tordering);
#endif
if( amd_result == AMD_OK ){
#if PRINTLEVEL > 2
PRINTTEXT("AMD ordering successfully computed.\n");
AMD_info(Info);
#endif
} else {
#if PRINTLEVEL > 2
PRINTTEXT("Problem in AMD ordering, exiting.\n");
AMD_info(Info);
#endif
return NULL;
}
/* calculate inverse permutation and permutation mapping of KKT matrix */
pinv(nK, P, mywork->KKT->Pinv);
Pinv = mywork->KKT->Pinv;
#if DEBUG > 0
dumpDenseMatrix_i(P, nK, 1, "P.txt");
dumpDenseMatrix_i(mywork->KKT->Pinv, nK, 1, "PINV.txt");
#endif
permuteSparseSymmetricMatrix(KU, mywork->KKT->Pinv, mywork->KKT->PKPt, mywork->KKT->PK);
/* permute sign vector */
for( i=0; i<nK; i++ ){ mywork->KKT->Sign[Pinv[i]] = Sign[i]; }
#if PRINTLEVEL > 3
PRINTTEXT("P = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%d ", (int)P[i]); }
PRINTTEXT("];\n");
PRINTTEXT("Pinv = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%d ", (int)Pinv[i]); }
PRINTTEXT("];\n");
PRINTTEXT("Sign = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%+d ", (int)Sign[i]); }
PRINTTEXT("];\n");
PRINTTEXT("SignP = [");
for( i=0; i<nK; i++ ){ PRINTTEXT("%+d ", (int)mywork->KKT->Sign[i]); }
PRINTTEXT("];\n");
#endif
/* symbolic factorization */
Ljc = (idxint *)MALLOC((nK+1)*sizeof(idxint));
#if PRINTLEVEL > 2
PRINTTEXT("Allocated memory for cholesky factor L\n");
#endif
LDL_symbolic2(
mywork->KKT->PKPt->n, /* A and L are n-by-n, where n >= 0 */
mywork->KKT->PKPt->jc, /* input of size n+1, not modified */
mywork->KKT->PKPt->ir, /* input of size nz=Ap[n], not modified */
Ljc, /* output of size n+1, not defined on input */
mywork->KKT->Parent, /* output of size n, not defined on input */
mywork->KKT->Lnz, /* output of size n, not defined on input */
mywork->KKT->Flag /* workspace of size n, not defn. on input or output */
);
/* assign memory for L */
lnz = Ljc[nK];
#if PRINTLEVEL > 2
PRINTTEXT("Nonzeros in L, excluding diagonal: %d\n", (int)lnz) ;
#endif
Lir = (idxint *)MALLOC(lnz*sizeof(idxint));
Lpr = (pfloat *)MALLOC(lnz*sizeof(pfloat));
mywork->KKT->L = createSparseMatrix(nK, nK, lnz, Ljc, Lir, Lpr);
#if PRINTLEVEL > 2
PRINTTEXT("Created Cholesky factor of K in KKT struct\n");
#endif
/* permute KKT matrix - we work on this one from now on */
permuteSparseSymmetricMatrix(KU, mywork->KKT->Pinv, mywork->KKT->PKPt, NULL);
#if DEBUG > 0
dumpSparseMatrix(mywork->KKT->PKPt, "PKPt.txt");
#endif
#if CONEMODE > 0
/* zero any off-diagonal elements in (permuted) scalings in KKT matrix */
for (i=0; i<mywork->C->nsoc; i++) {
for (j=1; j<mywork->C->soc[i].p; j++) {
for (k=0; k<j; k++) {
mywork->KKT->PKPt->pr[mywork->KKT->PK[mywork->C->soc[i].colstart[j]+k]] = 0;
}
}
}
#endif
#if DEBUG > 0
dumpSparseMatrix(mywork->KKT->PKPt, "PKPt0.txt");
#endif
/* set up RHSp for initialization */
k = 0; j = 0;
for( i=0; i<n; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = 0; }
for( i=0; i<p; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = b[i]; }
for( i=0; i<l; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = h[i]; j++; }
for( l=0; l<ncones; l++ ){
for( i=0; i < mywork->C->soc[l].p; i++ ){ mywork->KKT->RHS1[Pinv[k++]] = h[j++]; }
#if CONEMODE == 0
mywork->KKT->RHS1[Pinv[k++]] = 0;
mywork->KKT->RHS1[Pinv[k++]] = 0;
#endif
}
#if PRINTLEVEL > 2
PRINTTEXT("Written %d entries of RHS1\n", (int)k);
#endif
/* set up RHSd for initialization */
for( i=0; i<n; i++ ){ mywork->KKT->RHS2[Pinv[i]] = -c[i]; }
for( i=n; i<nK; i++ ){ mywork->KKT->RHS2[Pinv[i]] = 0; }
/* get scalings of problem data */
rx = norm2(c, n); mywork->resx0 = MAX(1, rx);
ry = norm2(b, p); mywork->resy0 = MAX(1, ry);
rz = norm2(h, m); mywork->resz0 = MAX(1, rz);
/* get memory for residuals */
mywork->rx = (pfloat *)MALLOC(n*sizeof(pfloat));
mywork->ry = (pfloat *)MALLOC(p*sizeof(pfloat));
mywork->rz = (pfloat *)MALLOC(m*sizeof(pfloat));
/* clean up */
mywork->KKT->P = P;
FREE(Sign);
if(At) freeSparseMatrix(At);
freeSparseMatrix(Gt);
freeSparseMatrix(KU);
#if PROFILING > 0
mywork->info->tsetup = toc(&tsetup);
#endif
return mywork;
}