mes_factor.h

在数学计算中经常需要用到矩阵运算

    }
    
    return 1;
}


/* makeQ -- constructs orthogonal matrix from Householder vectors stored in
   compact QR form */
template <typename T> int	Matrix<T>::mes_makeQ(Matrix<T>& QR,Vector<T>& diag,Matrix<T>& Qout)
{
    static	Vector<T> tmp1,tmp2;
    int	i, limit;
    double	beta, r_ii, tmp_val;
    int	j;
    
    limit = local_min(NumRows(QR),NumCols(QR));
    if ( diag.Size() < limit )
		mes_error(E_SIZES,"makeQ");
    if ( NumRows(Qout) != NumRows(QR) || NumCols(Qout) != NumRows(QR) )
		Qout.Resize(NumRows(QR),NumRows(QR));
    
    tmp1.Resize(NumRows(QR));	/* contains basis vec & columns of Q */
    tmp2.Resize(NumRows(QR));	/* contains H/holder vectors */
    
    for ( i=1; i<=NumRows(QR) ; i++ ){	/* get i-th column of Q */
		/* set up tmp1 as i-th basis vector */
		for ( j=1; j<=NumRows(QR) ; j++ )
			tmp1[j] = 0.0;
		tmp1[i] = 1.0;
		
		/* apply H/h transforms in reverse order */
		for ( j=limit; j>=1; j-- ){
			mes_get_col(QR,j,tmp2);
			r_ii = Abs(tmp2[j]);
			tmp2[j] = diag[j];
			tmp_val = (r_ii*Abs(diag[j]));
			beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
			mes_hhtrvec(tmp2,beta,j,tmp1,tmp1);
		}
		
		/* insert into Q */
		mes_set_col(Qout,i,tmp1);
    }
	
    return 1;
}


/* makeR -- constructs upper triangular matrix from QR (compact form)
   -- may be in-situ (all it does is zero the lower 1/2) */
template <typename T> int	Matrix<T>::mes_makeR(Matrix<T>& QR,Matrix<T>& Rout)
{
    int	i,j;
    
    Rout = QR;
    for ( i=2; i<=NumRows(QR); i++ )
	for ( j=1; j<=NumCols(QR) && j<i; j++ )
	    Rout[i][j] = 0.0;
    
    return 1;
}


/* QRsolve -- solves the system Q.R.x=b where Q & R are stored in compact form
   -- returns x, which is created if necessary */
template <typename T> int	Matrix<T>::mes_QRsolve(Matrix<T>& QR,Vector<T>& diag,const Vector<T>& b,Vector<T>& x)
{
    int	limit;
    static	Vector<T> tmp;
    
    limit = local_min(NumRows(QR),NumCols(QR));
    if ( diag.Size() < limit || b.Size() != NumRows(QR) )
		mes_error(E_SIZES,"QRsolve");
    tmp.Resize(limit);	
    x.Resize(NumCols(QR));
    mes_Qsolve_(QR,diag,b,x,tmp);
    mes_Usolve(QR,x,x,0.0);
    x.Resize(NumCols(QR));	
    return 1;
}


/* QRCPsolve -- solves A.x = b where A is factored by QRCPfactor()
   -- assumes that A is in the compact factored form */
template <typename T> int	Matrix<T>::mes_QRCPsolve(Matrix<T>& QR,Vector<T>& diag,Vector<int>& pivot,const Vector<T>& b,Vector<T>& x)
{
    static	Vector<T> tmp;
    
    if ( (NumRows(QR) > diag.Size() && NumCols(QR) > diag.Size()) || NumCols(QR) != pivot.Size() )
		mes_error(E_SIZES,"QRCPsolve");
    
    mes_QRsolve(QR,diag /* , beta */ ,b,tmp);
    mes_pxinv_vec(pivot,tmp,x);
	
    return 1;
}


/* Umlt -- compute out = upper_triang(U).x
	-- may be in situ */
template <typename T> int	Matrix<T>::mes_Umlt(const Matrix<T>& U,const Vector<T>& x,Vector<T>& out)
{
    int		i, limit;

    limit = local_min(NumRows(U),NumCols(U));
    if ( limit != x.Size() )
		mes_error(E_SIZES,"Umlt");
    if ( out.Size() != limit )
		out.Resize(limit);
	
    for ( i = 1; i <= limit; i++ ){
		T sum = T();
		for(int j=i; j<=limit; j++)
			sum += x[j]*U[i][j];
		out[i] = sum;
	}
    return 1;
}


/* UTmlt -- returns out = upper_triang(U)^T.x */
template <typename T> int	Matrix<T>::mes_UTmlt(const Matrix<T>& U,const Vector<T>& x,Vector<T>& out)
{
    T	sum;
    int		i, j, limit;
	
    limit = local_min(NumRows(U),NumCols(U));
    if ( out.Size() != limit )
		out.Resize(limit);
	
    for ( i = limit; i >= 1; i-- ){
		sum = 0.0;
		for ( j = 1; j <= i; j++ )
			sum += conj(U[j][i])*x[j];
		out[i] = sum;
    }
    return 1;
}


/* QRTsolve -- solve A^T.sc = c where the QR factors of A are stored in
	compact form
	-- returns sc
	-- original due to Mike Osborne modified Wed 09th Dec 1992 */
template <typename T> int	Matrix<T>::mes_QRTsolve(const Matrix<T>& A,Vector<T>& diag,Vector<T>& c,Vector<T>& sc)
{
    int		i, j, k, n, p;
    double	beta, r_ii, s, tmp_val;

    if ( diag.Size() < local_min(NumRows(A),NumCols(A)) )
		mes_error(E_SIZES,"QRTsolve");
    sc.Resize(sc,NumRows(A));
    n = sc.Size();
    p = c.Size();
    if ( n == p )
		k = p-1;
    else
		k = p;
    sc=0;
    sc[1] = c[1]/A[1][1];
    if ( n ==  1)
		return sc;
    if ( p > 2){
		for ( i = 2; i <= p; i++ ){
			s = 0.0;
			for ( j = 1; j < i; j++ )
				s += A[j][i]*sc[j];
			if ( A[i][i] == 0.0 )
				mes_error(E_SING,"QRTsolve");
			sc[i]=(c[i]-s)/A[i][i];
		}
    }
    for (i = k; i >= 1; i--){
		s = diag[i]*sc[i];
		for ( j = i+1; j <= n; j++ )
			s += A[j][i]*sc[j];
		r_ii = Abs(A[i][i]);
		tmp_val = (r_ii*Abs(diag[i]));
		beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
		tmp_val = beta*s;
		sc[i] -= tmp_val*diag[i];
		for ( j = i+1; j <= n; j++ )
			sc[j] -= tmp_val*A[j][i];
    }
	
    return 1;
}


/* QRcondest -- returns an estimate of the 2-norm condition number of the
		matrix factorised by QRfactor() or QRCPfactor()
	-- note that as Q does not affect the 2-norm condition number,
		it is not necessary to pass the diag, beta (or pivot) vectors
	-- generates a lower bound on the true condition number
	-- if the matrix is exactly singular, HUGE_VAL is returned
	-- note that QRcondest() is likely to be more reliable for
		matrices factored using QRCPfactor() */
template <typename T> double	Matrix<T>::mes_QRcondest(const Matrix<T>& QR, int p)
{
    static	Vector<T>	y;
    double	norm1, norm2, tmp1, tmp2;
    int		i, j, limit;
	T sum;

	if(p!=2){
		cerr << "ERROR: QRcondest() was not implemented for p!=2.\n";
		assert(0);
		exit(1);
		return HUGE_VAL;
	}
    limit = local_min(NumRows(QR),NumCols(QR));
    for ( i = 1; i <= limit; i++ ){
		if ( mtl_iszero(QR[i][i]) )
			return HUGE_VAL;
	}
	
	y.Resize(limit);
	/* use the trick for getting a unit vector y with ||R.y||_inf small
	from the LU condition estimator */
	for ( i = 1; i <= limit; i++ ){
		sum = T();
		for ( j = 1; j < i; j++ )
			sum -= QR[j][i]*y[j];
		sum += mtl_sign(sum); // to support complex
		y[i] = sum / QR[i][i];
	}
	mes_UTmlt(QR,y,y);
	
	/* now apply inverse power method to R^T.R */
	for ( i = 1; i <= 3; i++ ){
		tmp1 = Norm(y,p);
		y *= (1/tmp1);
		mes_UTsolve(QR,y,y,0.0);
		tmp2 = Norm(y,p);
		y *= (1/tmp2);
		mes_Usolve(QR,y,y,0.0);
	}
	/* now compute approximation for ||R^{-1}||_2 */
	norm1 = sqrt(tmp1)*sqrt(tmp2);
	
	/* now use complementary approach to compute approximation to ||R||_2 */
	for ( i = limit; i >= 1; i-- ){
		sum = T();
		for ( j = i+1; j <= limit; j++ )
			sum += QR[i][j]*y[j];
		y[i]=mtl_sign(sum)*T(Abs(QR[i][i])); // To support complex
	}
	
	/* now apply power method to R^T.R */
	for ( i = 1; i <= 3; i++ ){
		tmp1 = Norm(y,p);
		y *= (1/tmp1);
		mes_Umlt(QR,y,y);
		tmp2 = Norm(y,p);
		y *= (1/tmp2);
		mes_UTmlt(QR,y,y);
	}
	norm2 = sqrt(tmp1)*sqrt(tmp2);
	
	return norm1*norm2;
}


/* interchange -- a row/column swap routine */
template <typename T> void Matrix<T>::mes_interchange(Matrix<T>& A,int i,int j)
{
	T	tmp;
	int	k, n;

	n = NumCols(A);
	if ( i == j )
		return;
	if ( i > j ){	
		k = i;	i = j;	j = k;	
	}
	for ( k = 0; k < i; k++ ){
		tmp = A[k][i];
		A[k][i] = A[k][j];
		A[k][j] = tmp;
	}
	for ( k = j+1; k < n; k++ ){
		tmp = A[j][k];
		A[j][k] = A[i][k];
		A[i][k] = tmp;
	}
	for ( k = i+1; k < j; k++ ){
		tmp = A[k][j];
		A[k][j] = A[i][k];
		A[i][k] = tmp;
	}
	tmp = A[i][i];
	A[i][i] = A[j][j];
	A[j][j] = tmp;
}


/* BKPfactor -- Bunch-Kaufman-Parlett factorisation of A in-situ
	-- A is factored into the form P'AP = MDM' where 
	P is a permutation matrix, M lower triangular and D is block
	diagonal with blocks of size 1 or 2
	-- P is stored in pivot; blocks[i]==i iff D[i][i] is a block */
template <typename T> int Matrix<T>::mes_BKPfactor(Matrix<T>& A,Vector<int>& pivot,Vector<int>& blocks)
{
	const double mes_alpha = 0.6403882032022076; /* = (1+sqrt(17))/8 */
	int	i, j, k, n, onebyone, r;
	double	aii, aip1, aip1i, lambda, sigma, tmp;
	double	det, s, t;

	if ( NumRows(A) != NumCols(A) )
		mes_error(E_SQUARE,"BKPfactor");
	pivot.Resize(NumRows(A));
	blocks.Resize(NumRows(A));
	if ( NumRows(A) != pivot.Size() || pivot.Size() != blocks.Size() )
		mes_error(E_SIZES,"BKPfactor");

	n = NumCols(A);
	pivot.SetPermIdentity();
	blocks.SetPermIdentity();
	
	for ( i = 1; i <= n; i = onebyone ? i+1 : i+2 ){
		aii = Abs(A(i,i));
		lambda = 0.0;	r = (i+1 < n) ? i+1 : i;
		for ( k = i+1; k <= n; k++ ){
			tmp = Abs(A(i,k));
			if ( tmp >= lambda ){
				lambda = tmp;
				r = k;
			}
		}
		/* determine if 1x1 or 2x2 block, and do pivoting if needed */
		if ( aii >= mes_alpha*lambda ){
			onebyone = true;
			goto dopivot;
		}
		/* compute sigma */
		sigma = 0.0;
		for ( k = i; k <= n; k++ ){
			if ( k == r )
				continue;
			tmp = ( k > r ) ? Abs(A(r,k)) :
			Abs(A(k,r));
			if ( tmp > sigma )
				sigma = tmp;
		}
		if ( aii*sigma >= mes_alpha*(lambda*lambda) )
			onebyone = true;
		else if ( Abs(A(r,r)) >= mes_alpha*sigma ){
			mes_interchange(A,i,r);
			mes_px_transp(pivot,i,r);
			onebyone = true;
		}
		else{
			mes_interchange(A,i+1,r);
			mes_px_transp(pivot,i+1,r);
			mes_px_transp(blocks,i,i+1);
			onebyone = false;
		}
		
dopivot:
		if ( onebyone ){   /* do one by one block */
			if ( A(i,i) != 0.0 ){
				aii = A(i,i);
				for ( j = i+1; j <= n; j++ ){
					tmp = A(i,j)/aii;
					for ( k = j; k <= n; k++ )
						A[j][k] -= tmp*A(i,k);
					A[i][j]=tmp;
				}
			}
		}
		else{ /* onebyone == false */
		    /* do two by two block */
			det = A(i,i)*A(i+1,i+1)-(A(i,i+1)*A(i,i+1));
			/* Must have det < 0 */
			aip1i = A(i,i+1)/det;
			aii = A(i,i)/det;
			aip1 = A(i+1,i+1)/det;
			for ( j = i+2; j <= n; j++ ){
				s = - aip1i*A(i+1,j) + aip1*A(i,j);
				t = - aip1i*A(i,j) + aii*A(i+1,j);
				for ( k = j; k <= n; k++ )
					A[j][k] -= A(i,k)*s + A(i+1,k)*t;
				A[i][j]=s;
				A[i+1][j]=t;
			}
		}
	}
	
	/* set lower triangular half */
	for ( i = 1; i <= NumRows(A); i++ ){
		for ( j = 1; j < i; j++ )
			A[i][j]=A[j][i];
	}
	
	return 1;
}


/* BKPsolve -- solves A.x = b where A has been factored a la BKPfactor()
	-- returns x, which is created if NULL */
template <typename T> int Matrix<T>::mes_BKPsolve(const Matrix<T>& A,Vector<int>& pivot,Vector<int>& block,const Vector<T>& b,Vector<T>& x)
{
	static Vector<T> tmp;	/* dummy storage needed */
	int	i, j, n, onebyone;
	double	a11, a12, a22, b1, b2, det, sum, tmp_diag;

	if ( NumRows(A) != NumCols(A) )
		mes_error(E_SQUARE,"BKPsolve");
	n = NumCols(A);
	if ( b.Size() != n || pivot.Size() != n || block.Size() != n )
		mes_error(E_SIZES,"BKPsolve");
	x.Resize(n);
	tmp.Resize(n);

	Vector<T> btmp=b;
	mes_px_vec(pivot,btmp,tmp);
	/* solve for lower triangular part */
	for ( i = 1; i <= n; i++ ){
		sum = tmp(i);
		if ( block[i] < i ){
			for ( j = 1; j < i-1; j++ )
				sum -= A(i,j)*tmp(j);
		}
		else{
			for ( j = 1; j < i; j++ )
				sum -= A(i,j)*tmp(j);
		}
		tmp[i]=sum;
	}

	/* solve for diagonal part */
	for ( i = 1; i <= n; i = onebyone ? i+1 : i+2 ){
		onebyone = ( block[i] == i );
		if ( onebyone ){
			tmp_diag = A(i,i);
			if ( tmp_diag == 0.0 )
				mes_error(E_SING,"BKPsolve");
			tmp[i]=tmp[i]/tmp_diag;
		}
		else{
			a11 = A(i,i);
			a22 = A(i+1,i+1);
			a12 = A(i+1,i);
			b1 = tmp(i);	b2 = tmp(i+1);
			det = a11*a22-a12*a12;	/* < 0 : see BKPfactor() */
			if ( det == 0.0 )
				mes_error(E_SING,"BKPsolve");
			det = 1/det;
			tmp[i]=det*(a22*b1-a12*b2);
			tmp[i+1]=det*(a11*b2-a12*b1);
		}
	}

	/* solve for transpose of lower traingular part */
	for ( i = n; i >= 1; i-- ){	/* use symmetry of factored form to get stride 1 */
		sum = tmp(i);
		if ( block[i] > i ){
			for ( j = i+2; j <= n; j++ )
				sum -= A(i,j)*tmp(j);
		}
		else{
			for ( j = i+1; j <= n; j++ )
				sum -= A(i,j)*tmp(j);
		}
		tmp[i]=sum;
	}

	/* and do final permutation */
	mes_pxinv_vec(pivot,tmp,x);

	return 1;
}


#endif