dpr1fact.c

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

    colk = colperm[k];                     /* pointer into smult, firstpiv */
    betajc[k] = inz;
    mk = xsuper[k+1];
    betak = beta + inz;
    pk += xsuper[k];
    useperm = dodpr1fact(betak, perm, d, smult[colk], pk, mk, &nk, dep, pndep,
                         maxu, fwork, kdwork);
    ordered[k] = useperm;
    if(smult[colk] < 0.0)
      *pndep += findnewdep(dep,*pndep,maxndep,d);
/* ------------------------------------------------------------
   Forward solve on columns p(k+1:n)
   ------------------------------------------------------------ */
    if(smult[colk] != 0.0){
      if(useperm){
        for(j = k+1, pj = pk; j < n; j++){               /* with pivoting */
          pj += xsuper[j];
          if(firstpiv[colperm[j]] <= k)          /*Only if overlapping nzs*/
            fwipr1o(pj, perm, pk, betak, mk, nk);          /* o = ordered */
        }
        perm += mk;    /* full length permutation */
      }
      else
        for(j = k+1, pj = pk; j < n; j++){           /* without pivoting */
          pj += xsuper[j];
          if(firstpiv[colperm[j]] <= k)
            fwipr1(pj, pk, betak, mk, nk);
        }
    }
/* ------------------------------------------------------------
   Point to next column
   ------------------------------------------------------------ */
    inz += nk;
  }
/* ------------------------------------------------------------
   In total, we wrote inz <= length(p) nonzeros in beta.
   ------------------------------------------------------------ */
  betajc[n] = inz;
#ifdef DO_SUPER_SAFE
/* ------------------------------------------------------------
   If smult[i] < 0 for some i, then let dep = find(d<=0), and d(dep) = 0.
   Note: length(d) = m = xsuper[n].
   ------------------------------------------------------------ */
  mk = xsuper[n];
  inz = 0;
  for(j = 0; j < mk; j++)
    if(d[j] <= 0.0){
      d[j] = 0.0;
      dep[inz++] = j;
      mxAssert(inz <= maxndep, "Fatal numerical error in dpr1fact.");
    }
  *pndep = inz;
#endif
}

#define NLDEN_FIELDS 5
/* ============================================================
   MAIN: MEXFUNCTION
   ============================================================ */
/* ************************************************************
   PROCEDURE mexFunction - Entry for Matlab
   ************************************************************ */
void mexFunction(const int nlhs, mxArray *plhs[],
  const int nrhs, const mxArray *prhs[])
{
  mxArray *MY_FIELD;
  mxArray *myplhs[NPAROUT];
  int m,n,ndep,i,j, permj, pnnz, dznnz, permnnz;
  char *ordered;
  int *dep, *colperm, *invrowperm, *betajc, *pivperm, *firstpiv;
  double *beta, *d,*betajcPr, *pj, *orderedPr, *fwork, *p, *permPr, *lab;
  const double *colpermPr, *smult, *firstPr;
  const char *LdenFieldnames[] = {"betajc","beta","p","pivperm","dopiv"};
  keydouble *kdwork;
  double maxu;
  jcir x,dz;
/* ------------------------------------------------------------
   Check for proper number of arguments
   ------------------------------------------------------------ */
  mxAssert(nrhs >= NPARIN, "dpr1fact requires more input arguments");
  mxAssert(nlhs <= NPAROUT, "dpr1fact produces less output arguments");
/* ------------------------------------------------------------
   Get inputs (x, lab=d, smult, maxu)
   ------------------------------------------------------------ */
  m = mxGetM(X_IN);                              /* x */
  n = mxGetN(X_IN);
  mxAssert(mxIsSparse(X_IN), "x should be sparse.");
  x.jc = mxGetJc(X_IN);
  x.ir = mxGetIr(X_IN);
  x.pr = mxGetPr(X_IN);
  mxAssert( mxGetM(D_IN) * mxGetN(D_IN) == m, "Size mismatch d.");     /* d */
  mxAssert( mxGetM(SMULT_IN) * mxGetN(SMULT_IN) == n, "Size mismatch smult."); /* smult */
  smult = mxGetPr(SMULT_IN);
  maxu = mxGetScalar(MAXU_IN);                        /* maxu */
/* ------------------------------------------------------------
   DISASSEMBLE structure Lsymb.{dz,perm,first}
   ------------------------------------------------------------ */
  mxAssert(mxIsStruct(LSYMB_IN), "Lsymb should be a structure.");
  MY_FIELD = mxGetField(LSYMB_IN,0,"dz");      /* Lsymb.dz */
  mxAssert( MY_FIELD != NULL, "Missing field Lsymb.dz.");
  mxAssert(mxGetM(MY_FIELD) == m && mxGetN(MY_FIELD) == n, "Lsymb.dz size mismatch.");
  mxAssert(mxIsSparse(MY_FIELD), "Lsymb.dz must be sparse.");
  dz.jc = mxGetJc(MY_FIELD);
  dz.ir = mxGetIr(MY_FIELD);                             /* (rowperm) */
  MY_FIELD = mxGetField(LSYMB_IN,0,"perm");        /* Lsymb.perm */
  mxAssert(MY_FIELD != NULL, "Missing field Lsymb.perm.");
  mxAssert(mxGetM(MY_FIELD) * mxGetN(MY_FIELD) == n, "Size mismatch Lsymb.perm."); /* (colperm) */
  colpermPr = mxGetPr(MY_FIELD);
  MY_FIELD = mxGetField(LSYMB_IN,0,"first");        /* Lsymb.first */
  mxAssert( MY_FIELD != NULL, "Missing field Lsymb.first.");
  mxAssert( mxGetM(MY_FIELD) * mxGetN(MY_FIELD) == n, "Size mismatch Lsymb.first.");
  firstPr = mxGetPr(MY_FIELD);
/* ------------------------------------------------------------
   Let pnnz = sum(dz.jc), dznnz = dz.jc[n].
   ------------------------------------------------------------ */
  for(i = 1, pnnz = 0; i <= n; i++)
    pnnz += dz.jc[i];
  dznnz = dz.jc[n];
/* ------------------------------------------------------------
   Allocate working arrays:
   int: colperm(n), firstpiv(n), dep(m+1), betajc(n+1), pivperm(pnnz),
     invrowperm(m).
   char: ordered(n)
   double: fwork(dznnz), d(dznnz),
   keydouble: kdwork(dznnz).
   ------------------------------------------------------------ */
  firstpiv= (int *) mxCalloc(MAX(n,1), sizeof(int));
  colperm = (int *) mxCalloc(MAX(n,1), sizeof(int)); 
  dep     = (int *) mxCalloc(m+1, sizeof(int));
  betajc  = (int *) mxCalloc(n+1, sizeof(int));
  invrowperm = (int *) mxCalloc(MAX(m,1),sizeof(int));
  pivperm = (int *) mxCalloc(MAX(pnnz,1), sizeof(int));      /* pivperm */
  ordered = (char *) mxCalloc(MAX(n,1), sizeof(char));       /* boolean */
  fwork   = (double *) mxCalloc(MAX(dznnz,1), sizeof(double));   /* float */
  d = (double *) mxCalloc(MAX(dznnz,1), sizeof(double));
  kdwork  = (keydouble *) mxCalloc(MAX(dznnz,1), sizeof(keydouble)); /*(i,r)*/
/* ------------------------------------------------------------
   ALLOCATE vectors p(pnnz+m), beta(pnnz), .
   NB1: will be assigned to output vectors later.
   NB2: The +m for p is temporary. This will avoid memory problems when
   initializing p(invperm,:) = x, if Lsymb.dz is invalid.
   ------------------------------------------------------------ */
  p = (double *) mxCalloc(MAX(pnnz + m,1), sizeof(double));    /* p */
  beta = (double *) mxCalloc(MAX(pnnz,1), sizeof(double));      /* beta */
/* ------------------------------------------------------------
   Convert colperm and firstpiv  to integer
   ------------------------------------------------------------ */
  for(i = 0; i < n; i++){                 /* colperm(0:n-1) */
    j = colpermPr[i];
    colperm[i] = --j;
  }
  for(i = 0; i < n; i++){
    j = firstPr[i];
    firstpiv[i] = --j;
  }
/* ------------------------------------------------------------
   CREATE  OUTPUT vector lab := dOUT = dIN (duplicate)
   ------------------------------------------------------------ */
  D_OUT = mxDuplicateArray(D_IN);
  lab = mxGetPr(D_OUT);
/* ------------------------------------------------------------
   Let d(1:dznnz) = lab(dz.ir).
   ------------------------------------------------------------ */
  for(i = 0; i < dznnz; i++)
    d[i] = lab[dz.ir[i]];
/* ------------------------------------------------------------
   dep = [find(d<=0), m], ndep = length(find(d==0)
   ------------------------------------------------------------ */
  ndep = 0;
  for(i = 0; i < dznnz; i++)              /* dep = find(d <= 0) */
    if(d[i] <= 0.0)
      dep[ndep++] = i;
  dep[ndep] = m;           /* tail of dep */
/* ------------------------------------------------------------
   Let invrowperm(dz.ir) = 0:dznnz-1, where dznnz = dz.jc[n] <= m
   ------------------------------------------------------------ */
  mxAssert(dznnz <= m,"");
  for(i = 0; i < dznnz; i++)
    invrowperm[dz.ir[i]] = i;
/* ------------------------------------------------------------
   Let p(invrowperm,:) = x(:,colperm)
   ------------------------------------------------------------ */
  for(j = 0, pj = p; j < n; j++){
    pj += dz.jc[j];
    permj = colperm[j];
    for(i = x.jc[permj]; i < x.jc[permj+1]; i++)
      pj[invrowperm[x.ir[i]]] = x.pr[i];
  }
/* ------------------------------------------------------------
   Create output structure Lden
   ------------------------------------------------------------ */
  LDEN_OUT = mxCreateStructMatrix(1, 1, NLDEN_FIELDS, LdenFieldnames);
/* ------------------------------------------------------------
   Create LDEN.P(pnnz), and realloc p to the size it should have, i.e. pnnz
   ------------------------------------------------------------ */
  MY_FIELD = mxCreateDoubleMatrix(pnnz, 1, mxREAL);
  mxSetField(LDEN_OUT, 0,"p", MY_FIELD);
  if(pnnz > 0){
    mxFree(mxGetPr(MY_FIELD));
    if((p = (double *) mxRealloc(p, pnnz * sizeof(double))) == NULL)
      mexErrMsgTxt("Memory allocation error");
    mxSetPr(MY_FIELD, p);
  }
  else
    mxFree(p);
/* ------------------------------------------------------------
   The actual job is done here:
   Adding n rank-1 updates, with a multiple smult(1:n).
   ------------------------------------------------------------ */
  prodformfact(p, pivperm, beta, betajc, d, ordered, dz.jc, colperm,
               firstpiv, smult, n, dep, &ndep, maxu, fwork, kdwork);
/* ------------------------------------------------------------
   THE DIAGONAL IS PERMUTED BACK:
   Bring d back in original ordering: lab(dz.ir) = d(1:dznnz).
   ------------------------------------------------------------ */
  for(i = 0; i < dznnz; i++)
    lab[dz.ir[i]] = d[i];
/* ------------------------------------------------------------
   Let permnnz = sum{dz.jc[j] | ordered[j]==1}, and set
   Lden.pivperm = pivperm (int to double, but C-form)
   ------------------------------------------------------------ */
  for(i = 0, permnnz = 0; i < n; i++)
    permnnz += ordered[i] * dz.jc[i+1];
  mxAssert(permnnz <= pnnz, "");
  MY_FIELD = mxCreateDoubleMatrix(permnnz, 1, mxREAL);
  mxSetField(LDEN_OUT, 0,"pivperm", MY_FIELD);
  permPr = mxGetPr(MY_FIELD);
  for(i = 0; i < permnnz; i++)
    permPr[i] = pivperm[i];                 /* int to double */
/* ------------------------------------------------------------
   Create LDEN.BETAJC(n+1)
   ------------------------------------------------------------ */
  MY_FIELD = mxCreateDoubleMatrix(n + 1, 1, mxREAL);
  mxSetField(LDEN_OUT, 0,"betajc", MY_FIELD);
  betajcPr = mxGetPr(MY_FIELD);
  for(i = 0; i <= n; i++){
    j = betajc[i];
    betajcPr[i] = ++j;
  }
/* ------------------------------------------------------------
   Create LDEN.BETA(betajc[n])
   ------------------------------------------------------------ */
  MY_FIELD = mxCreateDoubleMatrix(betajc[n], 1, mxREAL);
  mxSetField(LDEN_OUT, 0,"beta", MY_FIELD);
  if(betajc[n] > 0){
    mxFree(mxGetPr(MY_FIELD));
    if((beta = (double *) mxRealloc(beta, betajc[n] * sizeof(double))) == NULL)
      mexErrMsgTxt("Memory allocation error");
    mxSetPr(MY_FIELD, beta);
  }
  else
    mxFree(beta);
/* ------------------------------------------------------------
   Create LDEN.DOPIV(n)
   ------------------------------------------------------------ */
  MY_FIELD = mxCreateDoubleMatrix(n, 1, mxREAL);
  mxSetField(LDEN_OUT, 0,"dopiv", MY_FIELD);
  orderedPr = mxGetPr(MY_FIELD);
  for(i = 0; i < n; i++)
    orderedPr[i] = ordered[i];
/* ------------------------------------------------------------
   Release working arrays
   ------------------------------------------------------------ */
  mxFree(kdwork);
  mxFree(d);
  mxFree(fwork);
  mxFree(ordered);
  mxFree(pivperm);
  mxFree(invrowperm);
  mxFree(betajc);
  mxFree(dep);
  mxFree(colperm);
  mxFree(firstpiv);
/* ------------------------------------------------------------
   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]);
}