makereal.c

经济学专业代码

/* ------------------------------------------------------------
   Write real PSD blocks into y, i.e. skip Hermitian blocks
   ------------------------------------------------------------ */
  memcpy(ypr, xpr, ipos[1] * sizeof(double));
  if(idelta != 0)
    intscalaradd(yir, idelta, xir, ipos[1]);
  else
    memcpy(yir, xir, ipos[1] * sizeof(int));
  jnz = ipos[1];
  j = idelta;             /* subscript adjustment */
  for(i = 0; i < twons; ){
    j -= cpxsi[i+1] - cpxsi[i];               /* skip complex block */
    i += 2;
    inz = ipos[i];
    k = ipos[i + 1] - inz;
    memcpy(ypr + jnz, xpr + inz, k * sizeof(double));
    intscalaradd(yir+jnz, j, xir+inz, k);
    jnz += k;
  }
/* ------------------------------------------------------------
   Write Hermitian PSD blocks into y
   ------------------------------------------------------------ */
  j += lenfull;    /* j points to 1st available index for Hermitian blocks */
  for(i = 0; i < twons; i += 2){
    k = cpxsi[i];              /* Old 1st index of Herm PSD block */
/* ---------- write real part ---------- */
    writenz(yir, ypr, &jnz, xir, xpr, ipos[i+1],ipos[i+2],j-k);
    j += cpxsi[i+1] - k;                 /* point to 1st available index */
/* ---------- write imag part ---------- */
    if(xpi != (double *) NULL)
      writenz(yir, ypr, &jnz, xir, xpi, ipos[i+1],ipos[i+2],j-k);
    j += cpxsi[i+1] - k;                 /* point to 1st available index */
  }
/* ------------------------------------------------------------
   RETURN number of nonzeros written into y
   ------------------------------------------------------------ */
  return jnz;
}

/* ************************************************************
   PROCEDURE makereal
   INPUT
     xir,xpr,xpi - sparse vector with xnnz nonzeros.
     xnnz - length of xir,xpr,xpi.
     cpxf - length nf integer array, listing free imaginary vars.
     nf - length of cpxf
     cpxx - length nx integer array, listing Lorentz constrained
       imaginary vars.
     nx - length of cpxx.
     cpxsi - length 2*ns increasing integer array. The old subscripts
       of the kth Hermitian block in x are cpxsi[2*k]:cpxsi[2*k+1]-1.
     ns - number of Hermitian PSD blocks.
     lenfull - length of full(x)-vector, 1 beyond last possible subscript.
     iwsize Length of iwork, should be 2 + MAXN + floor(log_2(1+MAXN)).
     lenfull - dimension of x.
     dimflqr - dimension of f/l/q/r part in x, i.e. 1st valid PSD subscript.
   OUTPUT
     yir, ypr - sparse real output vector, ynnz nonzeros.
   WORK ARRAYS
     cfound - length MAXN := MAX(nf,nx,2*ns) character working array.
     iwork - lengt iwsize working array. Needs
      iwsize >= 2 + MAXN + floor(log_2(1+MAXN)).
   RETURNS ynnz (<=2*xnnz), number of nonzeros in y.
   ************************************************************ */
int makereal(int *yir, double *ypr,
             const int *xir, const double *xpr, const double *xpi,
             const int xnnz, const int *cpxf, const int nf,
             const int *cpxx, const int nx, const int *cpxsi,
             const int ns, const int lenfull, const int dimflqr,
             char *cfound, int *iwork, int iwsize)
{
  int jcs, jnz;
/* ------------------------------------------------------------
   Find position of 1st PSD nonzero
   ------------------------------------------------------------ */
  jcs = 0;
  intbsearch(&jcs, xir, xnnz, dimflqr);
/* ------------------------------------------------------------
   Write free imaginary nonzeros into y
   ------------------------------------------------------------ */
  jnz = fmakereal(yir,ypr, xir,xpi,jcs, cpxf,nf, cfound,iwork,iwsize);
/* ------------------------------------------------------------
   Write LP/Lorentz nonzeros into y, make Lorentz bounded imag nonzeros
   explicit reals.
   ------------------------------------------------------------ */
  jnz += xmakereal(yir+jnz,ypr+jnz, xir,xpr,xpi,jcs, nf, cpxx,nx,
                   cfound,iwork,iwsize);
/* ------------------------------------------------------------
   Write PSD nonzeros into y. First the real blocks, then Hermitian.
   ------------------------------------------------------------ */
  if(xpi != (double *) NULL)
    xpi += jcs;                   /* point to 1st imag PSD nonzero */
  jnz += smakereal(yir+jnz, ypr+jnz, xir+jcs,xpr+jcs,xpi,xnnz-jcs,
                   nf+nx, cpxsi,2*ns, lenfull, cfound,iwork,iwsize);
/* ------------------------------------------------------------
   Return number of nonzeros in y
   ------------------------------------------------------------ */
  return jnz;
}

/* ============================================================
   MAIN: MEXFUNCTION
   ============================================================ */
/* ************************************************************
   PROCEDURE mexFunction - Entry for Matlab
   ************************************************************ */
void mexFunction(const int nlhs, mxArray *plhs[],
  const int nrhs, const mxArray *prhs[])
{
  int i,j,jnz,MAXN, m,dimflqr,lenfull,reallength, nf,nx,ns, iwsize;
  int *iwork, *sdpNL, *cpxf, *cpxx, *cpxs, *cpxsi;
  char *cwork;
  const double *cpxfPr, *cpxxPr, *cpxsPr, *xpi;
  mxArray *MY_FIELD;
  coneK cK;
  jcir x,y;
/* ------------------------------------------------------------
   Check for proper number of arguments
   ------------------------------------------------------------ */
  mxAssert(nrhs >= NPARIN, "makereal requires more input arguments");
  mxAssert(nlhs <= NPAROUT, "makereal produces less output arguments");
/* ------------------------------------------------------------
   Disassemble cone K structure
   ------------------------------------------------------------ */
  conepars(K_IN, &cK);
  dimflqr = cK.frN + cK.lpN + cK.qDim;
  for(i = 0; i < cK.rconeN; i++)        /* add dim of rotated cone */
    dimflqr += cK.rconeNL[i];
  lenfull =  dimflqr + cK.rDim + cK.hDim;
/* ------------------------------------------------------------
   Disassemble cpx structure
   ------------------------------------------------------------ */
  mxAssert(mxIsStruct(CPX_IN), "Parameter `cpx' should be a structure.");
  if( (MY_FIELD = mxGetField(CPX_IN,0,"f")) == NULL)  /* cpx.f */
    nf = 0;
  else{
    nf = mxGetM(MY_FIELD) * mxGetN(MY_FIELD);
    cpxfPr = mxGetPr(MY_FIELD);
  }
  if( (MY_FIELD = mxGetField(CPX_IN,0,"s")) == NULL)  /* cpx.s */
    ns = 0;
  else{
    ns = mxGetM(MY_FIELD) * mxGetN(MY_FIELD);
    cpxsPr = mxGetPr(MY_FIELD);
  }
  if( (MY_FIELD = mxGetField(CPX_IN,0,"x")) == NULL)  /* cpx.x */
    nx = 0;
  else{
    nx = mxGetM(MY_FIELD) * mxGetN(MY_FIELD);
    cpxxPr = mxGetPr(MY_FIELD);
  }
/* ------------------------------------------------------------
   Get input matrix x
   ------------------------------------------------------------ */
  mxAssert(mxGetM(X_IN) == lenfull, "Size x mismatch.");
  m = mxGetN(X_IN);                       /* number of columns to handle */
  mxAssert( mxIsSparse(X_IN), "X should be sparse.");
  x.pr = mxGetPr(X_IN);
  if(mxIsComplex(X_IN))
    xpi = mxGetPi(X_IN);
  else
    xpi = (double *) NULL;
  x.jc = mxGetJc(X_IN);
  x.ir = mxGetIr(X_IN);
/* ------------------------------------------------------------
   Allocate iwork[iwsiz], cwork[MAXN], {K.s, cpx.{f[nf],x[nx],s[ns],si[2*ns]}}
   ------------------------------------------------------------ */
  MAXN = MAX(MAX(nf,nx),2*ns);
  iwsize = floor(log(1 + MAXN) / log(2));       /* for binary tree search */
  iwsize += 2 * ns + 2 + MAXN;
  iwork = (int *) mxCalloc(iwsize, sizeof(int));
  cwork = (char *) mxCalloc(MAX(1,MAXN), sizeof(char));
  sdpNL = (int *) mxCalloc(MAX(1, cK.sdpN + nf + nx + 3*ns), sizeof(int));
  cpxf = sdpNL + cK.sdpN;
  cpxx = cpxf + nf;
  cpxs = cpxx + nx;
  cpxsi = cpxs + ns;        /* length 2*ns */
/* ------------------------------------------------------------
   Convert double to int
   ------------------------------------------------------------ */
  for(i = 0; i < cK.sdpN; i++){        /* K.s */
    j = cK.sdpNL[i];
    sdpNL[i] = j;                    /* These are lengths, not subscripts!*/
  }
  for(i = 0; i < nf; i++){             /* cpx.f */
    j = cpxfPr[i];
    cpxf[i] = --j;
  }
  for(i = 0; i < nx; i++){             /* cpx.x */
    j = cpxxPr[i];
    cpxx[i] = --j;
  }
  for(i = 0; i < ns; i++){             /* cpx.s */
    j = cpxsPr[i];
    cpxs[i] = --j;
  }
/* ------------------------------------------------------------
   Create cpxsi(1:2*ns). This lists the 1st subscript and the
   1-beyond-last subscript of each Hermitian PSD matrix in x.
   ------------------------------------------------------------ */
  jnz = dimflqr;
  j = 0;
  for(i = 0; i < ns; i++){
    for(; j < cpxs[i]; j++)
      jnz += SQR(sdpNL[j]);
    cpxsi[2 * i] = jnz;            /* start of Hermitian block */
    jnz += SQR(sdpNL[j]);
    j++;
    cpxsi[2 * i + 1] = jnz;        /* end of Hermitian block */
  }
/* ------------------------------------------------------------
   Allocate output Y = sparse([],[],[],reallength(x),m,2 * nnz(x))
   ------------------------------------------------------------ */
  reallength = lenfull + nf + nx;
  for(i = 0; i < ns; i++){
    reallength += cpxsi[2*i+1] - cpxsi[2*i];
  }
  jnz = x.jc[m];
  if(xpi != (double *) NULL)
    jnz *= 2;                       /* reserve room for imaginary parts */
  Y_OUT = mxCreateSparse(reallength, m, jnz, mxREAL);
  y.pr = mxGetPr(Y_OUT);
  y.jc = mxGetJc(Y_OUT);
  y.ir = mxGetIr(Y_OUT);
/* ------------------------------------------------------------
   The real job, MAKEREAL:
   ------------------------------------------------------------ */
  y.jc[0] = 0;
  jnz = 0;
  for(i = 0; i < m; i++){
    y.jc[i] = jnz;
    j = x.jc[i+1] - x.jc[i];
    jnz += makereal(y.ir+jnz, y.pr+jnz, x.ir+x.jc[i],x.pr+x.jc[i],xpi,j,
                    cpxf,nf, cpxx,nx, cpxsi,ns, lenfull, dimflqr,
                    cwork, iwork, iwsize);
    if(xpi != (double *) NULL)
      xpi += j;                 /* To next imaginary column */
  }
  y.jc[i] = jnz;
  mxAssert(jnz <= mxGetNzmax(Y_OUT),"");
/* ------------------------------------------------------------
   REALLOC: Shrink Y to its current size
   ------------------------------------------------------------ */
  jnz = MAX(jnz,1);
  if( (y.pr = (double *) mxRealloc(y.pr, jnz*sizeof(double))) == NULL)
    mexErrMsgTxt("Memory reallocation error");
  mxSetPr(Y_OUT,y.pr);
  if( (y.ir = (int *) mxRealloc(y.ir, jnz*sizeof(int))) == NULL)
    mexErrMsgTxt("Memory reallocation error");
  mxSetIr(Y_OUT,y.ir);
  mxSetNzmax(Y_OUT,jnz);
/* ------------------------------------------------------------
   Release working arrays
   ------------------------------------------------------------ */
  mxFree(sdpNL);
  mxFree(cwork);
  mxFree(iwork);
}