matrixalgo.hpp

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

}

/*
*
*	Calculate the householder transform of a vector
*	Type Condition  : 
*					none
*	Maths Condition :
*					_v : n * 1
*					_Vec : n * 1
*
*/

template<
		typename _ResultVecType,
		typename _VecType
		>
inline
void	HouseHolder(_ResultVecType& _v, typename _VecType::NumberType& _beta, const _VecType& _Vec)
{
	typedef	typename _VecType::NumberType	NumberType;
	RT_CONDITION(_Vec.col() == 1);

	FullMatrix	x(_Vec.row());
	x = _Vec;
	x /= Max(Abs(x));

	const Int32	n = x.row();
	Float	sigma = Scalar(Tran(x(MIDX(2), MIDX(n), MIDX(1), MIDX(1))) * x(MIDX(2), MIDX(n), MIDX(1), MIDX(1)));

	
	_v(MIDX(1)) = 1;
	_v(MIDX(2), MIDX(n), MIDX(1), MIDX(1)) = x(MIDX(2), MIDX(n), MIDX(1), MIDX(1));

	if (ZeroCmp<NumberType>::Cmp(sigma) != 0)
	{
		Float	alpha = sqrt(x(MIDX(1)) * x(MIDX(1)) + sigma);
		if (NumberSign<Float>::Sign(x(MIDX(1))) <= 0)
		{
			_v(MIDX(1)) = x(MIDX(1)) - alpha;
		}
		else
		{
			_v(MIDX(1)) = - sigma / (x(MIDX(1)) + alpha);
		}
		_beta = 2 * _v(MIDX(1)) * _v(MIDX(1)) / (sigma + _v(MIDX(1)) * _v(MIDX(1)));
		_v /= _v(MIDX(1));
	}
	else
	{
		_beta = 0;
	}
}

/*
*
*	QR factorization
*	Type Condition  : 
*	Maths Condition :
*					_d : m * 1
*					_Mat : m * n, m >= n
*/

template<
		typename _DMatType,
		typename _MatType
		>
inline
void	QR(_DMatType& _d, _MatType& _Mat)
{
	typedef typename _MatType::NumberType	NumberType;

	RT_CONDITION(_d.row() == _Mat.col() && _Mat.row() >= _Mat.col());

	const Int32 m = _Mat.row();
	const Int32 n = _Mat.col();

	DataBlock<NumberType>	temp_data(m);
	DataBlock<NumberType>	temp_data1(m);
	DataBlock<NumberType>	temp_data2(m*n);

	for (Int32 j = 0; j < m; ++j)
	{
		temp_data1[j] = 1;
	}

	for (Int32 j = MIDX(1); j <= MIDX(n); ++j)
	{
		FullMatrix	v(MIDX(m) - j + 1, 1, temp_data.Buffer(), 0);
		DiagMatrix	I(MIDX(m) - j + 1, MIDX(m) - j + 1, temp_data1.Buffer(), 0);
		FullMatrix	t(MIDX(m) - j + 1, MIDX(n) - j + 1, temp_data2.Buffer(), 0);
		Float	beta = 0;
		
		HouseHolder(v, beta, _Mat(j, MIDX(m), j, j));

		t = (I - beta * v * Tran(v)) * _Mat(j, MIDX(m), j, MIDX(n));

		_Mat(j, MIDX(m), j, MIDX(n)) = t;

		_d(j) = beta;

		if (j < MIDX(m))
		{
			_Mat(j+1, MIDX(m), j, j) = v(MIDX(2),  MIDX(MIDX(m) - j + 1), MIDX(1), MIDX(1));
		}
	}
}

template<
		typename _QMatType,
		typename _RMatType,
		typename _MatType
		>
inline
void	QR(_QMatType& _Q, _RMatType& _R, const _MatType& _Mat)
{
	typedef typename _MatType::NumberType NumberType;

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

	FullMatrix	t(_Mat.row(), _Mat.col());
	FullMatrix	d(_Mat.col());

	const Int32 m = _Mat.row();
	const Int32 n = _Mat.col();

	t = _Mat;
	QR(d, t);

	{
		_R.Zero();
		for (Int32 i = MIDX(1); i <= MIDX(m); ++i)
			for (Int32 j = i; j <= MIDX(n); ++j)
			{
				_R(i, j) = t(i, j);
			}
	}

	{
		DataBlock<NumberType>	temp_data(m);
		DataBlock<NumberType>	temp_data1(m);
		DataBlock<NumberType>	temp_data2(m*m);

		for (Int32 i = 0; i < m; ++i)
		{
				temp_data1[i] = 1;
		}
		
		_Q.Diag();
		for (Int32 i = MIDX(1); i <= MIDX(n); ++i)
		{
			FullMatrix	v(MIDX(m)-i+1, 1, temp_data.Buffer(), 0);
			DiagMatrix	I(MIDX(m)-i+1, MIDX(m)-i+1, temp_data1.Buffer(), 0);

			v(MIDX(1)) = 1;
			if (i+1 <= MIDX(m))
			{
				v(MIDX(2), MIDX(MIDX(m)-i+1), MIDX(1), MIDX(1)) = t(i+1, MIDX(m), i, i);
			}

			FullMatrix	temp(m, m, temp_data2.Buffer(), 0);
			temp.Diag();
			temp(i, MIDX(m), i, MIDX(m)) = I - d(i) * v * Tran(v);
			_Q *= temp;
		}
	}
}

/*
*
*	HessenBerg factorization
*	Type Condition  : 
*	Maths Condition :
*					_Mat : n * n
*/

template<
		typename _MatType
		>
inline
void	HessenBerg(_MatType& _Mat)
{
	typedef typename _MatType::NumberType NumberType;

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

	const Int32 n = _Mat.row();

	DataBlock<NumberType>	temp_data(n);
	DataBlock<NumberType>	temp_data1(n);
	DataBlock<NumberType>	temp_data2(n*n);

	for (Int32 j = 0; j < n; ++j)
	{
		temp_data1[j] = 1;
	}

	for (Int32 k = MIDX(1); k <= MIDX(n-2); ++k)
	{
		FullMatrix	v(MIDX(n) - k, 1, temp_data.Buffer(), 0);
		DiagMatrix	I(MIDX(n) - k, MIDX(n) - k, temp_data1.Buffer(), 0);
		FullMatrix	t1(MIDX(n) - k, MIDX(n) - k + 1, temp_data2.Buffer(), 0);
		FullMatrix	t2(n, MIDX(n) - k, temp_data2.Buffer(), 0);
		Float	beta = 0;

		HouseHolder(v, beta, _Mat(k+1, MIDX(n), k, k));

		t1 = (I - beta * v * Tran(v)) * _Mat(k+1, MIDX(n), k, MIDX(n));
		_Mat(k+1, MIDX(n), k, MIDX(n)) = t1;

		t2 = _Mat(MIDX(1), MIDX(n), k+1, MIDX(n)) * (I - beta * v * Tran(v));
		_Mat(MIDX(1), MIDX(n), k+1, MIDX(n)) = t2;
	}
}

/*
*
*	Find eigenvalues
*	Algorithm		:
*					QR Iterate
*	Type Condition  : 
*	Maths Condition :
*					_Result : n * 1
*					_Mat : n * n
*/

template<
		typename _ResultMatType,
		typename _MatType
		>
inline
void	EIG_QRIterateBasic(_ResultMatType& _Result, const _MatType& _Mat, Int32 _Max = 100)
{
	typedef typename _MatType::NumberType NumberType;

	RT_CONDITION(_Result.row() == _Mat.row() && _Result.col() == 1 && _Mat.row() == _Mat.col());

	const Int32 n = _Mat.row();

	FullMatrix	t(n, n);
	FullMatrix	Q(n, n);
	FullMatrix	R(n, n);

	t = _Mat;

	for (Int32 i = 0; i < _Max; ++i)
	{
		QR(Q, R, t);
		t = R * Q;
	}

	QR(Q, R, t);

	for (Int32 i = MIDX(1); i <= MIDX(n); ++i)
	{
		_Result(i) = R(i, i);
	}
}

template<
		typename _ResultMatType,
		typename _MatType
		>
inline
void	EIG_QRIterateOriginShift(_ResultMatType& _Result, const _MatType& _Mat, Int32 _Max = 100)
{
	typedef typename _MatType::NumberType NumberType;

	RT_CONDITION(_Result.row() == _Mat.row() && _Result.col() == 1 && _Mat.row() == _Mat.col());

	const Int32 n = _Mat.row();

	FullMatrix	t(n, n);
	FullMatrix	Q(n, n);
	FullMatrix	R(n, n);
	DiagMatrix	I(n, n, 1);

	t = _Mat;
	HessenBerg(t);

	for (Int32 i = 0; i < _Max; ++i)
	{
		Float Shift = t(MIDX(n), MIDX(n));
		t -= I * Shift;
		QR(Q, R, t);
		t = R * Q + I * Shift;
	}

	QR(Q, R, t);

	for (Int32 i = MIDX(1); i <= MIDX(n); ++i)
	{
		_Result(i) = R(i, i);
	}
}

/*
*
*	Rank
*	Algorithm		:
*					QR factorization
*	Type Condition  : 
*	Maths Condition :
*					_Mat : m * n, m >= n
*/

template<
		typename _MatType
		>
inline
Int32	RankImpl(_MatType& _Mat)
{
	typedef typename _MatType::NumberType	NumberType;

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

	const Int32 m = _Mat.row();
	const Int32 n = _Mat.col();

	DataBlock<NumberType>	temp_data(m);
	DataBlock<NumberType>	temp_data1(m);
	DataBlock<NumberType>	temp_data2(m*n);

	for (Int32 j = 0; j < m; ++j)
	{
		temp_data1[j] = 1;
	}

	for (Int32 j = MIDX(1); j <= MIDX(n); ++j)
	{
		FullMatrix	v(MIDX(m) - j + 1, 1, temp_data.Buffer(), 0);
		DiagMatrix	I(MIDX(m) - j + 1, MIDX(m) - j + 1, temp_data1.Buffer(), 0);
		FullMatrix	t(MIDX(m) - j + 1, MIDX(n) - j + 1, temp_data2.Buffer(), 0);
		Float	beta = 0;
		
		HouseHolder(v, beta, _Mat(j, MIDX(m), j, j));

		t = (I - beta * v * Tran(v)) * _Mat(j, MIDX(m), j, MIDX(n));

		_Mat(j, MIDX(m), j, MIDX(n)) = t;
	}

	Int32	Result = 0;

	for (Int32 i = MIDX(1); i <= MIDX(n); ++i)
	{
		if (ZeroCmp<NumberType>::Cmp(_Mat(i, i)) != 0)
		{
			++Result;
		}
	}

	return Result;
}

template<typename _MatType>
inline
Int32	Rank(const _MatType& _Mat)
{
	if (_Mat.row() >= _Mat.col())
	{
		FullMatrix temp(_Mat.row(), _Mat.col());
		temp = _Mat;
		return RankImpl(temp);
	}
	else
	{
		FullMatrix temp(_Mat.col(), _Mat.row());
		temp = Tran(_Mat);
		return RankImpl(temp);
	}
}

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

template<typename _MatType>
inline
typename _MatType::NumberType
Det(const _MatType& _Mat)
{
	typedef	typename _MatType::NumberType	NumberType;

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

	FullMatrix	temp(_Mat.row(), _Mat.col());

	const Int32 n = _Mat.row();
	NumberType Result = 1;

	temp = _Mat;

	for (int i = MIDX(1); i <= MIDX(n-1); ++i)
	{
		Float 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;
			}
		}

		if (i != pos)
		{
			temp.SwapRow(i, pos);
			Result = -Result;
		}

		Result *= 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;
		}
	}

	for (int i = MIDX(1); i <= MIDX(n); ++i)
	{
		Result *= temp(i, i);
	}
	
	return Result;
}

/*
*
*	Calculate the determinant of a matrix
*	Algorithm	    :
*					QR factorization
*	Type Condition  : 
*					none
*	Maths Condition :
*					_Mat : n * n
*
*/

template<typename _MatType>
inline
typename _MatType::NumberType
DetQR(const _MatType& _Mat)
{
	typedef	typename _MatType::NumberType	NumberType;

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

	FullMatrix	temp(_Mat.row(), _Mat.col());
	FullMatrix	d(_Mat.col());

	const Int32 n = _Mat.col();

	NumberType Result = 1;
	
	temp = _Mat;
	QR(d, temp);

	for (Int32 i = MIDX(1); i <= MIDX(n); ++i)
	{
		Result *= temp(i, i);
	}

	return Result;
}

SDL_MATHS_MATRIX_END

#endif