dpr1fact.c

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

  int ndep, n, i, j, nph2, nextj, idep;
  double psqrdep, h;
  double *mu;
  char deldep;

/* ------------------------------------------------------------
   If t = 0, then factor diag(d)+0*p*p' = I*diag(d)*I, i.e. beta=0.
   ------------------------------------------------------------ */
  if(t == 0.0){
    *pn = 0;              /* number of nonzeros in beta */
    return 0;
  }
/* ------------------------------------------------------------
   t is nonzero, replace by tnew := 1/t.
   We've to factor diag(d) + p*p' / tnew.
   ------------------------------------------------------------ */
  t = 1/t;
  ndep = *pndep;
/* ------------------------------------------------------------
   Use beta temporarily as mu(1:m), which lists max(psqr(i+1:m)).
   mu will be used only to select stable pivots, before writing beta.
   ------------------------------------------------------------ */
  mu = beta;
/* ------------------------------------------------------------
   Let psqr = p(1:m).^2
   ------------------------------------------------------------ */
  realHadamard(psqr, p, p, m);
/* ------------------------------------------------------------
   Case A: d(1:mk) > 0 (no dep). Then n = m.
   ------------------------------------------------------------ */
  if(dep[0] >= m){
    *pn = m;
/* ------------------------------------------------------------
   Let mu(m) = 0, mu(i) = max(psqr(i+1:mk)), for i=1:mk-1.
   ------------------------------------------------------------ */
    for(h = 0.0, i = m - 1; i >= 0; i--){
      mu[i] = h;
      h = MAX(h, psqr[i]);
    }
/* ------------------------------------------------------------
   1st round: pivot sequentially on 1:m, skipping instable ones.
   ------------------------------------------------------------ */
    nph2 = dpr1fact(psqr, d, kdwork, &t, m, mu, maxu);
/* ------------------------------------------------------------
   Write results 1st round: beta = p ./ psqr.
   ------------------------------------------------------------ */
    if(!nph2){                  /* all 1:m handled */
      realHadadiv(beta, p, psqr, m);
      return 0;
    }
    else{                       /* skipped kdwork.k */
      for(i = 0, j = 0; i < nph2; i++){
        nextj = (kdwork+i)->k;
        fromto(perm+j, j, nextj);       /* perm[j-i:nextj-i] = j:nextj */
        realHadadiv(beta + j, p + j, psqr + j, nextj - j);
        j = nextj + 1;              /* skip nextj == (kdwork+i)->k */
        --perm; --beta;             /* keep j valid index */
      }
      fromto(perm+j, j, m);       /* perm[j-i:nextj-i] = j:nextj */
      realHadadiv(beta + j, p + j, psqr + j, m - j);
      perm += m;           /* point just behind accepted pivots */
      beta += m;
/* ------------------------------------------------------------
   Sort rejected nodes in decreasing order of p.^2.
   ------------------------------------------------------------ */
      kdsortdec(kdwork, nph2);
/* ------------------------------------------------------------
   2nd round factorization: ordered.
   ------------------------------------------------------------ */
      ph2dpr1fact(kdwork, d, &t, nph2);
      for(i = 0; i < nph2; i++){
        j = (kdwork+i)->k;
        perm[i] = j;
        beta[i] = p[j] / (kdwork+i)->r;
      }
      return 1;
    } /* if nph2 > 0 */
  } /* if !dep */
/* ------------------------------------------------------------
   If d(1:mk) is NOT positive:
   Let (j,psqrdep) = max{psqr(i) | d(i)==0.0, i=1:m}
   ------------------------------------------------------------ */
  else{
    psqrdep = 0.0;
    for(i = 0; dep[i] < m; i++)
      if(psqr[dep[i]] > psqrdep){
        j = i;
        psqrdep = psqr[dep[i]];
      }
    mxAssert(i <= ndep, "");
/* ------------------------------------------------------------
   Threshold h = maxu^2 * psqrdep
   If all psqr>h have been factorized, we'll pivot on dep[k], if
   t * psqrdep > 0 (otherwise we view this as being zero).
   ------------------------------------------------------------ */
    if(psqrdep > 0.0){        /* we'll remove dependency at idep=dep[j] */
      idep = dep[j];
/* ------------------------------------------------------------
   If psqrdep>0, we can remove dependency idep=dep[j].
   Let dep[j:ndep-1] = dep[j+1:ndep] (incl tail dep[ndep]), then
   let dep[ndep] = idep, and --ndep. For Lorentz cones, removed
   dependencies may get dependent again at the t=-1 step.
   ------------------------------------------------------------ */
      if(t > 0.0){
        deldep = 1;
        memmove(dep+j, dep+j+1, (ndep - j) * sizeof(int));
        h = SQR(maxu) * psqrdep;
        dep[ndep] = idep;                /* remember removed dependency */
        *pndep = --ndep;
      }
/* ------------------------------------------------------------
   If we're subtracting a rank-1 factor (t<0), then psqrdep should
   be zero (up to rounding errors)
   ------------------------------------------------------------ */
      else{                  /* D - p*p' should be psd, so */
        h = psqrdep;         /* we've to round [0,psqrdep] to 0 */
        deldep = 0;
      }
    }
    else{
      idep = dep[0];           /* psqr(dep) == 0: remains dependent */
      h = 0.0;
      deldep = 0;
    }
/* ------------------------------------------------------------
   PARTITION: perm = [find(psqr > h), idep, remainder].
   Then let n be j = length(find(psqr > h)).
   Temporarily use nph2 = m-length(remainder).
   ------------------------------------------------------------ */
    for(i = 0, j = 0, nph2 = m; i < idep; i++)
      if(psqr[i] > h)
        perm[j++] = i;
      else
        perm[--nph2] = i;
    for(++i; i < m; i++)               /* skip over i = idep */
      if(psqr[i] > h)
        perm[j++] = i;
      else
        perm[--nph2] = i;
    mxAssert(j == nph2-1,"");
    perm[j] = idep;                     /* finally insert idep */
    n = j;                       /* length(find(psqr > h)) */
    *pn = j + deldep;            /* cardinality of beta */
/* ------------------------------------------------------------
   Now h=max(psqr(perm(n+1:m))).
   Let mu(i) = max(psqr(perm(i+1:m))).
   ------------------------------------------------------------ */
    for(i = n - 1; i >= 0; i--){
      mu[i] = h;
      h = MAX(h, psqr[perm[i]]);
    }
/* ------------------------------------------------------------
   1st round: pivot sequentially on perm(1:n), skipping instable ones.
   The stable pivots are re-alligned at start of perm.
   ------------------------------------------------------------ */
    nph2 = dpr1factperm(psqr, d, kdwork, &t, perm, n, mu, maxu);
/* ------------------------------------------------------------
   Write results 1st round: beta = p(perm(1:n-nph2)) ./ psqr(perm(1:n-nph2)).
   ------------------------------------------------------------ */
    n -= nph2;          /* cardinality 1st round */
    for(i = 0; i < n; i++){
      j = perm[i];
      beta[i] = p[j] / psqr[j];
    }
    perm += n;         /* handled 1st round */
    beta += n;
/* ------------------------------------------------------------
   Sort rejected nodes in decreasing order of p.^2.
   ------------------------------------------------------------ */
    if(nph2){
      kdsortdec(kdwork, nph2);
/* ------------------------------------------------------------
   2nd round factorization: ordered.
   ------------------------------------------------------------ */
      ph2dpr1fact(kdwork, d, &t, nph2);
      for(i = 0; i < nph2; i++){
        j = (kdwork+i)->k;
        perm[i] = j;
        beta[i] = p[j] / (kdwork+i)->r;
      }
    }
/* ------------------------------------------------------------
   If psqrdep > 0, we can now finish off the factorization by
   pivoting on idep == perm[nph2]:
   d_new(i) = p_i^2/t, beta = 1/p_i.
   ------------------------------------------------------------ */
    if(deldep){
      d[idep] = psqr[idep] / t;
      beta[nph2] = 1.0 / p[idep];
    }
  }
  return 1;
}

/* ************************************************************
   PROCEDURE findnewdep - CAUTION: this searches only over previously
     removed dependencies. The rank reduction could however have happened
     elsewehere, viz. last pivot location!!
   INPUT
     ndep    - Number of dependent nodes, d[dep[0:ndep-1]] == 0.
     maxndep - dep is length maxndep+1. dep[ndep+1:maxndep] are previously
          removed dependencies.
     d       - length m vector, m = dep[ndep].
   UPDATED
     dep - length maxndep+1 array. If d[dep[i]] <= 0 for some i > ndep,
       then dep[i] is inserted into dep(0:ndep), so that dep(0:ndep+1) remains
       sorted.
   RETURNS 1 if ndep has to be incremented, i.e. an entry of
     dep(ndep+1:maxndep) is inserted into dep(0:ndep). Otherwise returns 0.
   ************************************************************ */
int findnewdep(int *dep, const int ndep, const int maxndep, const double *d)
{
  int i, j, idep;

  for(i = ndep + 1; i <= maxndep; i++)
    if(d[dep[i]] <= 0.0)
      break;
  if(i <= maxndep){
    idep = dep[i];
    j = 0;
    intbsearch(&j, dep, ndep, idep);  /* first j s.t. dep[j] > idep */
    memmove(dep+j+1, dep+j, (i - j) * sizeof(int));
    dep[j] = idep;
    return 1;
  }
  else
    return 0;
}

/* ============================================================
   PRODFORMFACT does a dpr1fact for each rank-1 update.
   ============================================================ */

/* ************************************************************
   PROCEDURE prodformfact
   INPUT
     xsuper - column k consists of rows 0:xsuper(k+1)-1.
     n      - number of (dense) columns
     smult  - Length n vector. the k-th step adds (D+smult(k)*pk*pk').
     firstpiv - Length n array, first affecting pivot.
     colperm - Length n array, column permutation for smult and firstpiv.
     maxu   - max_k(max abs(Lk)) will be at most maxu. Rows may be
      reordered to achieve this.
   UPDATED
     p  - Length(p) = sum(xsuper). On input, contains the dense columns
       as in X = diag(d) + P*diag(smult(colperm))*P'. On output, a
       product-form forward solve has been made to p(:,2:n).
     d  - length xsuper[n] nonnegative vector. On input, the diagonal w/o dense
       columns. On output, the diagonal in the final product form Cholesky.
     dep - Length ndep+1 list of entries where d(i)=0; dep(0) < dep(1)...;
        dep[ndep] = xsuper[n], the tail.
     pndep  - length of dep, may be decreased on output, if dependencies
       are removed by adding the rank-1 updates..
   OUTPUT
     perm - sum_j(xsuper(j+1)|ordered(j)=1) array, contains a stable pivot
       ordering for those columns where ordered[j]=1.
     beta   - Length length(p). Such that L_k = eye(m) + tril(pk * betak, -1).
     betajc - Length n+1. start of betak. nnz(beta) <= nnz(p).
     ordered - length n. Ordered[j]==1 iff the rows of column j are
       reordered for numerical stability (controled by maxu).
   WORK
     fwork  - length xsuper[n] float working array.
     kdwork - length xsuper[n] (i,r)-working array.
   ************************************************************ */
void prodformfact(double *p, int *perm, double *beta, int *betajc,
                  double *d, char *ordered, const int *xsuper,
                  const int *colperm, const int *firstpiv,
                  const double *smult, const int n, int *dep, int *pndep,
                  const double maxu, double *fwork, keydouble *kdwork)
{
  int k, colk, mk, nk, j, inz, maxndep;
  double *betak, *pk, *pj;
  char useperm;
/* ------------------------------------------------------------
   Initialize. inz points to next avl. place in beta,
   perm is used to store pivot ordering,
   ------------------------------------------------------------ */
  inz = 0;
  maxndep = *pndep;
/* ------------------------------------------------------------
   For all columns k, mk = length(pk), nk = length(betak).
   ------------------------------------------------------------ */
  for(k = 0, pk = p; k < n; k++){