meschach_utils.c

JPEG2000实现的源码

   register int	i;

   for ( i = 0; i < len; i++ )
      dp1[i] += s*dp2[i];
}


/* hhvec -- calulates Householder vector to eliminate all entries after the
	i0 entry of the vector vec. It is returned as out. May be in-situ */
static VEC *hhvec(VEC *vec, int i0, double *beta, VEC *out, double *newval)
{
   double norm;

   out = _v_copy(vec,out,i0);
   norm = sqrt(_in_prod(out,out,i0));
   if ( norm <= 0.0 )
   {
      *beta = 0.0;
      return (out);
   }
   *beta = 1.0/(norm * (norm+fabs(out->ve[i0])));
   if ( out->ve[i0] > 0.0 )
      *newval = -norm;
   else
      *newval = norm;
   out->ve[i0] -= *newval;
   
   return (out);
}

/* hhtrcols -- transform a matrix by a Householder vector by columns
	starting at row i0 from column j0 -- in-situ */
static MAT *hhtrcols(MAT *M, int i0, int j0, VEC *hh, double beta)
{
   int i;
   static VEC *w = NULL;
   
   if ( M==(MAT *)NULL || hh==(VEC *)NULL )
      local_error("hhtrcols");
   if ( M->m != hh->dim )
      local_error("hhtrcols");
   if ( i0 > M->m || j0 > M->n )
      local_error("hhtrcols");

   if ( beta == 0.0 )	return (M);

   w = v_resize(w,M->n);
   v_zero(w);

   for ( i = i0; i < M->m; i++ )
      if ( hh->ve[i] != 0.0 )
	 __mltadd__(&(w->ve[j0]),&(M->me[i][j0]),hh->ve[i],
		    (int)(M->n-j0));
   for ( i = i0; i < M->m; i++ )
      if ( hh->ve[i] != 0.0 )
	 __mltadd__(&(M->me[i][j0]),&(w->ve[j0]),-beta*hh->ve[i],
		    (int)(M->n-j0));

   v_free(w); w = NULL;

   return (M);
}

/* hhtrrows -- transform a matrix by a Householder vector by rows
	starting at row i0 from column j0 -- in-situ */
static MAT *hhtrrows(MAT *M, int i0, int j0, VEC *hh, double beta)
{
   double ip, scale;
   int i;

   if ( M==(MAT *)NULL || hh==(VEC *)NULL )
      local_error("hhtrrows");
   if ( M->n != hh->dim )
      local_error("hhtrrows");
   if ( i0 > M->m || j0 > M->n )
      local_error("hhtrrows");
   
   if ( beta == 0.0 ) return (M);

   /* for each row ... */
   for ( i = i0; i < M->m; i++ )
   {	/* compute inner product */
      ip = __ip__(&(M->me[i][j0]),&(hh->ve[j0]),(int)(M->n-j0));
      scale = beta*ip;
      if ( scale == 0.0 )
	 continue;
      
      /* do operation */
      __mltadd__(&(M->me[i][j0]),&(hh->ve[j0]),-scale,
		 (int)(M->n-j0));
   }

   return (M);
}

/* hhtrvec -- apply Householder transformation to vector -- may be in-situ */
static VEC *hhtrvec(VEC *hh, double beta, int i0, VEC *in, VEC *out)
{
   double scale;

   if ( hh==(VEC *)NULL || in==(VEC *)NULL )
      local_error("hhtrvec");
   if ( in->dim != hh->dim )
      local_error("hhtrvec");
   if ( i0 > in->dim )
      local_error("hhtrvec");

   scale = beta*_in_prod(hh,in,i0);
   out = v_copy(in,out);
   __mltadd__(&(out->ve[i0]),&(hh->ve[i0]),-scale,(int)(in->dim-i0));

   return (out);
}

/* _set_col -- sets column of matrix to values given in vec (in situ) */
static MAT *_set_col(MAT *mat, int col, VEC *vec, int i0)
{
   int i,lim;
   
   if ( mat==(MAT *)NULL || vec==(VEC *)NULL )
      local_error("_set_col");
   if ( col >= mat->n )
      local_error("_set_col");
   lim = min(mat->m,vec->dim);
   for ( i=i0; i<lim; i++ )
      mat->me[i][col] = vec->ve[i];
   
   return (mat);
}

/* makeHQ -- construct the Hessenberg orthogonalising matrix Q;
	-- i.e. Hess M = Q.M.Q'	*/
static MAT *makeHQ(MAT *H, VEC *diag, VEC *beta, MAT *Qout)
{
   int i, j, limit;
   static VEC *tmp1 = NULL, *tmp2 = NULL;

   if ( H==(MAT *)NULL || diag==(VEC *)NULL || beta==(VEC *)NULL )
      local_error("makeHQ");
   limit = H->m - 1;
   if ( diag->dim < limit || beta->dim < limit )
      local_error("makeHQ");
   if ( H->m != H->n )
      local_error("makeHQ");
   Qout = m_resize(Qout,H->m,H->m);

   tmp1 = v_resize(tmp1,H->m);
   tmp2 = v_resize(tmp2,H->m);

   for ( i = 0; i < H->m; i++ )
   {
      /* tmp1 = i'th basis vector */
      for ( j = 0; j < H->m; j++ )
	 v_set_val(tmp1,j,0.0);
      v_set_val(tmp1,i,1.0);

      /* apply H/h transforms in reverse order */
      for ( j = limit-1; j >= 0; j-- )
      {
	 get_col(H,(int)j,tmp2);
	 v_set_val(tmp2,j+1,v_entry(diag,j));
	 hhtrvec(tmp2,beta->ve[j],j+1,tmp1,tmp1);
      }

      /* insert into Qout */
      set_col(Qout,(int)i,tmp1);
   }

   v_free(tmp1); tmp1 = NULL;
   v_free(tmp2); tmp2 = NULL;

   return (Qout);
}

/* givens -- returns c,s parameters for Givens rotation to
		eliminate y in the vector [ x y ]' */
static void givens(double x, double y, double *c, double *s)
{
   double norm;

   norm = sqrt(x*x+y*y);
   if ( norm == 0.0 )
   {	*c = 1.0;	*s = 0.0;	}	/* identity */
   else
   {	*c = x/norm;	*s = y/norm;	}
}

/* rot_cols -- postmultiply mat by givens rotation described by c,s */
static MAT *rot_cols(MAT *mat, int i, int k, double c, double s, MAT *out)
{
   int j;
   double temp;

   if ( mat==(MAT *)NULL )
      local_error("rot_cols");
   if ( i >= mat->n || k >= mat->n )
      local_error("rot_cols");
   out = m_copy(mat,out);

   for ( j=0; j<mat->m; j++ )
   {
      temp = c*m_entry(out,j,i) + s*m_entry(out,j,k);
      m_set_val(out,j,k, -s*m_entry(out,j,i) + c*m_entry(out,j,k));
      m_set_val(out,j,i,temp);
   }

   return (out);
}

/* Hfactor -- compute Hessenberg factorisation in compact form.
	-- factorisation performed in situ
	-- for details of the compact form see QRfactor.c and matrix2.doc */
static MAT *Hfactor(MAT *A, VEC *diag, VEC *beta)
{
   static VEC *tmp1 = NULL;
   int k, limit;

   if ( ! A || ! diag || ! beta )
      local_error("Hfactor");
   if ( diag->dim < A->m - 1 || beta->dim < A->m - 1 )
      local_error("Hfactor");
   if ( A->m != A->n )
      local_error("Hfactor");
   limit = A->m - 1;

   tmp1 = v_resize(tmp1,A->m);

   for ( k = 0; k < limit; k++ )
   {
      get_col(A,(int)k,tmp1);
      hhvec(tmp1,k+1,&beta->ve[k],tmp1,&A->me[k+1][k]);
      v_set_val(diag,k,v_entry(tmp1,k+1));
      hhtrcols(A,k+1,k+1,tmp1,v_entry(beta,k));
      hhtrrows(A,0  ,k+1,tmp1,v_entry(beta,k));
   }

   v_free(tmp1); tmp1 = NULL;

   return (A);
}

/* trieig -- finds eigenvalues of symmetric tridiagonal matrices
	-- matrix represented by a pair of vectors a (diag entries)
		and b (sub- & super-diag entries)
	-- eigenvalues in a on return */
static VEC *trieig(VEC *a, VEC *b, MAT *Q)
{
   int i, i_min, i_max, n, split;
   double *a_ve, *b_ve;
   double b_sqr, bk, ak1, bk1, ak2, bk2, z;
   double c, c2, cs, s, s2, d, mu;

   if ( ! a || ! b )
      local_error("trieig");
   if ( a->dim != b->dim + 1 || ( Q && Q->m != a->dim ) )
      local_error("trieig");
   if ( Q && Q->m != Q->n )
      local_error("trieig");

   n = a->dim;
   a_ve = a->ve; b_ve = b->ve;

   i_min = 0;
   while ( i_min < n )		/* outer while loop */
   {
      /* find i_max to suit;
	 submatrix i_min..i_max should be irreducible */
      i_max = n-1;
      for ( i = i_min; i < n-1; i++ )
	 if ( b_ve[i] == 0.0 )
	 {	i_max = i;	break;	}
      if ( i_max <= i_min )
      {
	 i_min = i_max + 1;
	 continue;	/* outer while loop */
      }

      /* repeatedly perform QR method until matrix splits */
      split = FALSE;
      while ( ! split )		/* inner while loop */
      {

	 /* find Wilkinson shift */
	 d = (a_ve[i_max-1] - a_ve[i_max])/2;
	 b_sqr = b_ve[i_max-1]*b_ve[i_max-1];
	 mu = a_ve[i_max] - b_sqr/(d + sgn(d)*sqrt(d*d+b_sqr));

	 /* initial Givens' rotation */
	 givens(a_ve[i_min]-mu,b_ve[i_min],&c,&s);
	 s = -s;
	 if ( fabs(c) < SQRT2 )
	 {	c2 = c*c;	s2 = 1-c2;	}
	 else
	 {	s2 = s*s;	c2 = 1-s2;	}
	 cs = c*s;
	 ak1 = c2*a_ve[i_min]+s2*a_ve[i_min+1]-2*cs*b_ve[i_min];
	 bk1 = cs*(a_ve[i_min]-a_ve[i_min+1]) +
	    (c2-s2)*b_ve[i_min];
	 ak2 = s2*a_ve[i_min]+c2*a_ve[i_min+1]+2*cs*b_ve[i_min];
	 bk2 = ( i_min < i_max-1 ) ? c*b_ve[i_min+1] : 0.0;
	 z  = ( i_min < i_max-1 ) ? -s*b_ve[i_min+1] : 0.0;
	 a_ve[i_min] = ak1;
	 a_ve[i_min+1] = ak2;
	 b_ve[i_min] = bk1;
	 if ( i_min < i_max-1 )
	    b_ve[i_min+1] = bk2;
	 if ( Q )
	    rot_cols(Q,i_min,i_min+1,c,-s,Q);

	 for ( i = i_min+1; i < i_max; i++ )
	 {
	    /* get Givens' rotation for sub-block -- k == i-1 */
	    givens(b_ve[i-1],z,&c,&s);
	    s = -s;
	    
	    /* perform Givens' rotation on sub-block */
	    if ( fabs(c) < SQRT2 )
	    {	c2 = c*c;	s2 = 1-c2;	}
	    else
	    {	s2 = s*s;	c2 = 1-s2;	}
	    cs = c*s;
	    bk  = c*b_ve[i-1] - s*z;
	    ak1 = c2*a_ve[i]+s2*a_ve[i+1]-2*cs*b_ve[i];
	    bk1 = cs*(a_ve[i]-a_ve[i+1]) +
	       (c2-s2)*b_ve[i];
	    ak2 = s2*a_ve[i]+c2*a_ve[i+1]+2*cs*b_ve[i];
	    bk2 = ( i+1 < i_max ) ? c*b_ve[i+1] : 0.0;
	    z  = ( i+1 < i_max ) ? -s*b_ve[i+1] : 0.0;
	    a_ve[i] = ak1;	a_ve[i+1] = ak2;
	    b_ve[i] = bk1;
	    if ( i < i_max-1 )
	       b_ve[i+1] = bk2;
	    if ( i > i_min )
	       b_ve[i-1] = bk;
	    if ( Q )
	       rot_cols(Q,i,i+1,c,-s,Q);
	 }

	 /* test to see if matrix should be split */
	 for ( i = i_min; i < i_max; i++ )
	    if ( fabs(b_ve[i]) < MACHEPS*
		 (fabs(a_ve[i])+fabs(a_ve[i+1])) )
	    {   b_ve[i] = 0.0;	split = TRUE;	}
      }
   }

   return a;
}

/* symmeig -- computes eigenvalues of a dense symmetric matrix
	-- A **must** be symmetric on entry
	-- eigenvalues stored in out
	-- Q contains orthogonal matrix of eigenvectors
	-- returns vector of eigenvalues */
VEC *symmeig(MAT *A, MAT *Q, VEC *out)
{
   int i;
   static MAT *tmp = NULL;
   static VEC *b = NULL, *diag = NULL, *beta = NULL;

   if ( ! A )
      local_error("symmeig");
   if ( A->m != A->n )
      local_error("symmeig");
   if ( ! out || out->dim != A->m )
      out = v_resize(out,A->m);

   tmp  = m_copy(A,tmp);
   b    = v_resize(b,A->m - 1);
   diag = v_resize(diag,(int)A->m);
   beta = v_resize(beta,(int)A->m);

   Hfactor(tmp,diag,beta);
   if ( Q )
      makeHQ(tmp,diag,beta,Q);

   for ( i = 0; i < A->m - 1; i++ )
   {
      out->ve[i] = tmp->me[i][i];
      b->ve[i] = tmp->me[i][i+1];
   }
   out->ve[i] = tmp->me[i][i];
   trieig(out,b,Q);

   m_free(tmp); tmp = NULL;
   v_free(b); b = NULL;
   v_free(diag); diag = NULL;
   v_free(beta); beta = NULL;

   return out;
}