/* ************************************************************
PROCEDURE psdinvscale - Computes y = D(d^{-1})x.
(transp == 0) Y = U\X/U'
(transp == 1) Y = U'\X/U.
INPUT
x - length lenud input vector.
ud - Cholesky factor of d for PSD part (after PERM ordering).
psdNL - length psdN array
rpsdN - number of real symmetric PSD blocks
sdpN - total number of psd blocks
transp - boolean
OUTPUT
y - length lenud output vector, y=D(d)x.
WORK
fwork - length max(rmaxn^2,2*hmaxn^2) working vector.
************************************************************ */
void psdinvscale(double *y, const double *ud, const double *x,
const mwIndex *psdNL, const mwIndex rsdpN, const mwIndex sdpN,
bool transp, double *fwork)
{
mwIndex k,nk,nksqr;
/* ------------------------------------------------------------
PSD, !transp: triu(Y) = triu(Ld' \ (X(perm,perm) / Ld)).
Needs ony tril(X/Ld).
------------------------------------------------------------ */
if(!transp){
for(k = 0; k < rsdpN; k++){ /* real symmetric */
nk = psdNL[k];
invltxl(y,ud,x,nk,fwork);
tril2sym(y,nk);
nksqr = SQR(nk);
y += nksqr; ud += nksqr;
x += nksqr;
}
for(; k < sdpN; k++){ /* complex Hermitian */
nk = psdNL[k];
nksqr = SQR(nk);
prpiinvltxl(y,y+nksqr,ud,ud+nksqr,x,x+nksqr,nk,fwork);
tril2herm(y,y+nksqr,nk);
nksqr += nksqr;
y += nksqr; ud += nksqr;
x += nksqr;
}
} /* !transp */
else{
/* ------------------------------------------------------------
PSD, transp: triu(Y) = triu(Ud' \ (X(perm,perm) / Ud)).
Needs ony triu(X/Ud).
------------------------------------------------------------ */
for(k = 0; k < rsdpN; k++){ /* real symmetric */
nk = psdNL[k];
invutxu(y,ud,x,nk,fwork);
triu2sym(y,nk);
nksqr = SQR(nk);
y += nksqr; ud += nksqr;
x += nksqr;
}
for(; k < sdpN; k++){ /* complex Hermitian */
nk = psdNL[k];
nksqr = SQR(nk);
prpiinvutxu(y,y+nksqr,ud,ud+nksqr,x,x+nksqr,nk,fwork);
triu2herm(y,y+nksqr,nk);
nksqr += nksqr;
y += nksqr; ud += nksqr;
x += nksqr;
}
}
}
/* ************************************************************
PROCEDURE mexFunction - Entry for Matlab
[ux,ispos] = psdfactor(x,K);
************************************************************ */
void mexFunction(const int nlhs, mxArray *plhs[],
const int nrhs, const mxArray *prhs[])
{
mxArray *myplhs[NPAROUT];
coneK cK;
mwIndex k,nk,nksqr, sdplen,sdpdim,lenfull, ispos;
const double *x;
double *ux;
/* ------------------------------------------------------------
Check for proper number of arguments
------------------------------------------------------------ */
mxAssert(nrhs >= NPARIN, "psdfactor requires more input arguments");
mxAssert(nlhs <= NPAROUT, "psdfactor produces less output arguments");
/* ------------------------------------------------------------
Disassemble cone K structure
------------------------------------------------------------ */
conepars(K_IN, &cK);
/* ------------------------------------------------------------
Compute statistics: sdpdim = rdim+hdim, sdplen = sum(K.s).
------------------------------------------------------------ */
lenfull = cK.lpN + cK.qDim + cK.rDim + cK.hDim;
sdpdim = cK.rDim + cK.hDim;
sdplen = cK.rLen + cK.hLen;
/* ------------------------------------------------------------
Get input vector x, skip LP + Lorentz part
------------------------------------------------------------ */
x = mxGetPr(X_IN);
if(mxGetM(X_IN) * mxGetN(X_IN) == lenfull)
x += cK.lpN + cK.qDim;
else mxAssert(mxGetM(X_IN) * mxGetN(X_IN) == sdpdim, "x size mismatch.");
/* ------------------------------------------------------------
Allocate output UX(sdpdim), ispos(1).
------------------------------------------------------------ */
UX_OUT = mxCreateDoubleMatrix(sdpdim, (mwSize)1, mxREAL);
ux = mxGetPr(UX_OUT);
ISPOS_OUT = mxCreateDoubleMatrix((mwSize)1,(mwSize)1,mxREAL);
/* ------------------------------------------------------------
PSD: Cholesky factorization.
Initialize ispos = 1 and ux = x.
------------------------------------------------------------ */
ispos = 1;
memcpy(ux, x, sdpdim * sizeof(double)); /* copy real + complex */
for(k = 0; k < cK.rsdpN; k++){ /* real symmetric */
nk = cK.sdpNL[k];
/* ------------------------------------------------------------
Attempt Cholesky on block k. Returns 1 if fail (i.e. not psd).
------------------------------------------------------------ */
if(cholnopiv(ux,nk)){
ispos = 0;
break;
}
triu2sym(ux,nk);
ux += SQR(nk);
}
/* ------------------------------------------------------------
Complex Hermitian PSD Cholesky factorization, no pivoting.
------------------------------------------------------------ */
if(ispos)
for(; k < cK.sdpN; k++){ /* complex Hermitian */
nk = cK.sdpNL[k];
nksqr = SQR(nk);
if(prpicholnopiv(ux,ux+nksqr,nk)){
ispos = 0;
break;
}
triu2herm(ux,ux+nksqr,nk);
ux += 2 * nksqr;
}
/* ------------------------------------------------------------
Return parameter ispos
------------------------------------------------------------ */
*mxGetPr(ISPOS_OUT) = ispos;
/* ------------------------------------------------------------
Copy requested output parameters (at least 1), release others.
------------------------------------------------------------ */
k = MAX(nlhs, 1);
memcpy(plhs,myplhs, k * sizeof(mxArray *));
for(; k < NPAROUT; k++)
mxDestroyArray(myplhs[k]);
}
/* ************************************************************
PROCEDURE mexFunction - Entry for Matlab
[qdetx,ux,ispos,perm] = factorK(x,K);
************************************************************ */
void mexFunction(const int nlhs, mxArray *plhs[],
const int nrhs, const mxArray *prhs[])
{
mxArray *myplhs[NPAROUT];
coneK cK;
int i,k,nk,nksqr, sdplen,sdpdim,lenfull, fwsiz, ispos;
const double *x;
double *ux, *fwork, *permPr, *qdetx, *up, *uppi;
int *iwork, *perm;
double uxk;
char use_pivot;
/* ------------------------------------------------------------
Check for proper number of arguments
------------------------------------------------------------ */
mxAssert(nrhs >= NPARIN, "factorK requires more input arguments");
mxAssert(nlhs <= NPAROUT, "factorK produces less output arguments");
use_pivot = (nlhs == NPAROUT);
/* ------------------------------------------------------------
Disassemble cone K structure
------------------------------------------------------------ */
conepars(K_IN, &cK);
/* ------------------------------------------------------------
Compute statistics: sdpdim = rdim+hdim, sdplen = sum(K.s).
------------------------------------------------------------ */
lenfull = cK.lpN + cK.qDim + cK.rDim + cK.hDim;
sdpdim = cK.rDim + cK.hDim;
sdplen = cK.rLen + cK.hLen;
/* ------------------------------------------------------------
Get input vector x, skip LP part
------------------------------------------------------------ */
mxAssert(mxGetM(X_IN) * mxGetN(X_IN) == lenfull, "x size mismatch.");
x = mxGetPr(X_IN) + cK.lpN;
/* ------------------------------------------------------------
Allocate output qdetx(lorN), UX(sdpdim), perm(sdplen), ispos(1).
------------------------------------------------------------ */
QDETX_OUT = mxCreateDoubleMatrix(cK.lorN, 1, mxREAL);
qdetx = mxGetPr(QDETX_OUT);
UX_OUT = mxCreateDoubleMatrix(sdpdim, 1, mxREAL);
ux = mxGetPr(UX_OUT);
ISPOS_OUT = mxCreateDoubleMatrix(1,1,mxREAL);
PERM_OUT = mxCreateDoubleMatrix(sdplen, 1, mxREAL);
permPr = mxGetPr(PERM_OUT);
/* ------------------------------------------------------------
Allocate working arrays iwork(sdplen),
fwork(MAX(rmaxn^2,2*hmaxn^2) + MAX(rmaxn,hmaxn))
------------------------------------------------------------ */
iwork = (int *) mxCalloc(sdplen, sizeof(int));
perm = iwork;
fwsiz = MAX(cK.rMaxn,cK.hMaxn);
fwork = (double *) mxCalloc(fwsiz + MAX(SQR(cK.rMaxn),2*SQR(cK.hMaxn)),
sizeof(double));
up = fwork + fwsiz;
uppi = up + SQR(cK.hMaxn);
/* ------------------------------------------------------------
LORENTZ: qdetx = sqrt(qdet(x))
------------------------------------------------------------ */
ispos = 1;
for(k = 0; k < cK.lorN; k++){
nk = cK.lorNL[k];
if( (uxk = qdet(x,nk)) < 0.0){
ispos = 0;
break;
}
else
qdetx[k] = sqrt(uxk);
x += nk;
}
/* ------------------------------------------------------------
PSD: Cholesky factorization. If use_pivot, then do pivoting.
------------------------------------------------------------ */
if(use_pivot){
if(ispos)
for(k = 0; k < cK.rsdpN; k++){ /* real symmetric */
nk = cK.sdpNL[k];
if(cholpivot(up,perm, x,nk, fwork)){
ispos = 0;
break;
}
uperm(ux, up, perm, nk);
triu2sym(ux,nk);
nksqr = SQR(nk);
x += nksqr; ux += nksqr;
perm += nk;
}
/* ------------------------------------------------------------
Complex Hermitian PSD pivoted Cholesky factorization
------------------------------------------------------------ */
if(ispos)
for(; k < cK.sdpN; k++){ /* complex Hermitian */
nk = cK.sdpNL[k];
nksqr = SQR(nk);
if(prpicholpivot(up,uppi,perm, x,x+nksqr,nk, fwork)){
ispos = 0;
break;
}
uperm(ux, up, perm, nk); /* real part */
uperm(ux+nksqr, uppi, perm, nk); /* imaginary part */
triu2herm(ux,ux+nksqr,nk);
nksqr += nksqr; /* 2*n^2 for real+imag */
x += nksqr; ux += nksqr;
perm += nk;
}
/* ------------------------------------------------------------
Convert "perm" to Fortran-index in doubles.
------------------------------------------------------------ */
for(i = 0; i < sdplen; i++)
permPr[i] = 1.0 + iwork[i];
}
/* ------------------------------------------------------------
PSD, !use_pivot: Cholesky without pivoting.
First let ux = x, then ux=chol(ux).
------------------------------------------------------------ */
else{ /* Cholesky real sym PSD without pivoting */
if(ispos){
memcpy(ux, x, sdpdim * sizeof(double)); /* copy real + complex */
for(k = 0; k < cK.rsdpN; k++){ /* real symmetric */
nk = cK.sdpNL[k];
if(cholnopiv(ux,nk)){
ispos = 0;
break;
}
triu2sym(ux,nk);
ux += SQR(nk);
}
}
/* ------------------------------------------------------------
Complex Hermitian PSD Cholesky factorization, no pivoting.
------------------------------------------------------------ */
if(ispos)
for(; k < cK.sdpN; k++){ /* complex Hermitian */
nk = cK.sdpNL[k];
nksqr = SQR(nk);
if(prpicholnopiv(ux,ux+nksqr,nk)){
ispos = 0;
break;
}
triu2herm(ux,ux+nksqr,nk);
ux += 2 * nksqr;
}
} /* !use_pivot */
/* ------------------------------------------------------------
Return parameter ispos
------------------------------------------------------------ */
*mxGetPr(ISPOS_OUT) = ispos;
/* ------------------------------------------------------------
Release working arrays
------------------------------------------------------------ */
mxFree(iwork);
mxFree(fwork);
/* ------------------------------------------------------------
Copy requested output parameters (at least 1), release others.
------------------------------------------------------------ */
i = MAX(nlhs, 1);
memcpy(plhs,myplhs, i * sizeof(mxArray *));
for(; i < NPAROUT; i++)
mxDestroyArray(myplhs[i]);
}
/* ************************************************************
PROCEDURE mexFunction - Entry for Matlab
************************************************************ */
void mexFunction(const int nlhs, mxArray *plhs[],
const int nrhs, const mxArray *prhs[])
{
mxArray *myplhs[NPAROUT];
int i,j,k, nk, nksqr, lenud, sdplen, gnnz, inz, maxKs,maxKssqr, rgnnz, hgnnz;
const double *uOld, *permOld;
double *u, *d, *gjcPr, *permPr, *fwork, *fworkpi;
int *perm, *gjc;
double *g, *gk;
double maxusqr;
coneK cK;
char use_pivot;
/* ------------------------------------------------------------
Check for proper number of arguments
------------------------------------------------------------ */
mxAssert(nrhs >= NPARINMIN, "urotorder requires more input arguments.");
mxAssert(nlhs <= NPAROUT, "urotorder generates less output arguments.");
/* ------------------------------------------------------------
Disassemble cone K structure
------------------------------------------------------------ */
conepars(K_IN, &cK);
/* ------------------------------------------------------------
Get statistics of cone K structure
------------------------------------------------------------ */
lenud = cK.rDim + cK.hDim;
sdplen = cK.rLen + cK.hLen;
/* ------------------------------------------------------------
Get scalar input MAXU and input vectors U_IN, PERM_IN
------------------------------------------------------------ */
maxusqr = mxGetScalar(MAXU_IN);
maxusqr *= maxusqr;
mxAssert(mxGetM(U_IN) * mxGetN(U_IN) == lenud, "u size mismatch");
uOld = mxGetPr(U_IN);
use_pivot = 0;
if(nrhs >= NPARIN) /* Optional permIN */
if(mxGetM(PERM_IN) * mxGetN(PERM_IN) > 0) {
mxAssert(mxGetM(PERM_IN) * mxGetN(PERM_IN) == sdplen, "perm size mismatch");
use_pivot = 1;
permOld = mxGetPr(PERM_IN);
}
/* ------------------------------------------------------------
Allocate output U_OUT, and initialize u_out = u_in.
------------------------------------------------------------ */
U_OUT = mxCreateDoubleMatrix(lenud, 1, mxREAL);
u = mxGetPr(U_OUT);
memcpy(u, mxGetPr(U_IN), lenud * sizeof(double));
/* ------------------------------------------------------------
Allocate outputs PERM(sum(K.s)), GJC(sum(K.s))
------------------------------------------------------------ */
PERM_OUT = mxCreateDoubleMatrix(sdplen, 1, mxREAL);
permPr = mxGetPr(PERM_OUT);
GJC_OUT = mxCreateDoubleMatrix(sdplen, 1, mxREAL);
gjcPr = mxGetPr(GJC_OUT);
/* ------------------------------------------------------------
Allocate g initially as length (lenud - cK.rLen) / 2. The final
length can be shorter (viz. gjc[sum(K.s)])
------------------------------------------------------------ */
rgnnz = (cK.rDim - cK.rLen) / 2; /* n(n-1)/2 real sym */
hgnnz = (cK.hDim - 2*cK.hLen) / 4; /* n(n-1)/2 complex herm */
gnnz = rgnnz * 2 + hgnnz * 3;
g = (double *) mxCalloc(MAX(1, gnnz),sizeof(double));
/* ------------------------------------------------------------
Allocate working arrays:
Let maxKssqr = max(rMaxn^2, 2*hMaxn^2), then
integer perm(max(K.s)), gjc(max(K.s))
double d(max(K.s)), fwork(maxKs)
------------------------------------------------------------ */
maxKs = MAX(cK.rMaxn,cK.hMaxn); /* max(K.s) */
maxKssqr = MAX(SQR(cK.rMaxn),2 * SQR(cK.hMaxn)); /* max(K.s.^2) */
perm = (int *) mxCalloc(MAX(1,maxKs), sizeof(int));
gjc = (int *) mxCalloc(MAX(1,maxKs), sizeof(int));
d = (double *) mxCalloc(MAX(1,maxKs), sizeof(double));
fwork = (double *) mxCalloc(MAX(1,maxKssqr), sizeof(double));
fworkpi = fwork + SQR(cK.hMaxn);
/* ------------------------------------------------------------
The actual job is done here: U_NEW = Q(g) * U_OLD
------------------------------------------------------------ */
inz = 0;
for(k = 0; k < cK.rsdpN; k++) { /* real symmetric */
nk = cK.sdpNL[k];
nksqr = SQR(nk);
memcpy(fwork, uOld, nksqr *sizeof(double)); /* k-th U-matrix */
gk = g+inz;
rotorder(perm, fwork, gjc, (twodouble *) gk, d, maxusqr, nk);
/* ------------------------------------------------------------
Physically reorder the columns from fwork into u. Then Let
tril(U) = triu(U)'
------------------------------------------------------------ */
uperm(u, fwork, perm, nk);
triu2sym(u,nk);
/* ------------------------------------------------------------
Let perm_out = perm_in(perm)
------------------------------------------------------------ */
if(use_pivot) {
for(i = 0; i < nk; i++)
permPr[i] = permOld[perm[i]];
permOld += nk;
}
else
for(i = 0; i < nk; i++)
permPr[i] = 1.0 + perm[i];
for(i = 0; i < nk; i++)
gjcPr[i] = gjc[i]; /* don't add 1 */
inz += 2 * gjc[nk-1]; /* next PSD block. Rotation g is 2 doubles */
gjcPr += nk;
permPr += nk;
uOld += nksqr;
u += nksqr;
}
/* ------------------------------------------------------------
Complex Hermitian
------------------------------------------------------------ */
for(; k < cK.sdpN; k++) { /* complex Hermitian */
nk = cK.sdpNL[k];
nksqr = SQR(nk);
memcpy(fwork, uOld, nksqr *sizeof(double)); /* k-th complex U-matrix */
memcpy(fworkpi, uOld+nksqr, nksqr *sizeof(double));
gk = g+inz;
prpirotorder(perm, fwork,fworkpi, gjc, (tridouble *) gk, d, maxusqr, nk);
/* ------------------------------------------------------------
Physically reorder the columns from fwork into u. Then Let
tril(U) = triu(U)'
------------------------------------------------------------ */
uperm(u, fwork, perm, nk); /* real part */
uperm(u+nksqr, fworkpi, perm, nk); /* imaginary part */
triu2herm(u,u+nksqr, nk);
/* ------------------------------------------------------------
Let perm_out = perm_in(perm)
------------------------------------------------------------ */
if(use_pivot) {
for(i = 0; i < nk; i++)
permPr[i] = permOld[perm[i]];
permOld += nk;
}
else
for(i = 0; i < nk; i++)
permPr[i] = 1.0 + perm[i];
for(i = 0; i < nk; i++)
gjcPr[i] = gjc[i]; /* don't add 1 */
inz += 3 * gjc[nk-1]; /* next PSD block. Rotation g is 3 doubles */
gjcPr += nk;
permPr += nk;
nksqr += nksqr;
uOld += nksqr;
u += nksqr;
}
/* ------------------------------------------------------------
In total, we used inz doubles in Givens rotations.
Reallocate (shrink) g accordingly.
------------------------------------------------------------ */
mxAssert(inz <= gnnz,"");
if(inz > 0) {
if((g = (double *) mxRealloc(g, inz * sizeof(double))) == NULL)
mexErrMsgTxt("Memory allocation error.");
}
else {
mxFree(g);
g = (double *) NULL;
}
/* ------------------------------------------------------------
Assign g to a length inz output vector
------------------------------------------------------------ */
G_OUT = mxCreateDoubleMatrix(1, 1, mxREAL);
mxFree(mxGetPr(G_OUT));
mxSetPr(G_OUT, (double *) g);
mxSetM(G_OUT, inz);
/* ------------------------------------------------------------
Release working arrays
------------------------------------------------------------ */
mxFree(fwork);
mxFree(d);
mxFree(gjc);
mxFree(perm);
/* ------------------------------------------------------------
Copy requested output parameters (at least 1), release others.
------------------------------------------------------------ */
i = MAX(nlhs, 1);
memcpy(plhs,myplhs, i * sizeof(mxArray *));
for(; i < NPAROUT; i++)
mxDestroyArray(myplhs[i]);
}