urotorder.c

matlab中uwb波形优化算法经常会使用的工具包:SeDuMi_1_1R3.

/* ------------------------------------------------------------
   Pivot on column pivk, and make U(:,perm)
   upper-triangular by pivk - k givens rotations on U(:,perm(k:n)).
   Givens at row i is {u(i,j), norm( u(i+1:pivk,j) )} for
   j=perm[pivk] and i = k:pivk-1.
   Thus, the rotation gi consists of 1 complex and 1 real entry,
   (gi.x,gi.xim) and gi.y, resp. The rotation is [conj(x), y;y, -x]
   ------------------------------------------------------------ */
      m = pivk - k;                    /* number of Givens rotations needed */
      j = perm[pivk];                  /* uj(1:m) should become 0 */
      uj = rowuk + j * n;
      ujpi = rowukpi + j * n;
      nexty = uj[m];                   /* last nonzero in col uj (real) */
      y = SQR(nexty);
      for(i = m-1; i >= 0; i--){
        gi.x = uj[i];
        gi.xim = ujpi[i];
        gi.y = nexty;
        y += SQR(gi.x) + SQR(gi.xim);
        nexty = sqrt(y);
        gi.x /= nexty;         /* Normalize to rotation [conj(x),y; y,-x] */
        gi.xim /= nexty;
        gi.y /= nexty;
        g[i] = gi;
      }                                /* y == d[j] after loop */
      uj[0] = nexty;                   /* New pivotal diagonal entry */
/* ------------------------------------------------------------
   move pivot j=perm[pivk] to head of perm (shifting old k:pivk-1)
   ------------------------------------------------------------ */
      memmove(perm+k+1, perm+k, m * sizeof(int));     /* move 1-> */
      perm[k] = j;                     /* inserted at k */
/* ------------------------------------------------------------
   Apply rotations to columns perm(k+1:n-1).
   Apply 1,2,...,m rotations on column k+1,..,k+m=pivk,
   and m rotations on cols pivk+1:n-1.
   ------------------------------------------------------------ */
      for(i = 1; i <= m; i++)
        prpigivensrotuj(rowuk + perm[k+i] * n,rowukpi + perm[k+i] * n, g,i);
      for(i += k; i < n; i++)
        prpigivensrot(rowuk + perm[i] * n,rowukpi + perm[i] * n, g,m);
      inz += m;                         /* point to next avl. place */
      g += m;
/* ------------------------------------------------------------
   Update d(perm(k+1:n)) -= u(k,perm(k+1:n)).^2.
   ------------------------------------------------------------ */
      for(j = k + 1; j < n; j++){
        i = perm[j];
        d[i] -= SQR(rowuk[i * n]) + SQR(rowukpi[i * n]);
      }
    }
  }
/* ------------------------------------------------------------
   We have reordered n-1 columns of U using inz Givens-rotations.
   ------------------------------------------------------------ */
  mxAssert(n > 0,"");
  gjc[n-1] = inz;
}

/* ============================================================
   MAIN: MEXFUNCTION
   ============================================================ */
/* ************************************************************
   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]);
}