//////////////////////////////////////////////////////////////////////
// 求實對稱矩陣的特征值和特征向量									 //
// 使用HouseHolder約化和因子分解方法								 //
// 開發: 余家忠														//
// 時間: 2000年6月27日												//
/////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "math.h"
#include "EigenSystem.h"

// 輔助函數,根據參數b的值來確定返回值a的符號
double Sign(double a,double b)
{
	return ( b>=0.0 ? fabs(a) : -fabs(a) );
}

// 輔助函數,求參數a和b的平方和的平方根
double Pythag(double a,double b)
{
	return sqrt( a*a + b*b );
}

// 輔助函數,利用HouseHolder變換將實對稱矩陣約化為三對角矩陣.
// symMatrix為實對稱矩陣,大小為n*n.輸出時,被產生變換的正交矩陣取代,
// d[n]返回三對角矩陣的對角元素,
// e[n]返回非對角元素,且有e[0]=0.
void Simplify(double * symMatrix,int n, double * d, double * e)
{
	int i,j,k,m;
	double scale,hh,h,g,f;

	ASSERT(symMatrix!=NULL && d!=NULL && e!=NULL);

	for( i=n-1; i>0; i-- )
	{
		m = i-1;
		h = scale = 0.0;
		if( m > 0 )
		{
			for( k=0; k<=m; k++)
				scale += fabs(symMatrix[i*n+k]);
			if( scale == 0.0 )	// 跳過變換
				e[i] = symMatrix[i*n+m];
			else
			{
				for( k=0; k<=m; k++ )
				{
					// 將標定的a用于變換
					symMatrix[i*n+k] /= scale;
					// 在h中形成delta
					h += symMatrix[i*n+k]*symMatrix[i*n+k];
				}
				f = symMatrix[i*n+m];
				g = ( f>=0.0 ? -sqrt(h) : sqrt(h) );
				e[i] = scale*g;
				h -= f*g;
				symMatrix[i*n+m] = f-g;	// 將u存入symMatrix的第i行
				f = 0.0;
				for(j=0; j<=m; j++)
				{
					// 將u/H存入symMatrix的第i列
					symMatrix[j*n+i] = symMatrix[i*n+j]/h;
					g = 0.0;	// 在g中A*u的一個元素
					for(k=0; k<=j; k++)
						g += symMatrix[j*n+k]*symMatrix[i*n+k];
					for(k=j+1; k<=m; k++)
						g += symMatrix[k*n+j]*symMatrix[i*n+k];
					e[j] = g/h;	// 在e的暫時不用的元素中形成p的元素

					f += e[j]*symMatrix[i*n+j];
				}

				hh = f/(h+h);	// 形成K
				for(j=0; j<=m; j++)	// 形成q,并存入e中p的位置上
				{
					f = symMatrix[i*n+j];
					e[j] = g = e[j]-hh*f;	// 注意:有e[m]=e[i-1]
					for(k=0; k<=j; k++)		// 約化symMatrix
						symMatrix[j*n+k] -= (f*e[k]+g*symMatrix[i*n+k]);
				}
			}
		}
		else
			e[i] = symMatrix[i*n+m];
		d[i] = h;
	}

	d[0] = 0.0;
	e[0] = 0.0;
	for(i=0; i<n; i++)		// 開始矩陣變換的積累
	{
		m = i-1;
		if(d[i])			// 當i=0時跳過這一塊
		{
			for(j=0; j<=m; j++)
			{
				g = 0.0;
				
				// 利用symMatrix中存儲的u和u/H來形成P*Q
				for(k=0; k<=m; k++)
					g += symMatrix[i*n+k]*symMatrix[k*n+j];
				for(k=0; k<=m; k++) 
					symMatrix[k*n+j] -= g*symMatrix[k*n+i];
			}
		}

		d[i] = symMatrix[i*n+i];

		// 為下一次迭代將symMatrix重新置成單位矩陣
		symMatrix[i*n+i] = 1.0;
		for(j=0; j<=m; j++)	symMatrix[j*n+i] = symMatrix[i*n+j] = 0.0;
	}
}

// 利用因子分解的方法求三對角矩陣的特征值和特征向量.
// 采用隱含位移的QL算法(正交、下三角)。輸入時，d[1...n]
// 為三對角矩陣的對角元素，輸出時，它返回特征值。e[1...n]
// 為輸入三對角矩陣的次對角元素值，e[1]為任意值，輸出時，
// e中的值已背破壞。如果需要求一個三對角矩陣的特征向量，則
// 矩陣tMatrix[1...n][1...n]中輸入單位矩陣;如果需要求一個由tred2
// 已約化的矩陣之特征向量,則tMatrix由tred2輸出的矩陣作為輸入. 不管
// 在哪種情況下,tMatrix的第k列返回與d[k]相對應的歸一化特征向量.
//
void TQLI(double * tMatrix,int n,double * d,double * e)
{
	int i,j,k,m,iter;
	double s,r,p,g,f,dd,c,b;

	ASSERT( tMatrix!=NULL && d!=NULL && e!=NULL );

	// 為方便,重編e中的元素
	for(i=1; i<n; i++)
		e[i-1] = e[i];
	e[n-1] = 0.0;

	for( j=0; j<n; j++)
	{
		iter = 0;
		do{
			// 尋找一個單一的小的次對角元素用來分裂矩陣
			for(m=j; m<n-1; m++)
			{
				dd = fabs(d[m])+fabs(d[m+1]);
				if((float)(fabs(e[m])+dd) == dd) break;
			}
			if( m != j )
			{
				if(iter++ == 30)
				{
					AfxMessageBox("Too many iterations!\nThe result is false!");
					return;
				}
				g = (d[j+1]-d[j])/(2.0*e[j]);	// 形成位移
				r = Pythag(g,1.0);
				g = d[m]-d[j]+e[j]/(g+Sign(r,g));	// 這是dm-ds
				s = c = 1.0;
				p = 0.0;
				for(i=m-1; i>=j; i--)	// 如同原來QL方法一樣的平面轉動,
				{						// 隨后吉文斯旋轉,用以恢復三對角形式.
					f = s*e[i];
					b = c*e[i];
					e[i+1] = ( r = Pythag(f,g));
					if(r == 0.0)		// 從下溢處恢復
					{
						d[i+1] -= p;
						e[m] = 0.0;
						break;
					}
					s = f/r;
					c = g/r;
					g = d[i+1]-p;
					r = (d[i]-g)*s+2.0*c*b;
					d[i+1] = g+(p = s*r);
					g = c*r-b;
					for(k=0; k<n; k++)	// 形成特征向量
					{
						f = tMatrix[k*n+i+1];
						tMatrix[k*n+i+1] = s*tMatrix[k*n+i]+c*f;
						tMatrix[k*n+i] = c*tMatrix[k*n+i]-s*f;
					}
				}
				if(r == 0.0 && i >= j) continue;
				d[j] -= p;
				e[j] = g;
				e[m] = 0.0;
			}
		}while(m != j);
	}
}