bdfactor.c

Meschach 可以解稠密或稀疏线性方程组、计算特征值和特征向量和解最小平方问题

   -- returns out^T <- x^T + s.y^T.bA
   -- if out is NULL then create out (as zero vector)
   -- error if either bA or x is NULL */
#ifndef ANSI_C
VEC	*vbd_mltadd(x,y,bA,s,out)
     BAND	*bA;
     VEC	*x, *y;
     double	s;
     VEC *out;
#else
VEC	*vbd_mltadd(const VEC *x, const VEC *y, const BAND *bA,
		    double s, VEC *out)
#endif
{
  int	i, j;

  if ( ! bA || ! x || ! y )
    error(E_NULL,"vbd_mltadd");
  if ( bA->mat->n != x->dim || y->dim != x->dim )
    error(E_SIZES,"vbd_mltadd");
  if ( ! out || out->dim != x->dim )
    out = v_resize(out,x->dim);
  out = v_copy(x,out);

  for ( j = 0; j < x->dim; j++ )
    for ( i = max(j-bA->ub,0); i <= j+bA->lb && i < x->dim; i++ )
      out->ve[j] += s*bd_get_val(bA,i,j)*y->ve[i];

  return out;
}

/* bd_zero -- zeros band matrix A which is returned */
#ifndef ANSI_C
BAND	*bd_zero(A)
BAND	*A;
#else
BAND	*bd_zero(BAND *A)
#endif
{
  if ( ! A )
    error(E_NULL,"bd_zero");

  m_zero(A->mat);
  return A;
}

/* bds_mltadd -- returns OUT <- A+alpha*B
	-- OUT is created (as zero) if NULL
	-- if OUT is not the correct size, it is re-sized before the operation
	-- if A or B are null, and error is generated */
#ifndef ANSI_C
BAND	*bds_mltadd(A,B,alpha,OUT)
BAND	*A, *B, *OUT;
Real	alpha;
#else
BAND	*bds_mltadd(const BAND *A, const BAND *B, double alpha, BAND *OUT)
#endif
{
  int	i;

  if ( ! A || ! B )
    error(E_NULL,"bds_mltadd");
  if ( A->mat->n != B->mat->n )
    error(E_SIZES,"bds_mltadd");
  if ( A == OUT || B == OUT )
    error(E_INSITU,"bds_mltadd");

  OUT = bd_copy(A,OUT);
  OUT = bd_resize(OUT,max(A->lb,B->lb),max(A->ub,B->ub),A->mat->n);
  for ( i = 0; i <= B->lb + B->ub; i++ )
    __mltadd__(OUT->mat->me[i+OUT->lb-B->lb],B->mat->me[i],alpha,B->mat->n);
  
  return OUT;
}

/* sbd_mlt -- returns OUT <- s.A */
#ifndef ANSI_C
BAND	*sbd_mlt(Real s, BAND *A, BAND *OUT)
#else
BAND	*sbd_mlt(Real s, const BAND *A, BAND *OUT)
#endif
{
  if ( ! A )
    error(E_NULL,"sbd_mlt");

  OUT = bd_resize(OUT,A->lb,A->ub,A->mat->n);
  sm_mlt(s,A->mat,OUT->mat);

  return OUT;
}

/* bdLUfactor -- gaussian elimination with partial pivoting
   -- on entry, the matrix A in band storage with elements 
      in rows 0 to lb+ub; 
      The jth column of A is stored in the jth column of 
      band A (bA) as follows:
      bA->mat->me[lb+j-i][j] = A->me[i][j] for 
      max(0,j-lb) <= i <= min(A->n-1,j+ub);
   -- on exit: U is stored as an upper triangular matrix
      with lb+ub superdiagonals in rows lb to 2*lb+ub, 
      and the matrix L is stored in rows 0 to lb-1.
      Matrix U is permuted, whereas L is not permuted !!!
      Therefore we save some memory.
   */
#ifndef ANSI_C
BAND	*bdLUfactor(bA,pivot)
BAND	*bA;
PERM	*pivot;
#else
BAND	*bdLUfactor(BAND *bA, PERM *pivot)
#endif
{
   int	i, j, k, l, n, n1, lb, ub, lub, k_end, k_lub;
   int	i_max, shift;
   Real	**bA_v;
   Real max1, temp;
   
   if ( bA==(BAND *)NULL || pivot==(PERM *)NULL )
     error(E_NULL,"bdLUfactor");

   lb = bA->lb;
   ub = bA->ub;
   lub = lb+ub;
   n = bA->mat->n;
   n1 = n-1;
   lub = lb+ub;

   if ( pivot->size != n )
     error(E_SIZES,"bdLUfactor");

   
   /* initialise pivot with identity permutation */
   for ( i=0; i < n; i++ )
     pivot->pe[i] = i;

   /* extend band matrix */
   /* extended part is filled with zeros */
   bA = bd_resize(bA,lb,min(n1,lub),n);
   bA_v = bA->mat->me;


   /* main loop */

   for ( k=0; k < n1; k++ )
   {
      k_end = max(0,lb+k-n1);
      k_lub = min(k+lub,n1);

      /* find the best pivot row */
      
      max1 = 0.0;	
      i_max = -1;
      for ( i=lb; i >= k_end; i-- ) {
	 temp = fabs(bA_v[i][k]);
	 if ( temp > max1 )
	 { max1 = temp;	i_max = i; }
      }
      
      /* if no pivot then ignore column k... */
      if ( i_max == -1 )
	continue;
      
      /* do we pivot ? */
      if ( i_max != lb )	/* yes we do... */
      {
	 /* save transposition using non-shifted indices */
	 shift = lb-i_max;
	 px_transp(pivot,k+shift,k);
	 for ( i=lb, j=k; j <= k_lub; i++,j++ )
	 {
	    temp = bA_v[i][j];
	    bA_v[i][j] = bA_v[i-shift][j];
	    bA_v[i-shift][j] = temp;
	 }
      }
      
      /* row operations */
      for ( i=lb-1; i >= k_end; i-- ) {
	 temp = bA_v[i][k] /= bA_v[lb][k];
	 shift = lb-i;
	 for ( j=k+1,l=i+1; j <= k_lub; l++,j++ )
	   bA_v[l][j] -= temp*bA_v[l+shift][j];
      }
   }
   
   return bA;
}


/* bdLUsolve -- given an LU factorisation in bA, solve bA*x=b */
/* pivot is changed upon return  */
#ifndef ANSI_C
VEC	*bdLUsolve(bA,pivot,b,x)
BAND	*bA;
PERM	*pivot;
VEC	*b,*x;
#else
VEC	*bdLUsolve(const BAND *bA, PERM *pivot, const VEC *b, VEC *x)
#endif
{
   int i,j,l,n,n1,pi,lb,ub,jmin, maxj;
   Real c;
   Real **bA_v;

   if ( bA==(BAND *)NULL || b==(VEC *)NULL || pivot==(PERM *)NULL )
     error(E_NULL,"bdLUsolve");
   if ( bA->mat->n != b->dim || bA->mat->n != pivot->size)
     error(E_SIZES,"bdLUsolve");
 
   lb = bA->lb;
   ub = bA->ub;
   n = b->dim;
   n1 = n-1;
   bA_v = bA->mat->me;

   x = v_resize(x,b->dim);
   px_vec(pivot,b,x);

   /* solve Lx = b; implicit diagonal = 1 
      L is not permuted, therefore it must be permuted now
    */
   
   px_inv(pivot,pivot);
   for (j=0; j < n; j++) {
      jmin = j+1;
      c = x->ve[j];
      maxj = max(0,j+lb-n1);
      for (i=jmin,l=lb-1; l >= maxj; i++,l--) {
	 if ( (pi = pivot->pe[i]) < jmin) 
	   pi = pivot->pe[i] = pivot->pe[pi];
	 x->ve[pi] -= bA_v[l][j]*c;
      }
   }

   /* solve Ux = b; explicit diagonal */

   x->ve[n1] /= bA_v[lb][n1];
   for (i=n-2; i >= 0; i--) {
      c = x->ve[i];
      for (j=min(n1,i+ub), l=lb+j-i; j > i; j--,l--)
	c -= bA_v[l][j]*x->ve[j];
      x->ve[i] = c/bA_v[lb][i];
   }
   
   return (x);
}

/* LDLfactor -- L.D.L' factorisation of A in-situ;
   A is a band matrix
   it works using only lower bandwidth & main diagonal
   so it is possible to set A->ub = 0
 */
#ifndef ANSI_C
BAND *bdLDLfactor(A)
BAND *A;
#else
BAND *bdLDLfactor(BAND *A)
#endif
{
   int i,j,k,n,n1,lb,ki,jk,ji,lbkm,lbkp;
   Real **Av;
   Real c, cc;

   if ( ! A )
     error(E_NULL,"bdLDLfactor");

   if (A->lb == 0) return A;

   lb = A->lb;
   n = A->mat->n;
   n1 = n-1;
   Av = A->mat->me;
   
   for (k=0; k < n; k++) {    
      lbkm = lb-k;
      lbkp = lb+k;

      /* matrix D */
      c = Av[lb][k];
      for (j=max(0,-lbkm), jk=lbkm+j; j < k; j++, jk++) {
	 cc = Av[jk][j];
	 c -= Av[lb][j]*cc*cc;
      }
      if (c == 0.0)
	error(E_SING,"bdLDLfactor");
      Av[lb][k] = c;

      /* matrix L */
      
      for (i=min(n1,lbkp), ki=lbkp-i; i > k; i--,ki++) {
	 c = Av[ki][k];
	 for (j=max(0,i-lb), ji=lb+j-i, jk=lbkm+j; j < k;
	      j++, ji++, jk++)
	   c -= Av[lb][j]*Av[ji][j]*Av[jk][j];
	 Av[ki][k] = c/Av[lb][k];
      }
   }
   
   return A;
}

/* solve A*x = b, where A is factorized by 
   Choleski LDL^T factorization */
#ifndef ANSI_C
VEC    *bdLDLsolve(A,b,x)
BAND   *A;
VEC    *b, *x;
#else
VEC    *bdLDLsolve(const BAND *A, const VEC *b, VEC *x)
#endif
{
   int i,j,l,n,n1,lb,ilb;
   Real **Av, *Avlb;
   Real c;

   if ( ! A || ! b )
     error(E_NULL,"bdLDLsolve");
   if ( A->mat->n != b->dim )
     error(E_SIZES,"bdLDLsolve");

   n = A->mat->n;
   n1 = n-1;
   x = v_resize(x,n);
   lb = A->lb;
   Av = A->mat->me;  
   Avlb = Av[lb];
   
   /* solve L*y = b */
   x->ve[0] = b->ve[0];
   for (i=1; i < n; i++) {
      ilb = i-lb;
      c = b->ve[i];
      for (j=max(0,ilb), l=j-ilb; j < i; j++,l++)
	c -= Av[l][j]*x->ve[j];
      x->ve[i] = c;
   }

   /* solve D*z = y */
   for (i=0; i < n; i++) 
     x->ve[i] /= Avlb[i];

   /* solve L^T*x = z */
   for (i=n-2; i >= 0; i--) {
      ilb = i+lb;
      c = x->ve[i];
      for (j=min(n1,ilb), l=ilb-j; j > i; j--,l++)
	c -= Av[l][i]*x->ve[j];
      x->ve[i] = c;
   }

   return x;
}


/* ******************************************************
  This function is a contribution from Ruediger Franke.
   His e-mail addres is: [e-mail]
   
   ******************************************************
*/

/* bd_mv_mlt --
 *   computes out = A * x
 *   may not work in situ (x != out)
 */

VEC *bd_mv_mlt(A, x, out)
BAND *A;
VEC *x, *out;
{
  int i, j, j_end, k;
  int start_idx, end_idx;
  int n, m, lb, ub;
  Real **A_me;
  Real *x_ve;
  Real sum;

  if (!A || !x)
    error(E_NULL,"bd_mv_mlt");
  if (x->dim != A->mat->n)
    error(E_SIZES,"bd_mv_mlt");
  if (!out || out->dim != A->mat->n)
    out = v_resize(out, A->mat->n);
  if (out == x)
    error(E_INSITU,"bd_mv_mlt");

  n = A->mat->n;
  m = A->mat->m;
  lb = A->lb;
  ub = A->ub;
  A_me = A->mat->me;
  start_idx = lb;
  end_idx = m + n-1 - ub;
  for (i=0; i<n; i++, start_idx--, end_idx--) {
    j = max(0, start_idx);
    k = max(0, -start_idx);
    j_end = min(m, end_idx);
    x_ve = x->ve + k;
    sum = 0.0;	     
    for (; j < j_end; j++, k++)
      sum += A_me[j][k] * *x_ve++;
    out->ve[i] = sum;
  }

  return out;
}