pmatlib.c

压缩文件中是Error Correction Coding - Mathematical Methods and Algorithms(Wiley 2005)作者:(Todd K. Moon )的配

/*****************************************************************/
void plubksubf(float ** a,int n,int *indx,float *b)
{
   int i,ii=-1,ip,j;
   float sum;
   
   for (i=0;i<n;i++) {
      pf = a[i];
      ip=indx[i];
      sum=b[ip];
      b[ip]=b[i];
      if (ii>= 0)
	 for (j=ii;j<=i-1;j++) sum -= pf[j]*b[j];
      else if (sum) ii=i;
      b[i]=sum;
   }
   for (i=n-1;i>=0;i--) {
      pf = a[i];
      sum=b[i];
      for (j=i+1;j<n;j++) sum -= pf[j]*b[j];
      b[i]=sum/pf[i];
   }
}

void plubksublf(double ** a,int n,int *indx,double *b)
{
   int i,ii=-1,ip,j;
   double sum;
   
   for (i=0;i<n;i++) {
      pd = a[i];
      ip=indx[i];
      sum=b[ip];
      b[ip]=b[i];
      if (ii>= 0)
	 for (j=ii;j<=i-1;j++) sum -= pd[j]*b[j];
      else if (sum) ii=i;
      b[i]=sum;
   }
   for (i=n-1;i>=0;i--) {
      sum=b[i];
      pd = a[i];
      for (j=i+1;j<n;j++) sum -= pd[j]*b[j];
      b[i]=sum/pd[i];
   }
}



/*****************************************************************/
/* perform a cholesky factorization of the positive definite symmetric */
/*  matrix A, A = LL' , L a */
/* lower triangular matrix */
/* See Burden and Faires, third ed. p. 351 */
void pcholf(float **A, float **L, int n)
{
   int i,j,k;
   float temp;
   float l;
   
   if(A[0][0] < 0) {
      FILE *fo;
      fprintf(stderr,
	      "Warning: sqrt domain error (0) in Cholesky: %f\n",A[0][0]);
      fo = fout;
      fout = stderr;
      pmatprintf("A",A,n,n);
      fout = fo;
   }
   L[0][0] = sqrt(A[0][0]);	/* step 1 */
   /* clear out the upper triangle */
   for(i = 0; i < n; i++) {
      pf = L[i];
      for(j = i+1; j < n; j++)
	 pf[j] = 0;
   }
   
   l = L[0][0];
   for(j = 1; j < n; j++) {	/* step 2 */
      L[j][0] = A[j][0]/l;
   }
   /* step 3 */
   for(i = 1; i < n-1; i++) {	/* step 4 */
      temp = 0;
      for(k = 0,  pf = L[i]; k <= i-1; k++,pf++) {
	 temp += *pf * *pf;
      }
      temp = A[i][i] - temp;
      if(temp < 0) {
	 FILE *fo;
	 fprintf(stderr,
		 "Warning: sqrt domain error (1) in Cholesky: %f\n",temp);
	 fo = fout;
	 fout = stderr;
	 pmatprintf("A",A,n,n);
	 fout = fo;
      }
      L[i][i] = sqrt(temp);
      
      for(j = i+1; j < n; j++) { /* step 5 */
	 temp = 0;
	 for(k = 0,pf=L[j],pf1=L[i]; k <= i-1; k++,pf++,pf1++) {
	    temp += *pf1 * *pf;
	 }
	 L[j][i] = 1./L[i][i]*(A[j][i] - temp);
      }
   } /* end step 3 */
   
   temp = 0;			/* step 6 */
   for(k = 0,pf = L[n-1]; k < n -1 ; k++,pf++) {
      temp += *pf * *pf;
   }
   temp = A[n-1][n-1] - temp;
   if(temp < 0) {
      fprintf(stderr,"Warning: sqrt domain error in (2) Cholesksy: %f\n",temp);
   }
   L[n-1][n-1] = sqrt(temp);
}



void pchollf(double **A, double **L, int n)
{
   int i,j,k;
   double temp;
   double l;
   
   if(A[0][0] < 0) {
      FILE *fo;
      fprintf(stderr,
	      "Warning: sqrt domain error (0) in Cholesky: %lf\n",A[0][0]);
      fo = fout;
      fout = stderr;
      pmatprintlf("A",A,n,n);
      fout = fo;
   }
   L[0][0] = sqrt(A[0][0]);	/* step 1 */
   for(i = 0; i < n; i++) {
      pd = L[i];
      for(j = i+1; j < n; j++)
	 pd[j] = 0;
   }
   
   l = L[0][0];
   for(j = 1; j < n; j++) {	/* step 2 */
      L[j][0] = A[j][0]/l;
   }
   /* step 3 */
   for(i = 1; i < n-1; i++) {	/* step 4 */
      temp = 0;
      for(k=0,pd=L[i] ; k <= i-1; k++,pd++) {
	 temp += *pd * *pd;
      }
      temp = A[i][i] - temp;
      if(temp < 0) {
	 fprintf(stderr,
		 "Warning: sqrt domain error (1) in Cholesky: %f\n",temp);

      }
      L[i][i] = sqrt(temp);
      for(j = i+1; j < n; j++) { /* step 5 */
	 temp = 0;
	 for(k=0,pd=L[j]; k <= i-1; k++,pd++) {
	    temp += L[j][k]*L[i][k];     /* *pd * *pd; */
	 }
	 L[j][i] = 1./L[i][i]*(A[j][i] - temp);
      }
   } /* end step 3 */
   
   temp = 0;			/* step 6 */
   for(k=0,pd=L[n-1]; k < n-1 ; k++,pd++) {
      temp += *pd * *pd;
   }
   temp = A[n-1][n-1] - temp;
   if(temp < 0) {
      FILE *fo;
      fprintf(stderr,"Warning: sqrt domain error in (2) Cholesksy: %f\n",temp);
      fo = fout;
      fout = stderr;
      pmatprintlf("A",A,n,n);
      fout = fo;
   }
   L[n-1][n-1] = sqrt(temp);
}


/*********************************************************
Computes the  LDL decomposition for a symmetric matrix
 (Algorithm lifted from Golub, 2nd ed. pp. 137-138.)

 Factor A so that A = L*diag(D)*L'

Where:
  A is the matrix.
  L is the lower-triangular component.
  D is the diagonal component.
  n is the dimension of the matrix.
  Return value:
  (none).
  *********************************************************/
void pLDLlf( double **A, double **L, double *D, int n )
{
   int i, k, p;
   
   TEMPVEC(tvec1lf,n,mv1lf,double);
   
   if ( n == 1 ) {
      L[0][0] = 1.0;
      D[0] = A[0][0];
      return;
   }
   tvec1lf[0] = 0.0;
   for ( i = 0; i < n; i++ ) {
      for( k = i,pd = (L[i]+k); k < n; k++,pd++)
	 *pd = (i == k) ? 1.0 : 0.0;
   }
   for ( k = 0; k < n; k++ ) {
      pd = L[k];
      for ( p = 0; p < k; p++ )
	 tvec1lf[p] = D[p] * pd[p];
      D[k] = A[k][k];
      for ( p = 0; p < k; p++ )
	 D[k] -= pd[p] * tvec1lf[p];
      if ( D[k] < THRESH ) {
	 pmaterr("LDL error");
      }
      for ( i = k + 1; i < n; i++ ) {
	 pd = L[i];
	 pd[k] = A[i][k];
	 for (p = 0; p < k; p++ )
	    pd[k] -= pd[p] * tvec1lf[p];
	 pd[k] /= D[k];
      }
   }
}

void pLDLf( float **A,float **L, float *D, int n )
{
   int i, k, p;
   
   TEMPVEC(tvec1f,n,mv1f,float);
   
   if ( n == 1 ) {
      L[0][0] = 1.0;
      D[0] = A[0][0];
      return;
   }
   tvec1f[0] = 0.0;
   for ( i = 0; i < n; i++ ) {
      for( k = i,pf = (L[i]+k); k < n; k++,pf++)
	 *pf = (i == k) ? 1.0 : 0.0;
   }
   for ( k = 0; k < n; k++ ) {
      pf = L[k];
      for ( p = 0; p < k; p++ )
	 tvec1f[p] = D[p] * pf[p];
      D[k] = A[k][k];
      for ( p = 0; p < k; p++ )
	 D[k] -= pf[p] * tvec1f[p];
      if ( D[k] < THRESH ) {
	 pmaterr("LDL error");
      }
      for ( i = k + 1; i < n; i++ ) {
	 pf = L[i];
	 pf[k] = A[i][k];
	 for (p = 0; p < k; p++ )
	    pf[k] -= pf[p] * tvec1f[p];
	 pf[k] /= D[k];
      }
   }
}


/***********************************************************************/
/* SVD factorization: a = u diag(w) v' */

void psvdlf(double **a,	/* matrix to factor */
	       double **u,	/* u */
	       double *w,	/* diagonal matrix of singular values */
	       double **v,	/* v */
	       int m,int n)	/* rows and columns of a */
{
   int flag,i,its,j,jj,k,l,nm;
   double c,f,h,s,x,y,z;
   double anorm=0.0,g=0.0,scale=0.0;
   
   if (m < n) pmaterr("SVD: You must augment A with extra zero rows");
   
   TEMPVEC(tvec1lf,m,mv1lf,double);

   l = 1;
   nm = 0;
   for(i = 0; i < m; i++) 
      for(j = 0,pd = u[i],pd1=a[i]; j < n; j++,pd++,pd1++)
	 *pd = *pd1;
   for (i=0;i<n;i++) {
      l=i+1;
      tvec1lf[i]=scale*g;
      g=s=scale=0.0;
      if (i < m) {
	 for (k=i;k<m;k++) scale += fabs(u[k][i]);
	 if (scale) {
	    for (k=i;k<m;k++) {
	       u[k][i] /= scale;
	       s += u[k][i]*u[k][i];
	    }
	    f=u[i][i];
	    g = -SVDSIGN(sqrt(s),f);
	    h=f*g-s;
	    u[i][i]=f-g;
	    if (i != n-1) {
	       for (j=l;j<n;j++) {
		  for (s=0.0,k=i;k<m;k++) s += u[k][i]*u[k][j];
		  f=s/h;
		  for (k=i;k<m;k++) u[k][j] += f*u[k][i];
	       }
	    }
	    for (k=i;k<m;k++) u[k][i] *= scale;
	 }
      }
      w[i]=scale*g;
      g=s=scale=0.0;
      if (i < m && i != n-1) {
	 for (k=l;k<n;k++) scale += fabs(u[i][k]);
	 if (scale) {
	    for (k=l;k<n;k++) {
	       u[i][k] /= scale;
	       s += u[i][k]*u[i][k];
	    }
	    f=u[i][l];
	    g = -SVDSIGN(sqrt(s),f);
	    h=f*g-s;
	    u[i][l]=f-g;
	    for (k=l;k<n;k++) tvec1lf[k]=u[i][k]/h;
	    if (i != m-1) {
	       for (j=l;j<m;j++) {
		  for (s=0.0,k=l;k<n;k++) s += u[j][k]*u[i][k];
		  for (k=l;k<n;k++) u[j][k] += s*tvec1lf[k];
	       }
	    }
	    for (k=l;k<n;k++) u[i][k] *= scale;
	 }
      }
      anorm=SVDMAX(anorm,(fabs(w[i])+fabs(tvec1lf[i])));
   }
   for (i=n-1;i>=0;i--) {
      if (i < n-1) {
	 if (g) {
	    for (j=l;j<n;j++)
	       v[j][i]=(u[i][j]/u[i][l])/g;
	    for (j=l;j<n;j++) {
	       for (s=0.0,k=l;k<n;k++) s += u[i][k]*v[k][j];
	       for (k=l;k<n;k++) v[k][j] += s*v[k][i];
	    }
	 }
	 for (j=l;j<n;j++) v[i][j]=v[j][i]=0.0;
      }
      v[i][i]=1.0;
      g=tvec1lf[i];
      l=i;
   }
   for (i=n-1;i>=0;i--) {
      l=i+1;
      g=w[i];
      if (i < n-1)
	 for (j=l;j<n;j++) u[i][j]=0.0;
      if (g) {
	 g=1.0/g;
	 if (i != n-1) {
	    for (j=l;j<n;j++) {
	       for (s=0.0,k=l;k<m;k++) s += u[k][i]*u[k][j];
	       f=(s/u[i][i])*g;
	       for (k=i;k<m;k++) u[k][j] += f*u[k][i];
	    }
	 }
	 for (j=i;j<m;j++) u[j][i] *= g;
      } else {
	 for (j=i;j<m;j++) u[j][i]=0.0;
      }
      ++u[i][i];
   }
   for (k=n-1;k>=0;k--) {
      for (its=1;its<=30;its++) {
	 flag=1;
	 for (l=k;l>=0;l--) {
	    nm=l-1;
	    if (fabs(tvec1lf[l])+anorm == anorm) {
	       flag=0;
	       break;
	    }
	    if (fabs(w[nm])+anorm == anorm) break;
	 }
	 if (flag) {
	    c=0.0;
	    s=1.0;
	    for (i=l;i<=k;i++) {
	       f=s*tvec1lf[i];
	       if (fabs(f)+anorm != anorm) {
		  g=w[i];
		  h=SVDPYTHAG(f,g);
		  w[i]=h;
		  h=1.0/h;
		  c=g*h;
		  s=(-f*h);
		  for (j=0;j<m;j++) {
		     y=u[j][nm];
		     z=u[j][i];
		     u[j][nm]=y*c+z*s;
		     u[j][i]=z*c-y*s;
		  }
	       }
	    }
	 }
	 z=w[k];
	 if (l == k) {
	    if (z < 0.0) {
	       w[k] = -z;
	       for (j=0;j<n;j++) v[j][k]=(-v[j][k]);
	    }
	    break;
	 }
	 if (its == 30) pmaterr("No convergence in 30 SVDCMP iterations");
	 x=w[l];
	 nm=k-1;
	 y=w[nm];
	 g=tvec1lf[nm];
	 h=tvec1lf[k];
	 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
	 g=SVDPYTHAG(f,1.0);
	 f=((x-z)*(x+z)+h*((y/(f+SVDSIGN(g,f)))-h))/x;
	 c=s=1.0;
	 for (j=l;j<=nm;j++) {
	    i=j+1;
	    g=tvec1lf[i];
	    y=w[i];
	    h=s*g;
	    g=c*g;
	    z=SVDPYTHAG(f,h);
	    tvec1lf[j]=z;
	    c=f/z;
	    s=h/z;
	    f=x*c+g*s;
	    g=g*c-x*s;
	    h=y*s;
	    y=y*c;
	    for (jj=0;jj<n;jj++) {
	       x=v[jj][j];
	       z=v[jj][i];
	       v[jj][j]=x*c+z*s;
	       v[jj][i]=z*c-x*s;
	    }
	    z=SVDPYTHAG(f,h);
	    w[j]=z;
	    if (z) {
	       z=1.0/z;
	       c=f*z;
	       s=h*z;
	    }
	    f=(c*g)+(s*y);
	    x=(c*y)-(s*g);
	    for (jj=0;jj<m;jj++) {
	       y=u[jj][j];
	       z=u[jj][i];
	       u[jj][j]=y*c+z*s;
	       u[jj][i]=z*c-y*s;
	    }
	 }
	 tvec1lf[l]=0.0;
	 tvec1lf[k]=f;
	 w[k]=x;
      }
   }
}




/* factor a = u diag(w) v' */
void psvdf(float **a,	/* matrix to factor */
	      float **u,	/* u */
	      float *w,		/* diagonal matrix of singular values */
	      float **v,	/* v */
	      int m,int n)	/* rows and columns of a */
{
   int flag,i,its,j,jj,k,l,nm;
   float c,f,h,s,x,y,z;
   float anorm=0.0,g=0.0,scale=0.0;
   
   if (m < n) pmaterr("SVDCMP: You must augment A with extra zero rows");

   TEMPVEC(tvec1f,m,mv1f,float);

   l = 1;
   nm = 0;
   for(i = 0; i < m; i++) for(j = 0; j < n; j++) u[i][j] = a[i][j];
   for (i=0;i<n;i++) {
      l=i+1;
      tvec1f[i]=scale*g;
      g=s=scale=0.0;
      if (i < m) {
	 for (k=i;k<m;k++) scale += fabs(u[k][i]);
	 if (scale) {
	    for (k=i;k<m;k++) {
	       u[k][i] /= scale;
	       s += u[k][i]*u[k][i];
	    }
	    f=u[i][i];
	    g = -SVDSIGN(sqrt(s),f);
	    h=f*g-s;
	    u[i][i]=f-g;
	    if (i != n-1) {
	       for (j=l;j<n;j++) {
		  for (s=0.0,k=i;k<m;k++) s += u[k][i]*u[k][j];
		  f=s/h;
		  for (k=i;k<m;k++) u[k][j] += f*u[k][i];
	       }
	    }
	    for (k=i;k<m;k++) u[k][i] *= scale;
	 }
      }
      w[i]=scale*g;
      g=s=scale=0.0;
      if (i < m && i != n-1) {
	 for (k=l;k<n;k++) scale += fabs(u[i][k]);
	 if (scale) {
	    for (k=l;k<n;k++) {
	       u[i][k] /= scale;
	       s += u[i][k]*u[i][k];
	    }
	    f=u[i][l];
	    g = -SVDSIGN(sqrt(s),f);
	    h=f*g-s;
	    u[i][l]=f-g;
	    for (k=l;k<n;k++) tvec1f[k]=u[i][k]/h;
	    if (i != m-1) {
	       for (j=l;j<m;j++) {
		  for (s=0.0,k=l;k<n;k++) s += u[j][k]*u[i][k];
		  for (k=l;k<n;k++) u[j][k] += s*tvec1f[k];
	       }
	    }
	    for (k=l;k<n;k++) u[i][k] *= scale;
	 }