// Matrix.inl		矩陣模板類函數(shù)(方法)定義
// Ver 1.0.0.0
// 版權(quán)所有(C) 何渝, 2002
// 最后修改: 2002.5.31

#ifndef _MATRIX_INL
#define _MATRIX_INL

//矩陣乘法函數(shù)
template <class _Tyout, class _Tylhs, class _Tyrhs>	//最后結(jié)果在mOut中
matrix<_Tyout>& MatrixMultiply(matrix<_Tyout>& mOut, const matrix<_Tylhs>& lhs, const matrix<_Tyrhs>& rhs)
{	//斷定左邊矩陣的列數(shù)與右邊矩陣的行數(shù)相等
	Assert(lhs.GetColNum() == rhs.GetRowNum());
	//生成矩陣新對(duì)象，用lhs的行作為新陣的行數(shù)，用rhs的列數(shù)作為新陣的列數(shù)
	matrix<_Tyout> mTmp(lhs.GetRowNum(), rhs.GetColNum());

	for(size_t i = 0; i < mTmp.GetRowNum(); i ++)
	{
		for(size_t j = 0; j < mTmp.GetColNum(); j ++)
		{
			mTmp(i, j) = _Tyout(0);		//賦初值0
			for(size_t k = 0; k < lhs.GetColNum(); k ++)
			{
				mTmp(i, j) += lhs(i, k) * rhs(k, j);
			}
		}
	}

	mOut = mTmp;		//將最后結(jié)果轉(zhuǎn)放入mOut矩陣中
	return mOut;		//返回結(jié)果矩陣mOut
}

//輸出矩陣函數(shù)		按一行一行進(jìn)行輸出
template <class _Ty>
void MatrixLinePrint(const matrix<_Ty>& mOut)
{	
	size_t sR, sC;
	sR=mOut.GetRowNum();
	sC=mOut.GetColNum();

	for(size_t stR=0; stR<mOut.GetRowNum(); stR++)
	{
		for(size_t stC=0; stC<mOut.GetColNum(); stC++)
		{
			cout.width(15);				//元素對(duì)齊，讓每個(gè)元素占15列
			cout << mOut(stR, stC) << ' ';
		}
		cout << endl;
	}
}

//輸出矩陣函數(shù)		按指定行進(jìn)行輸出
template <class _Ty>
void MatrixLinePrint(const matrix<_Ty>& mOut, size_t LineNo)
{	
	size_t sR, sC;

	sR=mOut.GetRowNum();
	sC=mOut.GetColNum();
	
	for(size_t stC=0; stC<mOut.GetColNum(); stC++)
	{
		cout.width(15);				//元素對(duì)齊，讓每個(gè)元素占15列
		cout << mOut(LineNo, stC) << ' ';
	}
	cout << endl;
}

//矩陣轉(zhuǎn)置	==	原陣在mIn，轉(zhuǎn)置后的矩陣在mOut  ==
template <class _Ty>
void MatrixTranspose(matrix<_Ty>& mIn, matrix<_Ty>& mOut)
{
	size_t sR, sC;
	sR = mIn.GetRowNum();			//取原矩陣行數(shù)
	sC = mIn.GetColNum();			//取原矩陣列數(shù)

	matrix<_Ty> mTemp(sC, sR);		//生成一新陣，行數(shù)與列數(shù)與原陣互換
	
	for(size_t stC=0; stC<sC; stC++)
		for(size_t stR=0; stR<sR; stR++)
			mTemp(stC, stR) = mIn(stR, stC);	//對(duì)新陣賦值

	mOut = mTemp;					//返回新的轉(zhuǎn)置陣
}

//判斷矩陣對(duì)稱
template <class _Ty>	
bool MatrixSymmetry(const matrix<_Ty>& rhs)
{
	bool bSy = true;
	size_t stRow = rhs.GetRowNum();	//取矩陣行數(shù)

	if(rhs.GetColNum() == stRow)	// 必須是方陣
	{
		for(size_t i = 1; i < stRow; i ++)			//判斷是否對(duì)稱
			for(size_t j = 0; j < i; j ++)
				if(FloatNotEqual((long double)rhs(i, j), (long double)rhs(j, i)))
				{
					bSy =  false;
					goto RET;
				}
	}
	else
		bSy = false;
	RET: return bSy;	//矩陣對(duì)稱
}

//判斷矩陣對(duì)稱正定	
template <class _Ty>	
int MatrixSymmetryRegular(const matrix<_Ty>& rhs, int sym)
{							
	long double ldDet;
	size_t i, j, k;

	size_t sC = rhs.GetColNum();	//矩陣列數(shù)
	size_t sR = rhs.GetRowNum();	//矩陣行數(shù)

	size_t stRank = sR;				// 矩陣階數(shù)
	if(stRank != rhs.GetRowNum())
		return int(-1);				// 不是方陣

	if(sym > 0)
		if(MatrixSymmetry(rhs)==false)
			return int(-2);			//rhs不是對(duì)稱陣

	cout << " K = 1 \t Determinant = " << rhs(0, 0) <<endl;
	
	for(k = 0; k < stRank; k ++)	//若要判別半正定，負(fù)定，這句要修改
	{
		if(FloatEqual(rhs(k, k), 0)||rhs(k, k) < 0)
			return int(-3);	//對(duì)角元不大于0，矩陣不是正定陣
	}

	for(k = 2; k <= sR; k++)
	{
		matrix<long double> m(k, k);	//生成一matrix對(duì)象

		for(i=0; i<k; i++)
			for(j=0; j<k; j++)
				m(i, j) = (long double)rhs(i, j);	//初始化

		ldDet = MatrixDeterminant(m);				// 順序主子式的值

		cout << " K = " << k << "\t Determinant = " << ldDet << endl; 
		
		if(FloatEqual(ldDet,0) || ldDet < 0.0)
			return (0);					//不是正定陣
	}
	if(sym == 1) return int(2);			//矩陣為正定對(duì)稱陣
	else		 return int(1);			//矩陣為正定陣
}

//全選主元法求矩陣行列式函數(shù)
template <class _Ty>
long double MatrixDeterminant(const matrix<_Ty>& rhs)		
{	
	long double  MaxValue, tmp;
	size_t stRank = rhs.GetColNum();	// 矩陣階數(shù)
	if(stRank != rhs.GetRowNum())
		return long double(0);			//rhs不是方陣

	matrix<long double> m(stRank, stRank);	//生成一matrix對(duì)象

	for(size_t i=0; i<stRank; i++)
		for(size_t j=0; j<stRank; j++)
			m(i, j) = (long double)rhs(i, j);	//初始化

	size_t iSign, jSign;				// 主元的位置標(biāo)志

	long double Det(1);					// 行列式的值

	int	nSgn = 1;						// 符號(hào)

	for(size_t k = 0; k < stRank - 1; k ++)	// 全選主元
	{	
		MaxValue = 0.0;
		for(i = k; i < stRank; i ++)
		{
			for(size_t j = k; j < stRank; j ++)
			{
				tmp = Abs(m(i, j));		//求m(i,j)絕對(duì)值
				if(tmp > MaxValue)
				{
					MaxValue = tmp;
					iSign = i;			//記下主元位置
					jSign = j;
				}
			}
		}
		
		if(FloatEqual(MaxValue, 0))	//絕對(duì)值最大元素為0，行列式也為0
			return long double(0);

		if(iSign != k)			//絕對(duì)值最大元素不在當(dāng)前行
		{
			nSgn = -nSgn;		//變換行列式符號(hào)
			for(size_t j = k; j < stRank; j ++)		//交換行
				swap(m(k, j), m(iSign, j));
		}

		if(jSign != k)			//絕對(duì)值最大元素不在當(dāng)前列
		{
			nSgn = -nSgn;		//變換行列式符號(hào)
			for(size_t i = k; i < stRank; i ++)		//交換列
				swap(m(i, jSign), m(i, k));
		}

		Det *= m(k, k);					//對(duì)角元相乘
		for(i = k + 1; i < stRank; i ++)
		{
			long double d(m(i, k) / m(k, k));		//消元因子
			for(size_t j = k + 1; j < stRank; j ++)	//將主元下方元素消為0
				m(i, j) -= d * m(k, j);	//當(dāng)前主元行下行其余元素作變換
		}
	}

	return Det * nSgn * m(stRank - 1, stRank - 1);	//返回行列式數(shù)值
}

//全選主元高斯(Gauss)消去法求一般矩陣的秩
template <class _Ty>						//返回值為秩數(shù)
size_t MatrixRank(const matrix<_Ty>& rhs)
{
	size_t iSign, jSign;					//主元的位置標(biāo)志
	size_t mRank = 0;						//矩陣秩數(shù)
	size_t stRow = rhs.GetRowNum();			//取矩陣行數(shù)
	size_t stCol = rhs.GetColNum();			//取矩陣列數(shù)
	size_t ColRowMin = Min(stRow, stCol);	//取行或列最小值

	matrix<_Ty> m(rhs);				//生成一matrix對(duì)象，用rhs初始化

	for(size_t k = 0; k < ColRowMin; k ++)
	{	// 全選主元
		long double MaxValue(0);
		for(size_t i = k; i < stRow; i ++)
		{
			for(size_t j = k; j < stCol; j ++)
			{
				long double tmp(Abs(m(i, j)));	//求m(i,j)絕對(duì)值
				if(tmp > MaxValue)
				{
					MaxValue = tmp;
					iSign = i;					//記下主元位置
					jSign = j;
				}
			}
		}
		
		//子陣元素絕對(duì)值最大者為0，	注意浮點(diǎn)數(shù)與0相等的定義，見comm.h
		if(FloatEqual(MaxValue, 0)) 
			break;	//return mRank;
		else				
			mRank++;		//子陣元素絕對(duì)值最大者不為0，矩陣秩加1

		if(k ==(ColRowMin - 1))		//已到最后一行(列)
			break;	//return mRank;
		
		if(iSign != k)				//主元不在當(dāng)前行
		{
			for(size_t j = k; j < stCol; j ++)	//交換行元素
				swap(m(k, j), m(iSign, j));
		}

		if(jSign != k)				//主元不在當(dāng)前列
		{
			for(size_t i = k; i < stRow; i ++)	//交換列元素
				swap(m(i, jSign), m(i, k));
		}

		for(i = k + 1; i < stRow; i ++)
		{
			const _Ty d(m(i, k) / m(k, k));		//消元因子
			for(size_t j = k + 1; j < stCol; j ++)	
				m(i, j) -= d * m(k, j);		//當(dāng)前主元右下陣元素作變換
		}
	}
	return mRank;
}

//全選主元高斯-約當(dāng)(Gauss-Jordan)法求矩陣逆
template <class _Ty>
int MatrixInversionGS(matrix<_Ty >& rhs)
{
	size_t stRank = rhs.GetColNum();	// 矩陣階數(shù)
	if(stRank != rhs.GetRowNum())
		return int(-1);					//rhs不是方陣

	valarray<size_t> is(stRank);		//行交換信息
	valarray<size_t> js(stRank);		//列交換信息
	
	matrix<_Ty> m(rhs);					//生成一matrix對(duì)象

	for(size_t k = 0; k < stRank; k++)
	{	// 全選主元
		long double MaxValue(0);
		for(size_t i = k; i < stRank; i ++)
		{
			for(size_t j = k; j < stRank; j ++)
			{
				long double tmp(Abs(m(i, j)));	//求m(i,j)絕對(duì)值
				if(tmp > MaxValue)				//主元不在對(duì)角線上
				{
					MaxValue = tmp;
					is[k] = i;					//記錄主元行數(shù)
					js[k] = j;					//記錄主元列數(shù)
				}
			}
		}
		
		if(FloatEqual(MaxValue, 0)) 
			return int(0);						//主元為0，矩陣奇異
		
		if(is[k] != k)							//主元不在當(dāng)前行
		{
			for(size_t j = 0; j < stRank; j ++)	//交換行元素
				swap(m(k, j), m(is[k], j));
		}

		if(js[k] != k)							//主元不在當(dāng)前列
		{
			for(size_t i = 0; i < stRank; i ++)	//交換列元素
				swap(m(i, k), m(i, js[k]));
		}

		m(k, k) = 1.0 / m(k, k);				//主元倒數(shù)
		for(size_t j = 0; j < stRank; j ++)
			if(j != k)
				m(k, j) *= m(k, k);

		for(i = 0; i < stRank; i ++)
			if(i != k)
				for(size_t j = 0; j < stRank; j ++)	
					if(j != k)
						m(i, j) = m(i, j) - m(i, k) * m(k, j);

		for(i = 0; i < stRank; i ++)
			if(i != k)
				m(i, k) = -m(i, k) * m(k, k);
	}

	for(int r = stRank - 1; r >= 0; r--)
	{
		if(js[r] != r)
			for(size_t j = 0; j < stRank; j ++)
				swap(m(r, j), m(js[r], j));
		if(is[r] != r)
			for(size_t i = 0; i < stRank; i ++)
				swap(m(i, r), m(i, is[r]));
	}

	rhs = m;

	return int(1);
}

//用“變量循環(huán)重新編號(hào)法”求對(duì)稱正定矩陣逆
//矩陣類型必須是浮點(diǎn)型
template <class _Ty>
int MatrixSymmetryRegularInversion(matrix<_Ty>& rhs)
{
	int iSym = MatrixSymmetryRegular(rhs, 1);	//判別是否對(duì)稱正定
	if(iSym < 2)
		return (iSym);			//rhs不是對(duì)稱正定陣

	size_t stRank = rhs.GetColNum();	// 矩陣階數(shù)
	
	matrix<_Ty> msr(rhs);			//生成一matrix對(duì)象，用rhs初始化

	valarray<_Ty> b(stRank);
	
	for(size_t k=0; k<stRank; k++)
	{
		_Ty w= msr(0, 0);
		size_t m = stRank - k -1;
		for(size_t i = 1; i < stRank; i++)
		{
			_Ty g = msr(i, 0);
			b[i] = g / w;
			if(i <= m) b[i] = -b[i];
			for(size_t j = 1; j <= i; j ++)
				msr((i-1),(j-1)) = msr(i, j) + g * b[j];
		}
		msr(stRank-1, stRank-1) = 1.0 / w;
		for(i = 1; i < stRank; i ++)
			msr(stRank-1,(i-1)) =  b[i];
	}
	for(size_t i = 0; i < stRank-1; i ++)
		for(size_t j = i+1; j < stRank; j ++)
			msr(i,j) = msr(j, i);

	rhs = msr;

	return (iSym);
}


//特蘭持(Trench)法求托伯利茲(Toeplitz)矩陣逆
//矩陣類型必須是浮點(diǎn)型
template <class _Ty>
int MatrixToeplitzInversionTrench(const valarray<_Ty>& t, const valarray<_Ty>& tuo, matrix<_Ty>& rhs)
{
	size_t stRank = rhs.GetColNum();	// 矩陣階數(shù)
	if(stRank != rhs.GetRowNum())
		return int(-1);					//rhs不是方陣

	if(FloatEqual(t[0], 0))
		return int(0);	

	valarray<_Ty> c(stRank);
	valarray<_Ty> r(stRank);
	valarray<_Ty> p(stRank);

	_Ty a=t[0]; 
	c[0]=tuo[1]/t[0];
	r[0]=t[1]/t[0];

	matrix<_Ty> b(rhs);
	for(size_t k=0; k<=stRank-3; k++)
	{
		_Ty s=0.0;
		for(size_t j=1; j<=k+1; j++)
			s=s+c[k+1-j]*tuo[j];
		s=(s-tuo[k+2])/a;
		for(size_t i=0; i<=k; i++)
			p[i]=c[i]+s*r[k-i];
        c[k+1]=-s;
        s=0.0;
        for(j=1; j<=k+1; j++)
			s=s+r[k+1-j]*t[j];
        s=(s-t[k+2])/a;
        for(i=0; i<=k; i++)
        {
			r[i]=r[i]+s*c[k-i];
            c[k-i]=p[k-i];
        }
        r[k+1]=-s;
		a=0.0;
        for(j=1; j<=k+2; j++)
			a=a+t[j]*c[j-1];
        a=t[0]-a;
		if(FloatEqual(a, 0))
			return int(0);	
    }
    b(0,0)=1.0/a;
    for(size_t i=0; i<stRank-1; i++)
    {	
		k=i+1;
        b(0, k)=-r[i]/a;
		b(i+1,0)=-c[i]/a;
    }
    for(i=0; i<stRank-1; i++)
    for(size_t j=0; j<stRank-1; j++)
        b(i+1, j+1)=b(i,j)-c[i]*b(0,j+1)+c[stRank-j-2]*b(0,stRank-i-1);

	rhs = b;
    
    return int(1);
}

//實(shí)矩陣LU分解
//矩陣必須是浮點(diǎn)型
template <class _Ty>
int MatrixLU(const matrix<_Ty>& rhs, matrix<_Ty>& lhs, matrix<_Ty>& uhs)
{
	size_t stRank = rhs.GetColNum();	// 矩陣階數(shù)
	if(stRank != rhs.GetRowNum())
		return int(-1);			//rhs不是方陣
	
	matrix<_Ty> m(rhs);
	for(size_t k=0; k<stRank-1; k++)
	{
		if(FloatEqual(m(k,k),0))
			return int(0);		//主元為0
		for(size_t i=k+1; i<stRank; i++)
			m(i,k) /= m(k,k);
		for(i=k+1; i<stRank; i++)
			for(size_t j=k+1; j<stRank; j++)
				m(i,j)=m(i,j)-m(i,k)*m(k,j);
	}
	//給上、下三角陣賦值
	for(size_t i=0; i<stRank; i++)
	{
		for(size_t j=0; j<i; j++)
		{
			lhs(i,j)=m(i,j);
			uhs(i,j)=0.0;
		}
		lhs(i,i)=1.0;
		uhs(i,i)=m(i,i);
		for(j=i+1; j<stRank; j++)
		{
			lhs(i,j)=0.0;
			uhs(i,j)=m(i,j);
		}
	}
	
	return (1);		//分解成功
}

//用豪斯荷爾德(Householder)變換對(duì)一般m*n階的實(shí)矩陣進(jìn)行QR分解
template <class _Ty>
int MatrixQR(matrix<_Ty>& rhs, matrix<_Ty>& rhq)
{
	size_t stRow = rhs.GetRowNum();	// 矩陣行數(shù)
	size_t stCol = rhs.GetColNum();	// 矩陣列數(shù)
	
	if(stRow < stCol)
		return (0);					//行不能小于列

	for(size_t i=0; i<stRow; i++)
    for(size_t j=0; j<stRow; j++)
    { 
		rhq(i,j)=0.0;
        if(i==j) rhq(i,j)=1.0;
    }

	size_t nn=stCol;
    
	if(stRow == stCol) nn=stRow-1;

	for(size_t k=0; k<nn; k++)
    {
		_Ty u=0.0;

        for(size_t i = k; i < stRow; i++)
        { 
			_Ty w = Abs(rhs(i,k));
            if(w > u) u = w;
        }
        _Ty alpha=0.0;
        for(i = k; i < stRow; i++)