schur.c

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

	    ********************/

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

	    /********************
	    a00 = A->me[k_min][k_min];
	    a01 = A->me[k_min][k_tmp];
	    a10 = A->me[k_tmp][k_min];
	    a11 = A->me[k_tmp][k_tmp];
	    ********************/
	    x = a00*a00 + a01*a10 - s*a00 + t;
	    y = a10*(a00+a11-s);
	    if ( k_min + 2 <= k_max )
		z = a10* /* m_entry(A,k_min+2,k_tmp) */ A->me[k_min+2][k_tmp];
	    else
		z = 0.0;

	    for ( k = k_min; k <= k_max-1; k++ )
	    {
		if ( k < k_max - 1 )
		{
		    hhldr3(x,y,z,&nu1,&beta2,&dummy);
		    tracecatch(hhldr3cols(A,k,max(k-1,0),  beta2,nu1,y,z),"schur");
		    tracecatch(hhldr3rows(A,k,min(n-1,k+3),beta2,nu1,y,z),"schur");
		    if ( Q != MNULL )
			hhldr3rows(Q,k,n-1,beta2,nu1,y,z);
		}
		else
		{
		    givens(x,y,&c,&s);
		    rot_cols(A,k,k+1,c,s,A);
		    rot_rows(A,k,k+1,c,s,A);
		    if ( Q )
			rot_cols(Q,k,k+1,c,s,Q);
		}
		/* if ( k >= 2 )
		    m_set_val(A,k,k-2,0.0); */
		/* x = A_me[k+1][k]; */
		x = m_entry(A,k+1,k);
		if ( k <= k_max - 2 )
		    /* y = A_me[k+2][k];*/
		    y = m_entry(A,k+2,k);
		else
		    y = 0.0;
		if ( k <= k_max - 3 )
		    /* z = A_me[k+3][k]; */
		    z = m_entry(A,k+3,k);
		else
		    z = 0.0;
	    }
	    /* if ( k_min > 0 )
		m_set_val(A,k_min,k_min-1,0.0);
	    if ( k_max < n - 1 )
		m_set_val(A,k_max+1,k_max,0.0); */
	    for ( k = k_min; k <= k_max-2; k++ )
	    {
		/* zero appropriate sub-diagonals */
		m_set_val(A,k+2,k,0.0);
		if ( k < k_max-2 )
		    m_set_val(A,k+3,k,0.0);
	    }

	    /* test to see if matrix should split */
	    for ( k = k_min; k < k_max; k++ )
		if ( fabs(A_me[k+1][k]) < MACHEPS*
		    (fabs(A_me[k][k])+fabs(A_me[k+1][k+1])) )
		{	A_me[k+1][k] = 0.0;	split = TRUE;	}
	}
    }
    
    /* polish up A by zeroing strictly lower triangular elements
       and small sub-diagonal elements */
    for ( i = 0; i < A->m; i++ )
	for ( j = 0; j < i-1; j++ )
	    A_me[i][j] = 0.0;
    for ( i = 0; i < A->m - 1; i++ )
	if ( fabs(A_me[i+1][i]) < MACHEPS*
	    (fabs(A_me[i][i])+fabs(A_me[i+1][i+1])) )
	    A_me[i+1][i] = 0.0;

#ifdef	THREADSAFE
    V_FREE(diag);	V_FREE(beta);
#endif

    return A;
}

/* schur_vals -- compute real & imaginary parts of eigenvalues
	-- assumes T contains a block upper triangular matrix
		as produced by schur()
	-- real parts stored in real_pt, imaginary parts in imag_pt */
#ifndef ANSI_C
void	schur_evals(T,real_pt,imag_pt)
MAT	*T;
VEC	*real_pt, *imag_pt;
#else
void	schur_evals(MAT *T, VEC *real_pt, VEC *imag_pt)
#endif
{
	int	i, n;
	Real	discrim, **T_me;
	Real	diff, sum, tmp;

	if ( ! T || ! real_pt || ! imag_pt )
		error(E_NULL,"schur_evals");
	if ( T->m != T->n )
		error(E_SQUARE,"schur_evals");
	n = T->n;	T_me = T->me;
	real_pt = v_resize(real_pt,(unsigned int)n);
	imag_pt = v_resize(imag_pt,(unsigned int)n);

	i = 0;
	while ( i < n )
	{
		if ( i < n-1 && T_me[i+1][i] != 0.0 )
		{   /* should be a complex eigenvalue */
		    sum  = 0.5*(T_me[i][i]+T_me[i+1][i+1]);
		    diff = 0.5*(T_me[i][i]-T_me[i+1][i+1]);
		    discrim = diff*diff + T_me[i][i+1]*T_me[i+1][i];
		    if ( discrim < 0.0 )
		    {	/* yes -- complex e-vals */
			real_pt->ve[i] = real_pt->ve[i+1] = sum;
			imag_pt->ve[i] = sqrt(-discrim);
			imag_pt->ve[i+1] = - imag_pt->ve[i];
		    }
		    else
		    {	/* no -- actually both real */
			tmp = sqrt(discrim);
			real_pt->ve[i]   = sum + tmp;
			real_pt->ve[i+1] = sum - tmp;
			imag_pt->ve[i]   = imag_pt->ve[i+1] = 0.0;
		    }
		    i += 2;
		}
		else
		{   /* real eigenvalue */
		    real_pt->ve[i] = T_me[i][i];
		    imag_pt->ve[i] = 0.0;
		    i++;
		}
	}
}

/* schur_vecs -- returns eigenvectors computed from the real Schur
		decomposition of a matrix
	-- T is the block upper triangular Schur matrix
	-- Q is the orthognal matrix where A = Q.T.Q^T
	-- if Q is null, the eigenvectors of T are returned
	-- X_re is the real part of the matrix of eigenvectors,
		and X_im is the imaginary part of the matrix.
	-- X_re is returned */
#ifndef ANSI_C
MAT	*schur_vecs(T,Q,X_re,X_im)
MAT	*T, *Q, *X_re, *X_im;
#else
MAT	*schur_vecs(MAT *T, MAT *Q, MAT *X_re, MAT *X_im)
#endif
{
	int	i, j, limit;
	Real	t11_re, t11_im, t12, t21, t22_re, t22_im;
	Real	l_re, l_im, det_re, det_im, invdet_re, invdet_im,
		val1_re, val1_im, val2_re, val2_im,
		tmp_val1_re, tmp_val1_im, tmp_val2_re, tmp_val2_im, **T_me;
	Real	sum, diff, discrim, magdet, norm, scale;
	STATIC VEC	*tmp1_re=VNULL, *tmp1_im=VNULL,
			*tmp2_re=VNULL, *tmp2_im=VNULL;

	if ( ! T || ! X_re )
	    error(E_NULL,"schur_vecs");
	if ( T->m != T->n || X_re->m != X_re->n ||
		( Q != MNULL && Q->m != Q->n ) ||
		( X_im != MNULL && X_im->m != X_im->n ) )
	    error(E_SQUARE,"schur_vecs");
	if ( T->m != X_re->m ||
		( Q != MNULL && T->m != Q->m ) ||
		( X_im != MNULL && T->m != X_im->m ) )
	    error(E_SIZES,"schur_vecs");

	tmp1_re = v_resize(tmp1_re,T->m);
	tmp1_im = v_resize(tmp1_im,T->m);
	tmp2_re = v_resize(tmp2_re,T->m);
	tmp2_im = v_resize(tmp2_im,T->m);
	MEM_STAT_REG(tmp1_re,TYPE_VEC);
	MEM_STAT_REG(tmp1_im,TYPE_VEC);
	MEM_STAT_REG(tmp2_re,TYPE_VEC);
	MEM_STAT_REG(tmp2_im,TYPE_VEC);

	T_me = T->me;
	i = 0;
	while ( i < T->m )
	{
	    if ( i+1 < T->m && T->me[i+1][i] != 0.0 )
	    {	/* complex eigenvalue */
		sum  = 0.5*(T_me[i][i]+T_me[i+1][i+1]);
		diff = 0.5*(T_me[i][i]-T_me[i+1][i+1]);
		discrim = diff*diff + T_me[i][i+1]*T_me[i+1][i];
		l_re = l_im = 0.0;
		if ( discrim < 0.0 )
		{	/* yes -- complex e-vals */
		    l_re = sum;
		    l_im = sqrt(-discrim);
		}
		else /* not correct Real Schur form */
		    error(E_RANGE,"schur_vecs");
	    }
	    else
	    {
		l_re = T_me[i][i];
		l_im = 0.0;
	    }

	    v_zero(tmp1_im);
	    v_rand(tmp1_re);
	    sv_mlt(MACHEPS,tmp1_re,tmp1_re);

	    /* solve (T-l.I)x = tmp1 */
	    limit = ( l_im != 0.0 ) ? i+1 : i;
	    /* printf("limit = %d\n",limit); */
	    for ( j = limit+1; j < T->m; j++ )
		tmp1_re->ve[j] = 0.0;
	    j = limit;
	    while ( j >= 0 )
	    {
		if ( j > 0 && T->me[j][j-1] != 0.0 )
		{   /* 2 x 2 diagonal block */
		    /* printf("checkpoint A\n"); */
		    val1_re = tmp1_re->ve[j-1] -
		      __ip__(&(tmp1_re->ve[j+1]),&(T->me[j-1][j+1]),limit-j);
		    /* printf("checkpoint B\n"); */
		    val1_im = tmp1_im->ve[j-1] -
		      __ip__(&(tmp1_im->ve[j+1]),&(T->me[j-1][j+1]),limit-j);
		    /* printf("checkpoint C\n"); */
		    val2_re = tmp1_re->ve[j] -
		      __ip__(&(tmp1_re->ve[j+1]),&(T->me[j][j+1]),limit-j);
		    /* printf("checkpoint D\n"); */
		    val2_im = tmp1_im->ve[j] -
		      __ip__(&(tmp1_im->ve[j+1]),&(T->me[j][j+1]),limit-j);
		    /* printf("checkpoint E\n"); */
		    
		    t11_re = T_me[j-1][j-1] - l_re;
		    t11_im = - l_im;
		    t22_re = T_me[j][j] - l_re;
		    t22_im = - l_im;
		    t12 = T_me[j-1][j];
		    t21 = T_me[j][j-1];

		    scale =  fabs(T_me[j-1][j-1]) + fabs(T_me[j][j]) +
			fabs(t12) + fabs(t21) + fabs(l_re) + fabs(l_im);

		    det_re = t11_re*t22_re - t11_im*t22_im - t12*t21;
		    det_im = t11_re*t22_im + t11_im*t22_re;
		    magdet = det_re*det_re+det_im*det_im;
		    if ( sqrt(magdet) < MACHEPS*scale )
		    {
		        det_re = MACHEPS*scale;
			magdet = det_re*det_re+det_im*det_im;
		    }
		    invdet_re =   det_re/magdet;
		    invdet_im = - det_im/magdet;
		    tmp_val1_re = t22_re*val1_re-t22_im*val1_im-t12*val2_re;
		    tmp_val1_im = t22_im*val1_re+t22_re*val1_im-t12*val2_im;
		    tmp_val2_re = t11_re*val2_re-t11_im*val2_im-t21*val1_re;
		    tmp_val2_im = t11_im*val2_re+t11_re*val2_im-t21*val1_im;
		    tmp1_re->ve[j-1] = invdet_re*tmp_val1_re -
		    		invdet_im*tmp_val1_im;
		    tmp1_im->ve[j-1] = invdet_im*tmp_val1_re +
		    		invdet_re*tmp_val1_im;
		    tmp1_re->ve[j]   = invdet_re*tmp_val2_re -
		    		invdet_im*tmp_val2_im;
		    tmp1_im->ve[j]   = invdet_im*tmp_val2_re +
		    		invdet_re*tmp_val2_im;
		    j -= 2;
	        }
	        else
		{
		    t11_re = T_me[j][j] - l_re;
		    t11_im = - l_im;
		    magdet = t11_re*t11_re + t11_im*t11_im;
		    scale = fabs(T_me[j][j]) + fabs(l_re);
		    if ( sqrt(magdet) < MACHEPS*scale )
		    {
		        t11_re = MACHEPS*scale;
			magdet = t11_re*t11_re + t11_im*t11_im;
		    }
		    invdet_re =   t11_re/magdet;
		    invdet_im = - t11_im/magdet;
		    /* printf("checkpoint F\n"); */
		    val1_re = tmp1_re->ve[j] -
		      __ip__(&(tmp1_re->ve[j+1]),&(T->me[j][j+1]),limit-j);
		    /* printf("checkpoint G\n"); */
		    val1_im = tmp1_im->ve[j] -
		      __ip__(&(tmp1_im->ve[j+1]),&(T->me[j][j+1]),limit-j);
		    /* printf("checkpoint H\n"); */
		    tmp1_re->ve[j] = invdet_re*val1_re - invdet_im*val1_im;
		    tmp1_im->ve[j] = invdet_im*val1_re + invdet_re*val1_im;
		    j -= 1;
		}
	    }

	    norm = v_norm_inf(tmp1_re) + v_norm_inf(tmp1_im);
	    sv_mlt(1/norm,tmp1_re,tmp1_re);
	    if ( l_im != 0.0 )
		sv_mlt(1/norm,tmp1_im,tmp1_im);
	    mv_mlt(Q,tmp1_re,tmp2_re);
	    if ( l_im != 0.0 )
		mv_mlt(Q,tmp1_im,tmp2_im);
	    if ( l_im != 0.0 )
		norm = sqrt(in_prod(tmp2_re,tmp2_re)+in_prod(tmp2_im,tmp2_im));
	    else
		norm = v_norm2(tmp2_re);
	    sv_mlt(1/norm,tmp2_re,tmp2_re);
	    if ( l_im != 0.0 )
		sv_mlt(1/norm,tmp2_im,tmp2_im);

	    if ( l_im != 0.0 )
	    {
		if ( ! X_im )
		error(E_NULL,"schur_vecs");
		set_col(X_re,i,tmp2_re);
		set_col(X_im,i,tmp2_im);
		sv_mlt(-1.0,tmp2_im,tmp2_im);
		set_col(X_re,i+1,tmp2_re);
		set_col(X_im,i+1,tmp2_im);
		i += 2;
	    }
	    else
	    {
		set_col(X_re,i,tmp2_re);
		if ( X_im != MNULL )
		    set_col(X_im,i,tmp1_im);	/* zero vector */
		i += 1;
	    }
	}

#ifdef	THREADSAFE
	V_FREE(tmp1_re);	V_FREE(tmp1_im);
	V_FREE(tmp2_re);	V_FREE(tmp2_im);
#endif

	return X_re;
}