mexschurold.c

这事很不错的半定规划matlab软件,this is the important software of SDP.

/****************************************************************************************
* mexschur.c : C mex file to compute  
*          
*    mexschur(blk,Avec,nzlistA1,nzlistA2,permA,U,V,colend,type,schur);  
*
*    schur(I,J) = schur(I,J) + Trace(Ai U Aj V),
*    where I = permA[i], J = permA[j],   1<=i,j<=colend. 
* 
*   input:  blk  = 1x2 a cell array describing the block structure of A.
*           Avec =  
*           nzlistA = 
*           permA = a permutation vector.  
*           U,V  = real symmetric matrices.
*           type = 0, compute Trace(Ai*(U Aj V + V Aj U)/2)) = Trace(Ai*(U Aj V))
*                = 1, compute Trace(Ai*(U Aj U)).
*
* SDPT3: version 3.0
* Copyright (c) 1997 by
* K.C. Toh, M.J. Todd, R.H. Tutuncu
* Last Modified: 2 Feb 01   
****************************************************************************************/

#include <math.h>
#include <mex.h>

#if !defined(MAX)
#define  MAX(A, B)   ((A) > (B) ? (A) : (B))
#endif

#if !defined(r2)
#define  r2   1.41421356237309504880      /* sqrt(2) */
#endif

#if !defined(ir2)
#define  ir2  0.70710678118654752440      /* 1/sqrt(2) */
#endif

/**********************************************************
*  compute  Trace(B U*A*U)
*
*  A,B are assumed to be real,sparse,symmetric.
*  U  is assumed to be real,dense,symmetric. 
**********************************************************/
void schurij1( int n, 
               double *Avec, int *idxstart, int *nzlistAi, int *nzlistAj,
               double *U, int col, double *schurcol)

{ int    i, ra, ca, rb, cb, rbn, cbn, l, k, kstart, kend, lstart, lend; 
  double tmp1, tmp2, tmp3, tmp4; 

  lstart = idxstart[col]; lend = idxstart[col+1]; 

  for (i=0; i<=col; i++) {
     kstart = idxstart[i]; kend = idxstart[i+1]; 
     tmp1 = 0; tmp2 = 0;  
     for (l=lstart; l<lend; ++l) { 
        rb = nzlistAi[l];    
        cb = nzlistAj[l];
        if (rb > cb) { mexErrMsgTxt("mexschur: nzlistA2 is incorrect"); }
        rbn = rb*n; cbn = cb*n;   
        tmp3 = 0; tmp4 = 0;
        for (k=kstart; k<kend; ++k) { 
           ra = nzlistAi[k];
           ca = nzlistAj[k];             
           if (ra<ca) {  
              tmp3 += Avec[k] * (U[ra+rbn]*U[ca+cbn]+U[ra+cbn]*U[ca+rbn]); }
           else { 
              tmp4 += Avec[k] * (U[ra+rbn]*U[ca+cbn]); }
	}
        if (rb<cb) { tmp1 += Avec[l]*(ir2*tmp3 + tmp4); }
        else       { tmp2 += Avec[l]*(ir2*tmp3 + tmp4); } 
     }
     schurcol[i] = r2*tmp1+tmp2; 
  }
return;
}
/**********************************************************
*  compute  Trace(B (U*A*V + V*A*U)/2) = Trace(B U*A*V)
*
*  A,B are assumed to be real,sparse,symmetric.
*  U,V are assumed to be real,dense,symmetric. 
**********************************************************/
void schurij3(int n,  
              double *Avec, int *idxstart, int *nzlistAi, int *nzlistAj,
              double *U, double *V, int col, double *schurcol)

{ int    ra, ca, rb, cb, rbn, cbn, l, k, idx1, idx2, idx3, idx4;
  int    i, kstart, kend, lstart, lend; 
  double tmp1, tmp2, tmp3, tmp4; 

  lstart = idxstart[col]; lend = idxstart[col+1]; 

  for (i=0; i<=col; i++) {
     kstart = idxstart[i]; kend = idxstart[i+1]; 
     tmp1 = 0; tmp2 = 0;  
     for (l=lstart; l<lend; ++l) { 
        rb = nzlistAi[l];    
        cb = nzlistAj[l];
        if (rb > cb) { mexErrMsgTxt("mexschur: nzlistA2 is incorrect"); }
        rbn = rb*n; cbn = cb*n;   
        tmp3 = 0; tmp4 = 0; 
        for (k=kstart; k<kend; ++k) { 
           ra = nzlistAi[k];
           ca = nzlistAj[k];
           idx1 = ra+rbn; idx2 = ca+cbn;
           if (ra<ca) { 
              idx3 = ra+cbn; idx4 = ca+rbn; 
	      tmp3 += Avec[k] *(U[idx1]*V[idx2]+U[idx2]*V[idx1] \
                                +U[idx3]*V[idx4]+U[idx4]*V[idx3]);  }
           else {
	      tmp4 += Avec[k] * (U[idx1]*V[idx2]+U[idx2]*V[idx1]);  }
	}
        if (rb<cb) { tmp1 += Avec[l]*(ir2*tmp3+tmp4); }
        else       { tmp2 += Avec[l]*(ir2*tmp3+tmp4); } 
     }
     schurcol[i] = ir2*tmp1+tmp2/2; 
  }
return; 
}
/**********************************************************
* stack multiple blocks into a long column vector
**********************************************************/
void vec(int numblk, int *cumblksize, int *blknnz, 
         double *A, int *irA, int *jcA, double *B) 

{  int idx0, idx, i, j, l, jstart, jend, istart, blksize;
   int k, kstart, kend; 
   
      for (l=0; l<numblk; l++) { 
  	  jstart = cumblksize[l]; 
  	  jend   = cumblksize[l+1];  
          blksize = jend-jstart; 
          istart = jstart;
          idx0 = blknnz[l]; 
          for (j=jstart; j<jend; j++) { 
              idx = idx0 + (j-jstart)*blksize; 
              kstart = jcA[j]; kend = jcA[j+1]; 
              for (k=kstart; k<kend; k++) { 
                  i = irA[k];
                  B[idx+i-istart] = A[k]; }
          }
      }  
return;
}
/**********************************************************
*  compute  Trace(B U*A*U)
*
*  A,B are assumed to be real,sparse,symmetric.
*  U  is assumed to be real,sparse,symmetric. 
**********************************************************/
void schurij2( double *Avec, 
               int *idxstart, int *nzlistAi, int *nzlistAj, double *Utmp,
               int *nzlistAr, int *nzlistAc, int *cumblksize, 
               int *blkidx, int col, double *schurcol)

{ int    r, ra, ca, rb, cb, l, k, kstart, kend, kstartnew, lstart, lend;
  int    colcb1, idxrb, idxcb, idx1, idx2, idx3, idx4;
  int    i, cblk, calk, firstime; 
  double tmp0, tmp1, tmp2, tmp3, tmp4; 

  lstart = idxstart[col]; lend = idxstart[col+1]; 

  for (i=0; i<=col; i++) { 
      kstart = idxstart[i]; kend = idxstart[i+1]; 
      kstartnew = kstart;
      tmp1 = 0; tmp2 = 0; 
      for (l=lstart; l<lend; ++l) { 
          rb = nzlistAi[l];    
          cb = nzlistAj[l];
          cblk = blkidx[cb];  
          idxcb = nzlistAc[l]; 
          idxrb = nzlistAr[l];
          tmp3 = 0; tmp4 = 0; firstime = 1; 
          for (k=kstart; k<kend; ++k) { 
              ca = nzlistAj[k];
              calk = blkidx[ca]; 
              if (calk==cblk) {
                 ra = nzlistAi[k];
                 idx1 = ra+idxrb; idx2 = ca+idxcb; 
                 if (ra<ca) {  
                    idx3 = ra+idxcb; idx4 = ca+idxrb; 
                    tmp3 += Avec[k] * (Utmp[idx1]*Utmp[idx2]+Utmp[idx3]*Utmp[idx4]); }
                 else {
	            tmp4 += Avec[k] * (Utmp[idx1]*Utmp[idx2]); }
                 if (firstime) { kstartnew = k; firstime = 0; } 
	      }
              else if (calk > cblk) {
	         break;
              }
	  }
          kstart = kstartnew; 
          if (rb<cb) { tmp1 += Avec[l]*(ir2*tmp3 + tmp4); }
          else       { tmp2 += Avec[l]*(ir2*tmp3 + tmp4); } 
      }
      tmp0 = r2*tmp1+tmp2; 
      schurcol[i] = tmp0; 
  }
return;
}
/**********************************************************
*  compute  Trace(B (U*A*V + V*A*U)/2) = Trace(B U*A*V)
*
*  A,B are assumed to be real,sparse,symmetric.
*  U,V are assumed to be real,sparse,symmetric. 
**********************************************************/
void schurij4( double *Avec, 
               int *idxstart, int *nzlistAi, int *nzlistAj,
               double *Utmp, double *Vtmp, 
               int *nzlistAr, int *nzlistAc, int *cumblksize, 
               int *blkidx, int col, double *schurcol)

{ int    r, ra, ca, rb, cb, l, k, kstart, kend, kstartnew, lstart, lend;
  int    colcb1, idxrb, idxcb, idx1, idx2, idx3, idx4; 
  int    i, cblk, calk, firstime;
  double tmp0, tmp1, tmp2, tmp3, tmp4;
  double hlf=0.5;  

  lstart = idxstart[col]; lend = idxstart[col+1];

  for (i=0; i<=col; i++) {
      kstart = idxstart[i]; kend = idxstart[i+1]; 
      kstartnew = kstart;
      tmp1 = 0; tmp2 = 0;  
      for (l=lstart; l<lend; ++l) { 
          rb = nzlistAi[l];    
          cb = nzlistAj[l];
          cblk = blkidx[cb];  
          idxcb = nzlistAc[l]; 
          idxrb = nzlistAr[l];
          tmp3 = 0; tmp4 = 0; firstime = 1; 
          for (k=kstart; k<kend; ++k) { 
              ca = nzlistAj[k];
              calk = blkidx[ca];