/********************************************************************
//	函數(shù)名稱:   Cal2DMartrixbyParam  
//	函數(shù)功能:   根據(jù)平移、旋轉(zhuǎn)參數(shù)移動坐標系，
//  入口參數(shù):   double pPara[]	：平移和旋轉(zhuǎn)量，順序為： x, y, angle 
//  返回參數(shù):   變換矩陣
//  創(chuàng)建日期:	2007/04/10
//  作者:       戚春燕
//  備注：		新坐標系原點在原坐標系的位置為（x,y)
                新坐標系的x軸正方向旋轉(zhuǎn)角度為angle(弧度）
				返回原坐標系到新坐標系的變換矩陣MO
*********************************************************************/
MartrixF CRServer::Cal2DMartrixbyParam(double pPara[])
{

	MartrixF TransM(3,3);
	TransM(0,0) = cos(pPara[2]);
	TransM(0,1) = sin(pPara[2]);
	TransM(0,2) = -(pPara[0] * cos(pPara[2]) + sin(pPara[2]) * pPara[1]);
	TransM(1,0) = -sin(pPara[2]);
	TransM(1,1) = cos(pPara[2]);
	TransM(1,2) = -(-pPara[0] * sin(pPara[2]) +pPara[1] * cos(pPara[2]));
	TransM(2,0) = 0;
	TransM(2,1) = 0;
	TransM(2,2) = 1;

	return TransM;

}

/*-----------------------------------------------------------------------------
* 函數(shù)名稱：	void CalMatByParam(double pPara[])
* 函數(shù)功能：	根據(jù)平移、旋轉(zhuǎn)參數(shù)移動坐標系
* 入口參數(shù)：	double pPara[]	：平移和旋轉(zhuǎn)量，順序為： x, y, z, rz, rx, ry 
* 返回參數(shù)：	CRegMatrix，求得的變換矩陣（新坐標系到原坐標系）
* 備注    ：	旋轉(zhuǎn)指的是繞數(shù)據(jù) (0, 0, 0) 點旋轉(zhuǎn)
-----------------------------------------------------------------------------*/
MartrixF CRServer::CalMatByParam(double pPara[])
{
	MartrixF matTran(4,4);

	matTran(0, 0) = double( cos(pPara[4])*cos(pPara[5]));
	matTran(0, 1) = double(-cos(pPara[3])*sin(pPara[5]) + sin(pPara[3])*sin(pPara[4])*cos(pPara[5]));
	matTran(0, 2) = double( sin(pPara[3])*sin(pPara[5]) + cos(pPara[3])*sin(pPara[4])*cos(pPara[5]));
	matTran(0, 3) = pPara[0];

	matTran(1, 0) = double( cos(pPara[4])*sin(pPara[5]));
	matTran(1, 1) = double( cos(pPara[3])*cos(pPara[5]) + sin(pPara[3])*sin(pPara[4])*sin(pPara[5]));
	matTran(1, 2) = double(-sin(pPara[3])*cos(pPara[5]) + cos(pPara[3])*sin(pPara[4])*sin(pPara[5]));
	matTran(1, 3) = pPara[1];

	matTran(2, 0) = double(-sin(pPara[4]));
	matTran(2, 1) = double( sin(pPara[3])*cos(pPara[4]));
	matTran(2, 2) = double( cos(pPara[3])*cos(pPara[4]));
	matTran(2, 3) = pPara[2];

	//////////////////////////////////////////////////////////////////////////

	matTran(3, 0) = 0.0f;
	matTran(3, 1) = 0.0f;
	matTran(3, 2) = 0.0f;
	matTran(3, 3) = 1.0f;

	//此時matTran為變換矩陣M，通過它可以計算某點經(jīng)旋轉(zhuǎn)平移變換的新坐標
	//其逆矩陣為原坐標系到新坐標系下的矩陣MO，通過它可以計算原坐標系下的點在新坐標系下的坐標
	return matTran.InverMartrix();
}