schur.c

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

/**************************************************************************
**
** Copyright (C) 1993 David E. Stewart & Zbigniew Leyk, all rights reserved.
**
**			     Meschach Library
** 
** This Meschach Library is provided "as is" without any express 
** or implied warranty of any kind with respect to this software. 
** In particular the authors shall not be liable for any direct, 
** indirect, special, incidental or consequential damages arising 
** in any way from use of the software.
** 
** Everyone is granted permission to copy, modify and redistribute this
** Meschach Library, provided:
**  1.  All copies contain this copyright notice.
**  2.  All modified copies shall carry a notice stating who
**      made the last modification and the date of such modification.
**  3.  No charge is made for this software or works derived from it.  
**      This clause shall not be construed as constraining other software
**      distributed on the same medium as this software, nor is a
**      distribution fee considered a charge.
**
***************************************************************************/


/*	
	File containing routines for computing the Schur decomposition
	of a real non-symmetric matrix
	See also: hessen.c
*/

#include	<stdio.h>
#include	<math.h>
#include	"matrix.h"
#include        "matrix2.h"


static char rcsid[] = "$Id: schur.c,v 1.7 1994/03/17 05:36:53 des Exp $";



#ifndef ANSI_C
static	void	hhldr3(x,y,z,nu1,beta,newval)
double	x, y, z;
Real	*nu1, *beta, *newval;
#else
static	void	hhldr3(double x, double y, double z,
		       Real *nu1, Real *beta, Real *newval)
#endif
{
	Real	alpha;

	if ( x >= 0.0 )
		alpha = sqrt(x*x+y*y+z*z);
	else
		alpha = -sqrt(x*x+y*y+z*z);
	*nu1 = x + alpha;
	*beta = 1.0/(alpha*(*nu1));
	*newval = alpha;
}

#ifndef ANSI_C
static	void	hhldr3cols(A,k,j0,beta,nu1,nu2,nu3)
MAT	*A;
int	k, j0;
double	beta, nu1, nu2, nu3;
#else
static	void	hhldr3cols(MAT *A, int k, int j0, double beta,
			   double nu1, double nu2, double nu3)
#endif
{
	Real	**A_me, ip, prod;
	int	j, n;

	if ( k < 0 || k+3 > A->m || j0 < 0 )
		error(E_BOUNDS,"hhldr3cols");
	A_me = A->me;		n = A->n;

	/* printf("hhldr3cols:(l.%d) j0 = %d, k = %d, A at 0x%lx, m = %d, n = %d\n",
	       __LINE__, j0, k, (long)A, A->m, A->n); */
	/* printf("hhldr3cols: A (dumped) =\n");	m_dump(stdout,A); */

	for ( j = j0; j < n; j++ )
	{
	    /*****	    
	    ip = nu1*A_me[k][j] + nu2*A_me[k+1][j] + nu3*A_me[k+2][j];
	    prod = ip*beta;
	    A_me[k][j]   -= prod*nu1;
	    A_me[k+1][j] -= prod*nu2;
	    A_me[k+2][j] -= prod*nu3;
	    *****/
	    /* printf("hhldr3cols: j = %d\n", j); */

	    ip = nu1*m_entry(A,k,j)+nu2*m_entry(A,k+1,j)+nu3*m_entry(A,k+2,j);
	    prod = ip*beta;
	    /*****
	    m_set_val(A,k  ,j,m_entry(A,k  ,j) - prod*nu1);
	    m_set_val(A,k+1,j,m_entry(A,k+1,j) - prod*nu2);
	    m_set_val(A,k+2,j,m_entry(A,k+2,j) - prod*nu3);
	    *****/
	    m_add_val(A,k  ,j,-prod*nu1);
	    m_add_val(A,k+1,j,-prod*nu2);
	    m_add_val(A,k+2,j,-prod*nu3);

	}
	/* printf("hhldr3cols:(l.%d) j0 = %d, k = %d, m = %d, n = %d\n",
	       __LINE__, j0, k, A->m, A->n); */
	/* putc('\n',stdout); */
}

#ifndef ANSI_C
static	void	hhldr3rows(A,k,i0,beta,nu1,nu2,nu3)
MAT	*A;
int	k, i0;
double	beta, nu1, nu2, nu3;
#else
static	void	hhldr3rows(MAT *A, int k, int i0, double beta,
			   double nu1, double nu2, double nu3)
#endif
{
	Real	**A_me, ip, prod;
	int	i, m;

	/* printf("hhldr3rows:(l.%d) A at 0x%lx\n", __LINE__, (long)A); */
	/* printf("hhldr3rows: k = %d\n", k); */
	if ( k < 0 || k+3 > A->n )
		error(E_BOUNDS,"hhldr3rows");
	A_me = A->me;		m = A->m;
	i0 = min(i0,m-1);

	for ( i = 0; i <= i0; i++ )
	{
	    /****
	    ip = nu1*A_me[i][k] + nu2*A_me[i][k+1] + nu3*A_me[i][k+2];
	    prod = ip*beta;
	    A_me[i][k]   -= prod*nu1;
	    A_me[i][k+1] -= prod*nu2;
	    A_me[i][k+2] -= prod*nu3;
	    ****/

	    ip = nu1*m_entry(A,i,k)+nu2*m_entry(A,i,k+1)+nu3*m_entry(A,i,k+2);
	    prod = ip*beta;
	    m_add_val(A,i,k  , - prod*nu1);
	    m_add_val(A,i,k+1, - prod*nu2);
	    m_add_val(A,i,k+2, - prod*nu3);

	}
}

/* schur -- computes the Schur decomposition of the matrix A in situ
	-- optionally, gives Q matrix such that Q^T.A.Q is upper triangular
	-- returns upper triangular Schur matrix */
#ifndef ANSI_C
MAT	*schur(A,Q)
MAT	*A, *Q;
#else
MAT	*schur(MAT *A, MAT *Q)
#endif
{
    int		i, j, iter, k, k_min, k_max, k_tmp, n, split;
    Real	beta2, c, discrim, dummy, nu1, s, t, tmp, x, y, z;
    Real	**A_me;
    Real	sqrt_macheps;
    STATIC	VEC	*diag=VNULL, *beta=VNULL;
    
    if ( ! A )
	error(E_NULL,"schur");
    if ( A->m != A->n || ( Q && Q->m != Q->n ) )
	error(E_SQUARE,"schur");
    if ( Q != MNULL && Q->m != A->m )
	error(E_SIZES,"schur");
    n = A->n;
    diag = v_resize(diag,A->n);
    beta = v_resize(beta,A->n);
    MEM_STAT_REG(diag,TYPE_VEC);
    MEM_STAT_REG(beta,TYPE_VEC);
    /* compute Hessenberg form */
    Hfactor(A,diag,beta);
    
    /* save Q if necessary */
    if ( Q )
	Q = makeHQ(A,diag,beta,Q);
    makeH(A,A);

    sqrt_macheps = sqrt(MACHEPS);

    k_min = 0;	A_me = A->me;

    while ( k_min < n )
    {
	Real	a00, a01, a10, a11;
	double	scale, t, numer, denom;

	/* find k_max to suit:
	   submatrix k_min..k_max should be irreducible */
	k_max = n-1;
	for ( k = k_min; k < k_max; k++ )
	    /* if ( A_me[k+1][k] == 0.0 ) */
	    if ( m_entry(A,k+1,k) == 0.0 )
	    {	k_max = k;	break;	}

	if ( k_max <= k_min )
	{
	    k_min = k_max + 1;
	    continue;		/* outer loop */
	}

	/* check to see if we have a 2 x 2 block
	   with complex eigenvalues */
	if ( k_max == k_min + 1 )
	{
	    /* tmp = A_me[k_min][k_min] - A_me[k_max][k_max]; */
	    a00 = m_entry(A,k_min,k_min);
	    a01 = m_entry(A,k_min,k_max);
	    a10 = m_entry(A,k_max,k_min);
	    a11 = m_entry(A,k_max,k_max);
	    tmp = a00 - a11;
	    /* discrim = tmp*tmp +
		4*A_me[k_min][k_max]*A_me[k_max][k_min]; */
	    discrim = tmp*tmp + 4*a01*a10;
	    if ( discrim < 0.0 )
	    {	/* yes -- e-vals are complex
		   -- put 2 x 2 block in form [a b; c a];
		   then eigenvalues have real part a & imag part sqrt(|bc|) */
		numer = - tmp;
		denom = ( a01+a10 >= 0.0 ) ?
		    (a01+a10) + sqrt((a01+a10)*(a01+a10)+tmp*tmp) :
		    (a01+a10) - sqrt((a01+a10)*(a01+a10)+tmp*tmp);
		if ( denom != 0.0 )
		{   /* t = s/c = numer/denom */
		    t = numer/denom;
		    scale = c = 1.0/sqrt(1+t*t);
		    s = c*t;
		}
		else
		{
		    c = 1.0;
		    s = 0.0;
		}
		rot_cols(A,k_min,k_max,c,s,A);
		rot_rows(A,k_min,k_max,c,s,A);
		if ( Q != MNULL )
		    rot_cols(Q,k_min,k_max,c,s,Q);
		k_min = k_max + 1;
		continue;
	    }
	    else /* discrim >= 0; i.e. block has two real eigenvalues */
	    {	/* no -- e-vals are not complex;
		   split 2 x 2 block and continue */
		/* s/c = numer/denom */
		numer = ( tmp >= 0.0 ) ?
		    - tmp - sqrt(discrim) : - tmp + sqrt(discrim);
		denom = 2*a01;
		if ( fabs(numer) < fabs(denom) )
		{   /* t = s/c = numer/denom */
		    t = numer/denom;
		    scale = c = 1.0/sqrt(1+t*t);
		    s = c*t;
		}
		else if ( numer != 0.0 )
		{   /* t = c/s = denom/numer */
		    t = denom/numer;
		    scale = 1.0/sqrt(1+t*t);
		    c = fabs(t)*scale;
		    s = ( t >= 0.0 ) ? scale : -scale;
		}
		else /* numer == denom == 0 */
		{
		    c = 0.0;
		    s = 1.0;
		}
		rot_cols(A,k_min,k_max,c,s,A);
		rot_rows(A,k_min,k_max,c,s,A);
		/* A->me[k_max][k_min] = 0.0; */
		if ( Q != MNULL )
		    rot_cols(Q,k_min,k_max,c,s,Q);
		k_min = k_max + 1;	/* go to next block */
		continue;
	    }
	}

	/* now have r x r block with r >= 2:
	   apply Francis QR step until block splits */
	split = FALSE;		iter = 0;
	while ( ! split )
	{
	    iter++;
	    
	    /* set up Wilkinson/Francis complex shift */
	    k_tmp = k_max - 1;

	    a00 = m_entry(A,k_tmp,k_tmp);
	    a01 = m_entry(A,k_tmp,k_max);
	    a10 = m_entry(A,k_max,k_tmp);
	    a11 = m_entry(A,k_max,k_max);

	    /* treat degenerate cases differently
	       -- if there are still no splits after five iterations
	          and the bottom 2 x 2 looks degenerate, force it to
		  split */
#ifdef DEBUG
	    printf("# schur: bottom 2 x 2 = [%lg, %lg; %lg, %lg]\n",
		   a00, a01, a10, a11);
#endif
	    if ( iter >= 5 &&
		 fabs(a00-a11) < sqrt_macheps*(fabs(a00)+fabs(a11)) &&
		 (fabs(a01) < sqrt_macheps*(fabs(a00)+fabs(a11)) ||
		  fabs(a10) < sqrt_macheps*(fabs(a00)+fabs(a11))) )
	    {
	      if ( fabs(a01) < sqrt_macheps*(fabs(a00)+fabs(a11)) )
		m_set_val(A,k_tmp,k_max,0.0);
	      if ( fabs(a10) < sqrt_macheps*(fabs(a00)+fabs(a11)) )
		{
		  m_set_val(A,k_max,k_tmp,0.0);
		  split = TRUE;
		  continue;
		}
	    }

	    s = a00 + a11;
	    t = a00*a11 - a01*a10;

	    /* break loop if a 2 x 2 complex block */
	    if ( k_max == k_min + 1 && s*s < 4.0*t )
	    {
		split = TRUE;
		continue;
	    }

	    /* perturb shift if convergence is slow */
	    if ( (iter % 10) == 0 )
	    {	s += iter*0.02;		t += iter*0.02;
	    }

	    /* set up Householder transformations */
	    k_tmp = k_min + 1;
	    /********************
	    x = A_me[k_min][k_min]*A_me[k_min][k_min] +
		A_me[k_min][k_tmp]*A_me[k_tmp][k_min] -
		    s*A_me[k_min][k_min] + t;
	    y = A_me[k_tmp][k_min]*
		(A_me[k_min][k_min]+A_me[k_tmp][k_tmp]-s);
	    if ( k_min + 2 <= k_max )
		z = A_me[k_tmp][k_min]*A_me[k_min+2][k_tmp];
	    else
		z = 0.0;