matrixalgo.hpp

简单的C++计算库.其中实现了矩阵运算,大数运算.矩阵处理中使用lazy calculate,并且封装常见算法,可以将普通matlab程序用SDL改写.

#ifndef 	_SDL_MATHS_MATRIX_MATRIX_ALGO_HPP_
#define 	_SDL_MATHS_MATRIX_MATRIX_ALGO_HPP_

#ifndef		_SDL_MATHS_MATRIX_MATRIX_HPP_
#error		"must be included by Matrix.hpp"
#endif

/*

									Algorithm List

void	LU_Col(_PMatType& _P, _AMatType& _Mat)
void	LU_Col(_PMatType& _P, _LMatType& _L, _UMatType& _U, const _AMatType& _Mat)

void	GaussSolve_U(_ResultMatType& _Result, const _UMatType& _U, const _BMatType& _B)
void	GaussSolve_L(_ResultMatType& _Result, const _LMatType& _L, const _BMatType& _B)
void	GaussSolve(_ResultMatType& _Result, const _AMatType& _A, const _BMatType& _B)

void	CGSSolveImpl(_ResultMatType& _Result, const _AMatType& _A, const _BMatType& _B,
						Int32 _Max, Float _eps)
void	CGSSolve(_ResultMatType& _Result, const _AType& _A, const _BType& _B, const _CType& _C,
						Int32 _Max = 100, Float _eps = SDL_eps)
void	CGSSolve(_ResultMatType& _Result, const _AType& _A, const _BType& _B,
						Int32 _Max = 100, Float _eps = SDL_eps)

BasicMatrixExpr<BasicMatrix<typename _MatType::NumberType> >	Inv(const _MatType& _Mat)

void	HouseHolder(_ResultVecType& _v, typename _VecType::NumberType& _beta, const _VecType& _Vec)

void	QR(_DMatType& _d, _MatType& _Mat)
void	QR(_QMatType& _Q, _RMatType& _R, const _MatType& _Mat)

void	HessenBerg(_MatType& _Mat)

void	EIG_QRIterateBasic(_ResultMatType& _Result, const _MatType& _Mat, Int32 _Max = 100)
void	EIG_QRIterateOriginShift(_ResultMatType& _Result, const _MatType& _Mat, Int32 _Max = 100)

Int32	RankImpl(_MatType& _Mat)
Int32	Rank(const _MatType& _Mat)

typename _MatType::NumberType	Det(const _MatType& _Mat)

typename _MatType::NumberType	DetQR(const _MatType& _Mat)

*/

SDL_MATHS_MATRIX_BEGIN

/*
*
*	LU factorization
*	Algorithm	    :
*					Selecting the column major element
*	Type Condition  : 
*					_P is FullMatrix
*	Maths Condition :
*					_P : n * n
*					_Mat : n * n
*
*/

template<
		typename	_PMatType,
		typename	_AMatType
		>
inline
void	LU_Col(_PMatType& _P, _AMatType& _Mat)
{
	typedef	typename _AMatType::NumberType NumberType;

	CT_CONDITION(IsFullMatrix<_PMatType>::Result);

	RT_CONDITION(_P.row() == _P.col());
	RT_CONDITION(_Mat.row() == _Mat.col());
	RT_CONDITION(_P.row() == _Mat.row());

	const Int32 n = _Mat.row();

	_P.Diag();

	DataBlock<NumberType>	temp_data(n*n);

	for (Int32 k = MIDX(1); k <= MIDX(n-1); ++k)
	{
		Float max = -1;
		Int32 pos = -1;

		for (Int32 i = k; i <= MIDX(n); ++i)
		{
			NumberType temp = fabs(_Mat(i, k));
			if (temp > max)
			{
				max = temp, pos = i;
			}
		}

		_Mat.SwapRow(k, pos);
		_P.SwapRow(k, pos);

		_Mat(k+1, MIDX(n), k, k) = _Mat(k+1, MIDX(n), k, k) / (_Mat(k, k) + SDL_eps);

		BasicMatrix<NumberType> temp(MIDX(n)-k, MIDX(n)-k, temp_data.Buffer(), 0);

		temp = _Mat(k+1, MIDX(n), k, k) * _Mat(k, k, k+1, MIDX(n));

		_Mat(k+1, MIDX(n), k+1, MIDX(n)) = _Mat(k+1, MIDX(n), k+1, MIDX(n)) - temp;
	}
}

/*
*
*	LU factorization
*	Algorithm	    :
*					Selecting the column major element
*	Type Condition  : 
*					_P is FullMatrix
*					_L is FullMatrix or LowerMatrix
*					_U is FullMatrix or UpperMatrix
*	Maths Condition :
*					_P : n * n
*					_L : n * n
*					_U : n * n
*					_Mat : n * n
*
*/

template<
		typename	_PMatType,
		typename	_LMatType,
		typename	_UMatType,
		typename	_AMatType
		>
inline
void	LU_Col(_PMatType& _P, _LMatType& _L, _UMatType& _U, const _AMatType& _Mat)
{
	typedef	typename _AMatType::NumberType NumberType;

	CT_CONDITION(IsFullMatrix<_PMatType>::Result &&
				(IsFullMatrix<_LMatType>::Result || IsLowerMatrix<_LMatType>::Result) && 
				(IsFullMatrix<_UMatType>::Result || IsUpperMatrix<_UMatType>::Result));

	RT_CONDITION(_P.row() == _P.col());
	RT_CONDITION(_L.row() == _L.col());
	RT_CONDITION(_U.row() == _U.col());
	RT_CONDITION(_Mat.row() == _Mat.col());
	RT_CONDITION(_P.row() == _L.row() && _P.row() == _U.row() && _P.row() == _Mat.row());

	const Int32 n = _L.row();

	BasicMatrix<NumberType> Mat(_Mat.row(), _Mat.col());

	Mat = _Mat;

	_L.Diag();
	_U.Diag();

	LU_Col(_P, Mat);

	_L(MIDX(2), MIDX(n), MIDX(1), MIDX(1)) = 
					Mat(MIDX(2), MIDX(n), MIDX(1), MIDX(1));
	_U(MIDX(1), MIDX(1), MIDX(n), MIDX(n)) =
					Mat(MIDX(1), MIDX(1), MIDX(n), MIDX(n));

	for (Int32 i = MIDX(2); i <= MIDX(n-1); ++i)
	{
		_L(i+1, MIDX(n), i, i) = Mat(i+1, MIDX(n), i, i);
		_U(i, i, i, MIDX(n)) = Mat(i, i, i, MIDX(n));
	}

	_U(MIDX(n), MIDX(n)) = Mat(MIDX(n), MIDX(n));
}

/*
*
*	Solve linear equation (_U * _Result = _B)
*	Type Condition : 
*					_Result is FullMatrix
*	Maths Condition :
*					_U is upper triangle matrix (no check)
*					_U : n * n
*					_B : n * 1
*					_Result : n * 1
*
*/

template<
		typename _ResultMatType,
		typename _UMatType,
		typename _BMatType
		>
inline
void	GaussSolve_U(_ResultMatType& _Result, const _UMatType& _U, const _BMatType& _B)
{
	typedef typename _UMatType::NumberType NumberType;

	CT_CONDITION(IsFullMatrix<_ResultMatType>::Result);

	RT_CONDITION(_U.row() == _U.col() && _U.row() == _B.row() && _U.row() == _Result.row());

	const Int32 n = _U.row();

	_Result = _B;

	DataBlock<NumberType>	temp_data(n);

	for (int j = MIDX(n); j >= MIDX(2); --j)
	{
		_Result(j, MIDX(1)) /= _U(j, j) + SDL_eps;

		BasicMatrix<NumberType> temp(j-MIDX(1), 1, temp_data.Buffer(), 0);

		temp = _Result(MIDX(1), j-1, MIDX(1), MIDX(1)) - 
					_Result(j, MIDX(1)) * _U(MIDX(1), j-1, j, j);

		_Result(MIDX(1), j-1, MIDX(1), MIDX(1)) = temp;
	}

	_Result(MIDX(1), MIDX(1)) /= _U(MIDX(1), MIDX(1)) + SDL_eps;
}

/*
*
*	Solve linear equation (_L * _Result = _B)
*	Type Condition : 
*					_Result is FullMatrix
*	Maths Condition :
*					_L is lower triangle matrix (no check)
*					_L : n * n
*					_B : n * 1
*					_Result : n * 1
*
*/

template<
		typename _ResultMatType,
		typename _LMatType,
		typename _BMatType
		>
inline
void	GaussSolve_L(_ResultMatType& _Result, const _LMatType& _L, const _BMatType& _B)
{
	typedef typename _LMatType::NumberType NumberType;

	CT_CONDITION(IsFullMatrix<_ResultMatType>::Result);

	RT_CONDITION(_L.row() == _L.col() && _L.row() == _B.row() && _L.row() == _Result.row());

	const Int32 n = _L.row();

	_Result = _B;

	DataBlock<NumberType>	temp_data(n);

	for (int j = MIDX(1); j <= MIDX(n-1); ++j)
	{
		_Result(j, MIDX(1)) /= _L(j, j) + SDL_eps;

		BasicMatrix<NumberType> temp(MIDX(n)-j, 1, temp_data.Buffer(), 0);

		temp = _Result(j+1, MIDX(n), MIDX(1), MIDX(1)) - 
					_Result(j, MIDX(1)) * _L(j+1, MIDX(n), j, j);

		_Result(j+1, MIDX(n), MIDX(1), MIDX(1)) = temp;
	}
	
	_Result(MIDX(n), MIDX(1)) /= _L(MIDX(n), MIDX(n));
}

/*
*
*	Solve linear equation (_A * _Result = _B)
*	Type Condition : 
*					_Result is FullMatrix
*	
*	Maths Condition :
*					_A : n * n
*					_B : n * 1
*					_Result : n * 1
*
*/

template<
		typename _ResultMatType,
		typename _AMatType,
		typename _BMatType
		>
inline
void	GaussSolve(_ResultMatType& _Result, const _AMatType& _A, const _BMatType& _B)
{
	typedef typename _AMatType::NumberType NumberType;

	CT_CONDITION(IsFullMatrix<_ResultMatType>::Result);

	RT_CONDITION(_A.row() == _A.col() && _A.row() == _B.row() && _A.row() == _Result.row());

	const Int32 n = _A.row();

	BasicMatrix<NumberType>	P(n, n);
	BasicMatrix<NumberType, LowerMatrixPolicy<NumberType> > L(n, n);
	BasicMatrix<NumberType, UpperMatrixPolicy<NumberType> > U(n, n);
	BasicMatrix<NumberType>	y(n);

	LU_Col(P, L, U, _A);
	GaussSolve_L(y, L, P*_B);
	GaussSolve_U(_Result, U, y);
}

/*
*
*	Solve linear equation (_A * _Result = _B)
*	Algorithm	    :
*					 Conjugate gradients squared method
*	Type Condition  : 
*					_Result is FullMatrix
*	Maths Condition :
*					_A : n * n
*					_B : n * 1
*					_Result : n * 1
*
*/

template<
		typename _ResultMatType,
		typename _AMatType,
		typename _BMatType
		>
inline
void	CGSSolveImpl(_ResultMatType& _Result, const _AMatType& _A, const _BMatType& _B,
						Int32 _Max, Float _eps)
{
	typedef typename _AMatType::NumberType NumberType;

	CT_CONDITION(IsFullMatrix<_ResultMatType>::Result);

	RT_CONDITION(_A.row() == _A.col() && _A.row() == _B.row() && _A.row() == _Result.row());

	BasicMatrix<NumberType>		r(_A.row());
	BasicMatrix<NumberType>		p(_A.row());
	BasicMatrix<NumberType>		w(_A.row());
	double						ro = 0;
	double						_ro = 0;
	double						cond = _eps * Scalar(Tran(_B) * _B);
	double						beta = 0;

	r = _B - _A * _Result;
	ro = Scalar(Tran(r) * r);

	for (Int32 i = 0; i < _Max && sqrt(ro) > cond; ++i)
	{
		if (i != 0)
		{
			beta = ro / _ro;
			p = r + beta * p;
		}
		else
		{
			p = r;
		}
		w = _A * p;
		double alpha = ro / Scalar(Tran(p) * w);
		_Result += alpha * p;
		r -= alpha * w;
		_ro = ro;
		ro = Scalar(Tran(r) * r);
	}
}

template<
		typename _ResultMatType,
		typename _AType,
		typename _BType,
		typename _CType
		>
inline
void	CGSSolve(_ResultMatType& _Result, const _AType& _A, const _BType& _B, const _CType& _C,
						Int32 _Max = 100, Float _eps = SDL_eps)
{
		_Result = _C;
		CGSSolveImpl(_Result, _A, _B, _Max, _eps);
}

template<
		typename _ResultMatType,
		typename _AType,
		typename _BType
		>
inline
void	CGSSolve(_ResultMatType& _Result, const _AType& _A, const _BType& _B,
						Int32 _Max = 100, Float _eps = SDL_eps)
{
		_Result = _B;
		CGSSolveImpl(_Result, _A, _B, _Max, _eps);
}

/*
*
*	Calculate the inverse of a matrix
*	Algorithm	    :
*					Elementary row transform
*	Type Condition  : 
*					none
*	Maths Condition :
*					_Mat : n * n
*
*/

template<typename _MatType>
inline
BasicMatrixExpr<BasicMatrix<typename _MatType::NumberType> >
Inv(const _MatType& _Mat)
{
	typedef	typename _MatType::NumberType	NumberType;
	
	RT_CONDITION(_Mat.row() == _Mat.col());
	
	if (_Mat.row() == 1)
	{
		BasicMatrix<NumberType>	result(_Mat.row(), _Mat.col(), 0);
		result = _Mat;
		result(MIDX(1), MIDX(1)) = NumberType(1) / result(MIDX(1), MIDX(1));
		return BasicMatrixExpr<BasicMatrix<NumberType> >(result);
	}

	BasicMatrix<NumberType>	temp(_Mat.row(), _Mat.col());
	BasicMatrix<NumberType>	result(_Mat.row(), _Mat.col(), 0);

	const Int32 n = _Mat.row();

	temp = _Mat;

	for (int i = MIDX(1); i <= MIDX(n); ++i)
	{
		result(i, i) = 1;
	}

	for (int i = MIDX(1); i <= MIDX(n-1); ++i)
	{
		NumberType max = -1;
		Int32 pos = -1;

		for (Int32 j = i; j <= MIDX(n); ++j)
		{
			NumberType t = fabs(temp(j, i));
			if (t > max)
			{
				max = t, pos = j;
			}
		}

		temp.SwapRow(i, pos);
		result.SwapRow(i, pos);

		result(i, i, 1, MIDX(n)) = result(i, i, 1, MIDX(n)) / temp(i, i);
		temp(i, i, i, MIDX(n)) = temp(i, i, i, MIDX(n)) / temp(i, i);

		for (int j = i + 1; j <= MIDX(n); ++j)
		{
			NumberType	t = -temp(j, i);
			temp(j, j, i, MIDX(n)) =
				temp(j, j, i, MIDX(n)) + temp(i, i, i, MIDX(n)) * t;
			result(j, j, MIDX(1), MIDX(n)) =
				result(j, j, MIDX(1), MIDX(n)) + result(i, i, MIDX(1), MIDX(n)) * t;
		}
	}

	for (int i = MIDX(n); i >= MIDX(2); --i)
	{
		result(i, i, MIDX(1), MIDX(n)) =
			result(i, i, MIDX(1), MIDX(n)) / temp(i, i);
		temp(i, i, i, MIDX(n)) =
			temp(i, i, i, MIDX(n)) / temp(i, i);

		for (int j = i - 1; j >= MIDX(1); --j)
		{
			NumberType	t = -temp(j, i);
			temp(j, j, i, MIDX(n)) =
				temp(j, j, i, MIDX(n)) + temp(i, i, i, MIDX(n)) * t;
			result(j, j, MIDX(1), MIDX(n)) =
				result(j, j, MIDX(1), MIDX(n)) + result(i, i, MIDX(1), MIDX(n)) * t;
		}
	}

	return BasicMatrixExpr<BasicMatrix<NumberType> >(result);