getada3.c

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

/* ************************************************************
%                            [ADA,absd] = getada3(ADA, A,Ajc1,Aord, udsqr,K)
% GETADA3  Compute ADA(i,j) = (D(d^2)*A.t(:,i))' *A.t(:,j),
%   and exploit sparsity as much as possible.
%   absd - length m output vector, containing
%     absd(i) = abs((D(d^2)*A.t(:,i))' *abs(A.t(:,i)).
%     Hence, diag(ADA)./absd gives a measure of cancelation (in [0,1]).
%
% SEE ALSO sedumi, getada1, getada2
% ******************** INTERNAL FUNCTION OF SEDUMI ********************
  
   function [ADA,absd] = getada3(ADA, A,Ajc1,Aord, udsqr,K)

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA

   ************************************************************ */

#include <string.h>
#include <math.h>
#include "mex.h"
#include "blksdp.h"

#define ADA_OUT myplhs[0]
#define ABSD_OUT myplhs[1]
#define NPAROUT 2

#define ADA_IN prhs[0]       /* sparsity struct ADA */
#define AT_IN prhs[1]        /* N x m sparse At */
#define AJC1_IN prhs[2]      /* start of PSD blocks in At */
#define AORD_IN prhs[3]
#define UDSQR_IN prhs[4]     /* Scale data */
#define K_IN  prhs[5]
#define NPARIN 6

/* ========================= G E N E R A L ========================= */

void spzeros(double *x,const int *xir,const int n)
{
  int i;
  for(i = 0; i < n; i++)
    x[xir[i]] = 0.0;
}

/* ************************************************************
   PROCEDURE exmerge - mergeing 2 exclusive, increasing integer arrays.
   INPUT
     x - length nx array, increasing entries.
     y - length ny array, its entries are increasing, and do not occur in x.
     nx,ny - order of x and y.
   OUTPUT
     z - length nx+ny vector
   WORK
     cwork - length ny
     iwork - length ny+2+floor(log_2(1+ny)).
   ************************************************************ */
void exmerge(int *x, const int *y, const int nx, const int ny,
             const int iwsize, char *cwork, int *ipos)
{
  int i,j,inz;
/* ------------------------------------------------------------
   Search all "insertion" positions of y-entries in list x
   ------------------------------------------------------------ */
  intmbsearch(ipos,cwork, x,nx,y,ny, ipos+ny+2,iwsize-(ny+2));
/* ------------------------------------------------------------
   Shift x-entries down to make room for y-entries
   ------------------------------------------------------------ */
  for(i = ny; i > 0; i--){         /* shift i entries down */
    inz = ipos[i];
    if((j = ipos[i+1]-inz) > 0)
      memmove(x+inz+i,x+inz,j*sizeof(int));
  }
/* ------------------------------------------------------------
   Insert y-entries
   ------------------------------------------------------------ */
  ++ipos;
  for(i = 0; i < ny; i++)
    x[ipos[i]+i] = y[i];          /* add i, number prev. inserted y's */
#ifndef NDEBUG
  for(i = 1; i < nx+ny; i++)
    mxAssert(x[i] > x[i-1],"");
#endif
}

/* ************************************************************
   PROCEDURE cpspdiag -- Let d := diag(X), where X is sparse m x m.
   INPUT
     m - number of columns in ada
     x - contains COMPLETE SYMMETRIC sparsity structure,
   OUTPUT
     d - length m vector, diag(X).
   ************************************************************ */
void cpspdiag(double *d, const jcir x, const int m)
{
  int j, inz;
  const int *diagFound;
  
/* ------------------------------------------------------------
   For each column j: let dj = x(j,j)
   ------------------------------------------------------------ */
  for(j = 0; j < m; j++){
    if((diagFound = ibsearch(&j,x.ir+x.jc[j],x.jc[j+1]-x.jc[j])) != NULL){
      inz = diagFound - x.ir;                 /* convert pointer to index */
      d[j] = x.pr[inz];                       /* diag entry found */
    }
    else
      d[j] = 0.0;                   /* no diag entry -> d[j] = 0.0 */
  }
}

/* ************************************************************
   PROCEDURE spmakesym -- Let X := X+X'-diag(diag(X)), with X a
      real sparse square matrix.
   INPUT
     m - number of columns in ada
   UPDATED
     x - on input, contains COMPLETE SYMMETRIC sparsity structure,
         and the values (possibly 0's on some locations). On return,
         X = X+X'-diag(diag(X)), i.e. X is symmetrized, and the off-
         diagonal entries are "DUPLICATED" (allowing part in xij and xji).
   WORK
     iwork - length m array of integers. Points to "below row j"
       part of columns (trilstart). (initial contents irrelevant)
   ************************************************************ */
void spmakesym(jcir x, const int m, int *iwork)
{
  int i, j, inz, jend;
  double xij;
  
/* ------------------------------------------------------------
   Initialize: let iwork(0:m-1) = ada.jc(0:m-1)
   ------------------------------------------------------------ */
  memcpy(iwork, x.jc, m * sizeof(int));   /* don't copy x.jc[m] */
/* ------------------------------------------------------------
   For each column j:   for each index i > j:
   let xij = x(i,j) + x(j,i).
   Let iwork point to next nonzero in col i
   Guard: x.ir[iwork(i)] >= j for all i >= j.
   ------------------------------------------------------------ */
  for(j = 0; j < m; j++){
    jend = x.jc[j+1];                        /* Let [inz,jend) be tril(:,j) */
    inz = iwork[j];
    if(inz < jend){
      if(x.ir[inz] == j)                         /* skip diagonal entry */
        inz++;
      while(inz < jend){                         /* off-diagonal entries */
        i = x.ir[inz];
        xij = x.pr[iwork[i]] + x.pr[inz];  /* xji + xij */
        x.pr[iwork[i]++] = xij;
        x.pr[inz++] = xij;
      }
    }
  }
}

/* ************************************************************
   PROCEDURE dzblkpartit
   INPUT
     dzir - length dznnz array, NOT sorted
     xblk - length max(dzir) array, xblk(dzir) maps into 0:nblk-1.
     dznnz - order of dzir
     nblk - 1+max(xblk), number of blocks
   OUTPUT
     dzjc - length nblk+1 array. Has blockstarts so that all subscripts
       in dzir fit in the resulting partition.
   ************************************************************ */
void dzblkpartit(int *dzjc, const int *dzir, const int *xblk,
                 const int dznnz, const int nblk)
{
  int i,j;
/* ------------------------------------------------------------
   Init dzjc = all-0
   ------------------------------------------------------------ */
  for(i = 0; i <= nblk; i++)
    dzjc[i] = 0;
/* ------------------------------------------------------------
   Pre-partition dzir(dznnz) space into blocks
   ------------------------------------------------------------ */
  ++dzjc;
  for(i = 0; i < dznnz; i++)
    dzjc[xblk[dzir[i]]]++;      /* accumulate nnz */
  j = 0;
  for(i = 0; i < nblk; i++){    /* cumsum */
    j += dzjc[i];
    dzjc[i] = j;
  }
  mxAssert(dzjc[nblk-1] == dznnz,"");
}    


/* ************************************************************
   PROCEDURE: getada3
   INPUT
     ada.{jc,ir} - sparsity structure of ada.
     At - sparse N x m matrix.
     udsqr - lenud vector containing D, D(ud.perm,ud.perm) = Ud'*Ud.
     Ajc1 - m int array, Ajc1(:,1) points to start of PSD nz's in At.
     dzjc - psdN+1, partition of dz rowsubscipts into PSD blocks.
     dzstructjc, dzstructir - sparse N x m matrix, giving NEW PSD-nonzero
       positions of At(:,perm(j)).
     blkstart - length(K.s): starting indices of PSD  blocks
     xblk - length psdDim array, with k = xblk(i-blkstart[0]) iff
       blkstart[k] <= i < blkstart[k+1], k=0:nblk-1.
       psdDim:=blkstart[end]-blkstart[0].
     psdNL - K.s in integer
     perm, invperm - length(m) array, ordering in which ADA should be computed,
       and its inverse. We compute in order triu(ADA(perm,perm)), but store
       at original places. OPTIMAL order: start with sparsest dzstruct.
     m  - order of ADA, number of constraints.
     lenud - blkstart[end] - blkstart[0], PSD dimension.
     pcK - pointer to cone K structure.
     rpsdN - sum(K.s(i) | i is real sym PSD block).
   UPDATED
     ada.pr - ada(i,j) += ai'*D(d^2; PSD)*aj. PSD-part. Only entries
       in triu(ADA(perm,perm) are affected.
   OUTPUT
     absd - length(m) vector, contains abs(aj)'*abs(D(d^2)*aj).
   WORKING ARRAYS
     fwork - work vector, size
         fwsiz = lenud + 2 * max(rMaxn^2, 2*hMaxn^2).
     iwork - integer work array, size 
         iwsiz = 2*psdN + dznnz + max(srchsize, max(nk(PSD))).
	 where dznnz = dzstructjc[m] and
         srchsize = 2+maxadd+floor(log(1+maxadd))
     cwork - maxadd, where maxadd = max(dzstructjc(i+1)-dzstructjc(i))
   ************************************************************ */
void getada3(jcir ada, double *absd, jcir At, const double *udsqr,
             const int *Ajc1, const int *dzjc,
             const int *dzstructjc, const int *dzstructir,
             const int *blkstart, const int *xblk, const int *psdNL,
             const int *perm, const int *invperm,
             const int m, const int lenud, const coneK *pcK,
             double *fwork, int fwsiz, int *iwork, int iwsiz,
             char *cwork)
{
  int i,j,k, knz,inz, dznnz, addnnz, permj, rsdpN, nblk, nnzbj;
  double *daj;
  double adaij, termj, absadajj;
  int *dzknnz, *dzir, *blksj;

  rsdpN = pcK->rsdpN;
/* ------------------------------------------------------------
   Partition working arrays
   int: dzknnz(psdN=length(K.s)), blksj(psdN), dzir(dznnz = dzstructjc[m]),
     iwork[iwsiz],
     with iwsiz = max(dznnz, max(nk(PSD))).
   double:    daj(lenud), fwork[fwsiz],
     with fwsiz = 2 * max(rMaxn^2, 2*hMaxn^2).
   ------------------------------------------------------------ */
  nblk = pcK->sdpN;
  dznnz = dzstructjc[m];
  if(dznnz <= 0)
    return;                                 /* nothing to do if no PSD*/
  dzknnz = iwork;                           /* dzknnz(nblk) */
  blksj = dzknnz + nblk;                    /* blksj(nblk) */
  dzir = blksj + nblk;                     /* dzir(dznnz) */
  iwork  = dzir + dznnz;                    /* iwork(iwsiz) */
  iwsiz -= 2*nblk + dznnz;