xmatrixfunc.cpp

XMathLib是一个通用的3D图形数学库。 其中包含两个部分： XMathLib和XGeomLib。分别处理数学和几何运算。 数学部分包含向量、矩阵、四元数的运算。以及其它的运算。 几何部分

/********************************************************************************
声明：
　　这里的代码版权归潘李亮所有．你可以自由使用分发这些代码．但是不得用于商业用途．
  如有需要请于作者联系．
  　在你使用本代码时候，请务必保留本声明
   　潘李亮　2003-10
	Stanly Lee. 2003-10

	Email : [e-mail]

	提供矩阵和向量等类的操作函数．这些函数不属于任何一个类．
	使用这些类可以加快运算速度．函数不需要返回．参数以引用
	的方式传递．




	*******************************************************************************/


#include "XMathLib.h"
namespace XMathLib
{

    void XMatrix3x3::From(XMatrix& _m)
    {
        for( int r = 0 ; r< 3; r++)
            for(int c = 0 ; c < 3 ; c++)
            {
                m[r][c] = _m.m[r][c];
            }
    }

    void XMatrix::From(XMatrix3x3& _m)
    {
        //效率不高。要改。但是必要不大
        LoadIdentity();
        for( int r = 0 ; r< 3; r++)
            for(int c = 0 ; c < 3 ; c++)
            {
                m[r][c] = _m.m[r][c];
            }
    }

	bool	_MATH_LIB_EXPORT_ XMatrix_Inv(XMatrix3x3& m1 , XMatrix3x3& mOut)
	{
		XMatrix3x3 m = m1;
		int max_r;
		//int max_c;
		float pivot = 1.0;
		mOut.LoadIdentity();
		for(int row = 0 ; row < 3 ; row++ )
		{
			//选主元．
			pivot = m.m[row][row];

			max_r = row;
			for( int i = row ; i < 3 ; i++ )
			{
				float t = (float)fabs(m.m[i][row]);

				if( t > fabs(pivot))
				{
					pivot = m.m[i][row];
					max_r = i;
				}
			}
			//主元选定．
			if(pivot == 0)
				return false;
			//交换行
			if(max_r != row)
			{
				XM_Swap(m.m[max_r][0],m.m[row][0]); 	XM_Swap(mOut.m[max_r][0],mOut.m[row][0]);
				XM_Swap(m.m[max_r][1],m.m[row][1]); 	XM_Swap(mOut.m[max_r][1],mOut.m[row][1]);
				XM_Swap(m.m[max_r][2],m.m[row][2]); 	XM_Swap(mOut.m[max_r][2],mOut.m[row][2]);
			}

			//归一化到1

			for( int x = 0 ; x < 3 ; x ++)
			{
				mOut.m[row][x] /= pivot;
				m.m[row][x] /= pivot; 
			}

			//消去当前列，除了当前行上的以外。该列全部成为0
			for(int y = 0 ; y < 3 ; y ++)
			{
				if(y == row )
					continue;
				float times = m.m[y][row];
				//消去。
				for (int x = 0 ; x < 3 ; x ++)
				{
					m.m[y][x] -= m.m[row][x] * times;  
					mOut.m[y][x] -= mOut.m[row][x] * times;
				}
			}

		}

		return true;

	}




	/*
	*	逆矩阵，采用高斯消去法．
	*/
	/*******************************************************************************
	This function is tested by Stanly Lee at 2003-11
	*******************************************************************************/
	bool _MATH_LIB_EXPORT_ XMatrix_Inv(XMatrix&m1 , XMatrix& mOut)
	{
		XMatrix m = m1;
		int max_r;
		//int max_c;
		float pivot = 1.0;
		mOut.LoadIdentity();
		for(int row = 0 ; row < 4 ; row++ )
		{
			//选主元．
			pivot = m.m[row][row];

			max_r = row;
			for( int i = row ; i < 4 ; i++ )
			{
				float t = (float)fabs(m.m[i][row]);

				if( t > fabs(pivot))
				{
					pivot = m.m[i][row];
					max_r = i;
				}
			}
			//主元选定．
			if(pivot == 0)
				return false;
			//交换行
			if(max_r != row)
			{
				XM_Swap(m.m[max_r][0],m.m[row][0]); 	XM_Swap(mOut.m[max_r][0],mOut.m[row][0]);
				XM_Swap(m.m[max_r][1],m.m[row][1]); 	XM_Swap(mOut.m[max_r][1],mOut.m[row][1]);
				XM_Swap(m.m[max_r][2],m.m[row][2]); 	XM_Swap(mOut.m[max_r][2],mOut.m[row][2]);
				XM_Swap(m.m[max_r][3],m.m[row][3]);  	XM_Swap(mOut.m[max_r][3],mOut.m[row][3]);
			}

			//归一化到1

			for( int x = 0 ; x < 4 ; x ++)
			{
				mOut.m[row][x] /= pivot;
				m.m[row][x] /= pivot; 
			}

			//消去当前列，除了当前行上的以外。该列全部成为0
			for(int y = 0 ; y < 4 ; y ++)
			{
				if(y == row )
					continue;
				float times = m.m[y][row];
				//消去。
				for (int x = 0 ; x < 4 ; x ++)
				{
					m.m[y][x] -= m.m[row][x] * times;  
					mOut.m[y][x] -= mOut.m[row][x] * times;
				}
			}

		}

		return true;
	}

	/*
	*	转置
	*/

	void _MATH_LIB_EXPORT_ XMatrix_Transpos(XMatrix&m , XMatrix& mOut)
	{
		for(int r = 0 ;  r < 4 ; r ++)
		{
			mOut.m[0][r] = m.m[r][0];
			mOut.m[1][r] = m.m[r][1];
			mOut.m[2][r] = m.m[r][2];
			mOut.m[3][r] = m.m[r][3];
		}
		return;
	}

/*
 *	经过该投影矩阵投影后，投影体为一个范体，

	       x( -1 , 1 )
		   y( -1 , 1 )
		   z( -1 , 1 )

*/

	void	_MATH_LIB_EXPORT_ XM_Perspective(XMatrix& mOut,float fov_angle,float aspect,float Znear,float Zfar)
	{
		float dz = Zfar - Znear;
		
		float yp = XM_CosD(fov_angle / 2 ) / XM_SinD(fov_angle / 2 ) ;
	    float xp = yp / aspect;

		float z1 = - (Zfar + Znear) / dz;
		float z2 = - 2*Zfar *  Znear / dz ;

		mOut.m[0][0] = xp ; mOut.m[0][1] = 0  ; mOut.m[0][2] = 0  ; mOut.m[0][3] =  0;
		mOut.m[1][0] = 0  ; mOut.m[1][1] = yp ; mOut.m[1][2] = 0  ; mOut.m[1][3] =  0;
		mOut.m[2][0] = 0  ; mOut.m[2][1] = 0  ; mOut.m[2][2] = z1 ; mOut.m[2][3] = -1;
		mOut.m[3][0] = 0  ; mOut.m[3][1] = 0  ; mOut.m[3][2] = z2 ; mOut.m[3][3] =  0;

		return ;

	}

/*
 *	正交投影
 　　　经过该投影矩阵后，物体大小不会变形．

	x  = x/w + dx;
	y  = y/h + dy;
	
	注意投影后的范体是(-1,1);
 */

	void	_MATH_LIB_EXPORT_ XM_Ortho3D(XMatrix& mOut,float Znear ,float Zfar ,float top,float bottom ,float left ,float right)
	{

		float cx = (right + left) / 2.0f ;
		float cy = (top   + bottom) / 2.0f ;
		float w = (float)fabs(right - left) / 2.0f ;
		float h = (float)fabs(top - bottom) / 2.0f ;

		float x = 1.0f/w;
		float dx = -cx/w;

		float y = 1.0f/h;
		float dy = -cy/h;

	    
		float z = -2.0f/(Zfar - Znear);
		float dz = -((Zfar + Znear)) / (Zfar - Znear);

		mOut.m[0][0] = x  ; mOut.m[0][1] = 0  ; mOut.m[0][2] = 0  ; mOut.m[0][3] = 0;
		mOut.m[1][0] = 0  ; mOut.m[1][1] = y  ; mOut.m[1][2] = 0  ; mOut.m[1][3] = 0;
		mOut.m[2][0] = 0  ; mOut.m[2][1] = 0  ; mOut.m[2][2] = z  ; mOut.m[2][3] = 0;
		mOut.m[3][0] = dx ; mOut.m[3][1] = dy ; mOut.m[3][2] = dz ; mOut.m[3][3] = 1;


		return ;
	}

	void	_MATH_LIB_EXPORT_ XM_Transform(XMatrix& mOut,float tx,float ty,float tz)
	{
		mOut.m[0][0] = 1  ; mOut.m[0][1] = 0  ; mOut.m[0][2] = 0  ; mOut.m[0][3] = 0;
		mOut.m[1][0] = 0  ; mOut.m[1][1] = 1  ; mOut.m[1][2] = 0  ; mOut.m[1][3] = 0;
		mOut.m[2][0] = 0  ; mOut.m[2][1] = 0  ; mOut.m[2][2] = 1  ; mOut.m[2][3] = 0;
		mOut.m[3][0] = tx ; mOut.m[3][1] = ty ; mOut.m[3][2] = tz ; mOut.m[3][3] = 1;
		return ;
	}
	void	_MATH_LIB_EXPORT_ XM_Scale(XMatrix& mOut,float sx,float sy,float sz)
	{
		mOut.m[0][0] = sx  ; mOut.m[0][1] = 0  ; mOut.m[0][2] = 0  ; mOut.m[0][3] = 0;
		mOut.m[1][0] = 0  ; mOut.m[1][1] = sy  ; mOut.m[1][2] = 0  ; mOut.m[1][3] = 0;
		mOut.m[2][0] = 0  ; mOut.m[2][1] = 0   ; mOut.m[2][2] = sz ; mOut.m[2][3] = 0;
		mOut.m[3][0] = 0  ; mOut.m[3][1] = 0   ; mOut.m[3][2] = 0  ; mOut.m[3][3] = 1;
		return ;
	}


	void	_MATH_LIB_EXPORT_ XM_RotateX(XMatrix& mOut,float angle)
	{
		float c = XM_CosD(angle);
		float s = XM_SinD(angle);
		mOut.m[0][0] = 1  ; mOut.m[0][1] = 0  ; mOut.m[0][2] = 0  ; mOut.m[0][3] = 0;
		mOut.m[1][0] = 0  ; mOut.m[1][1] = c  ; mOut.m[1][2] = s  ; mOut.m[1][3] = 0;
		mOut.m[2][0] = 0  ; mOut.m[2][1] = -s ; mOut.m[2][2] = c  ; mOut.m[2][3] = 0;
		mOut.m[3][0] = 0  ; mOut.m[3][1] = 0  ; mOut.m[3][2] = 0  ; mOut.m[3][3] = 1;
		return ;
	}


	void	_MATH_LIB_EXPORT_ XM_RotateY(XMatrix& mOut,float angle)
	{
		float c = XM_CosD(angle);
		float s = XM_SinD(angle);
		mOut.m[0][0] = c  ; mOut.m[0][1] = 0  ; mOut.m[0][2] = -s  ; mOut.m[0][3] = 0;
		mOut.m[1][0] = 0  ; mOut.m[1][1] = 1  ; mOut.m[1][2] =  0  ; mOut.m[1][3] = 0;
		mOut.m[2][0] = s  ; mOut.m[2][1] = 0  ; mOut.m[2][2] =  c  ; mOut.m[2][3] = 0;
		mOut.m[3][0] = 0  ; mOut.m[3][1] = 0  ; mOut.m[3][2] =  0  ; mOut.m[3][3] = 1;
		return ;
    }

	void	_MATH_LIB_EXPORT_ XM_RotateZ(XMatrix& mOut,float angle)
	{
		float c = XM_CosD(angle);
		float s = XM_SinD(angle);
		mOut.m[0][0] = c  ; mOut.m[0][1] = s  ; mOut.m[0][2] = 0  ; mOut.m[0][3] = 0;
		mOut.m[1][0] = -s ; mOut.m[1][1] = c  ; mOut.m[1][2] = 0  ; mOut.m[1][3] = 0;
		mOut.m[2][0] = 0  ; mOut.m[2][1] = 0  ; mOut.m[2][2] = 1  ; mOut.m[2][3] = 0;
		mOut.m[3][0] = 0  ; mOut.m[3][1] = 0  ; mOut.m[3][2] = 0  ; mOut.m[3][3] = 1;
		return ;

	}


	/*******************************************************************************
	This function is tested by Stanly Lee at 2003-11

	*******************************************************************************/
	void _MATH_LIB_EXPORT_ XM_RotatePV(XMatrix& mOut,XVector& v , float angle)
	{
		float vsin = XM_SinD(angle);
		float vcos = XM_CosD(angle);

		float a = v.x * vsin;
		float b = v.y * vsin;
		float c = v.z * vsin;

		mOut.m[0][0] = vcos  ; mOut.m[0][1] = c     ; mOut.m[0][2] = -b    ; mOut.m[0][3] = 0;
		mOut.m[1][0] = -c    ; mOut.m[1][1] = vcos  ; mOut.m[1][2] = a     ; mOut.m[1][3] = 0;
		mOut.m[2][0] = b     ; mOut.m[2][1] = -a    ; mOut.m[2][2] = vcos  ; mOut.m[2][3] = 0;
		mOut.m[3][0] = 0     ; mOut.m[3][1] = 0     ; mOut.m[3][2] = 0     ; mOut.m[3][3] = 1;
		return ;
		
	}


	/*******************************************************************************
	This function is tested by Stanly Lee at 2003-11
	*******************************************************************************/
	void _MATH_LIB_EXPORT_ XM_Rotate(XMatrix& mOut,XVector& v , float angle)
	{
		//This code and algrithm is copy from the mesaLib4.0
		float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c, s, c;


		float x = v.x;
		float y = v.y;
		float z = v.z;

		s = (float) XM_SinD( angle  );
		c = (float) XM_CosD( angle  );

		mOut.LoadIdentity();

#define M(row,col)  mOut.m [row ] [ col ]

			float mag = (float) sqrt(x * x + y * y + z * z);

			if (mag <= 1.0e-4) 
			{
				/* no rotation, leave mat as-is */
				return;
			}

			x /= mag;
			y /= mag;
			z /= mag;


			xx = x * x;		yy = y * y;
			zz = z * z;		xy = x * y;
			yz = y * z;		zx = z * x;
			xs = x * s;		ys = y * s;
			zs = z * s;		one_c = 1.0F - c;

			/* We already hold the identity-matrix so we can skip some statements */
			M(0,0) = (one_c * xx) + c;
			M(1,0) = (one_c * xy) - zs;
			M(2,0) = (one_c * zx) + ys;
		/*  M(3,0) = 0.0F; */

			M(0,1) = (one_c * xy) + zs;
			M(1,1) = (one_c * yy) + c;
			M(2,1) = (one_c * yz) - xs;
		/*  M(3,1) = 0.0F; */

			M(0,2) = (one_c * zx) - ys;
			M(1,2) = (one_c * yz) + xs;
			M(2,2) = (one_c * zz) + c;
		/*  M(3,2) = 0.0F; */

		/*
			M(0,3) = 0.0F;
			M(1,3) = 0.0F;
			M(2,3) = 0.0F;
			M(3,3) = 1.0F;
		*/
#undef M
 
	}



    /*******************************************************************************
		This function is tested by Stanly Lee at 2003-11
	*******************************************************************************/
	void	_MATH_LIB_EXPORT_ XM_LookAt(XMatrix& mOut,XVector& eyeAt,XVector eyeTarget,XVector& upDir)
	{
		XVector vZ =   eyeAt - eyeTarget;//注意这里是因为OpenGL用的you手系．默认的方向是向着Z的负方向．
		XVector vX =   upDir.cp(vZ);

		vX.normalize();
		vZ.normalize();
		XVector vY =   vZ.cp(vX);//重新计算向上的方向．我们可以保证vY也是个单位向量，就不需要再做单位化了。

		#define M(row,col)  mOut.m[row ] [ col ]

		M(0,0) = vX.x;
		M(1,0) = vX.y;
		M(2,0) = vX.z;
		M(3,0) = - vX.dp3(eyeAt) ; 


		M(0,1) =  vY.x;
		M(1,1) =  vY.y;
		M(2,1) =  vY.z;
		M(3,1) =  -vY.dp3(eyeAt) ; 

		M(0,2) =  vZ.x;
		M(1,2) =  vZ.y;
		M(2,2) =  vZ.z;
		M(3,2) =  -vZ.dp3(eyeAt) ; 

		M(0,3) = 0;
		M(1,3) = 0;
		M(2,3) = 0;
		M(3,3) = 1;
		#undef M


	}
};