magicmathutil.cpp

Wild Math Library数值计算库

 #include "stdafx.h"
#include "magicMathUtil.h"

#ifndef PI
#define	PI	3.1415926
#endif

//Euler Anglex:yaw*pitch*roll
//Euler AnglexXYZ:RotateX*RotateY*RotateZ
//Euler AngleZYX:RotateZ*RotateY*RotateX

void PrintMatix(Wml::Matrix3<double>& RotateMatrix)
{
	int i;
	printf("\n");
	for(i=0;i<9;i++){
		printf("%.7lf\t",RotateMatrix(i/3,i%3));
		if((i+1)%3==0)
			printf("\n");
	}
}

void RayToRotationXY(double x,double y ,double z,Wml::Matrix3<double>& RotateMatrix)
{

	double sinAlpha,cosAlpha,sinBeta,cosBeta;
	double r,rxz;
	r=sqrt(x*x+y*y+z*z);
	rxz=sqrt(x*x+z*z);
	if(rxz<1e-9){
		sinAlpha = 1.0;
		cosAlpha = 0.0;
		sinBeta = 0.0;
		cosBeta = 1.0;
	}
	else{
	sinAlpha = -y/r;
	cosAlpha = rxz/r;
	sinBeta = x/rxz;
	cosBeta = z/rxz;
	}
	//RotateY*RotateX
//	[            cos(beta), sin(beta)*sin(alpha), sin(beta)*cos(alpha)]
//	[                    0,           cos(alpha),          -sin(alpha)]
//	[           -sin(beta), cos(beta)*sin(alpha), cos(beta)*cos(alpha)]
	RotateMatrix(0,0)=cosBeta;	RotateMatrix(0,1)=sinBeta*sinAlpha;	RotateMatrix(0,2)=sinBeta*cosAlpha;
	RotateMatrix(1,0)=0;		RotateMatrix(1,1)=cosAlpha;			RotateMatrix(1,2)=-sinAlpha;
	RotateMatrix(2,0)=-sinBeta;	RotateMatrix(2,1)=sinAlpha*cosBeta; RotateMatrix(2,2)=cosAlpha*cosBeta;
	
}

void Matrix3RotateWithAngle(Wml::Matrix3<double>& RotateMatrix,double x,double y,double z,double theta)
{
	Wml::Matrix3<double>	tmpMatrix;
	Wml::Vector3<double>	axis(-x,-y,-z);//轴向跟它的方向正好相反
	tmpMatrix.FromAxisAngle(axis,theta);
	RotateMatrix=tmpMatrix*RotateMatrix;
}

void AxbRotateToMatrix(double x,double y,double z,double theta,
					   Wml::Matrix3<double>& RotateMatrix)
{
	RayToRotationXY(x,y,z,RotateMatrix);
	Matrix3RotateWithAngle(RotateMatrix,x,y,z,theta);
}

void GetRotFromTransform(const Wml::Matrix4d& transformM,Wml::Matrix3d& rotM)
{
	int i,j;
	for(i=0;i<3;i++)
		for(j=0;j<3;j++){
			rotM(i,j)=transformM(i,j);
		}

}

Wml::Matrix3d GetRotFromTransform(const Wml::Matrix4d& transformM)
{
	int i,j;
	Wml::Matrix3d rotM;

	for(i=0;i<3;i++)
		for(j=0;j<3;j++){
			rotM(i,j)=transformM(i,j);
		}
	return rotM;
}

void GetTranslateFromTransform(const Wml::Matrix4d& transformM,Wml::Vector3d& translateGM)
{
	int i;

	for(i=0;i<3;i++)
		translateGM[i]=transformM(i,3);

}

Wml::Vector3d	GetTranslateFromTransform(const Wml::Matrix4d& transformM)
{
	int i;
	Wml::Vector3d	translateGM;

	for(i=0;i<3;i++)
		translateGM[i]=transformM(i,3);

	return translateGM;
}

void TransformFromRotAndTranslate(Wml::Matrix4d& transformM,const Wml::Matrix3d& rotM,
				  const Wml::Vector3d& translateGM)
{
	int i,j;

	for(i=0;i<3;i++)
		for(j=0;j<3;j++)
			transformM(i,j)=rotM(i,j);

	for(i=0;i<3;i++)
		transformM(i,3)=translateGM[i];

	for(j=0;j<3;j++)
		transformM(3,j)=0.0;

	transformM(3,3)=1.0;

}

Wml::Matrix4d TransformFromRotAndTranslate(const Wml::Matrix3d& rotM,
										  const Wml::Vector3d& translateGM)
{
	int i,j;
	Wml::Matrix4d transformM;

	for(i=0;i<3;i++)
		for(j=0;j<3;j++)
			transformM(i,j)=rotM(i,j);

	for(i=0;i<3;i++)
		transformM(i,3)=translateGM[i];

	for(j=0;j<3;j++)
		transformM(3,j)=0.0;

	transformM(3,3)=1.0;
	
	return transformM;
}


void ObjTransformToViewTransform(const Wml::Matrix4d& objTransform,Wml::Matrix4d& viewTransform)
{
	int i,j;
	Wml::Matrix3d rotM;
	Wml::Vector3d translateGM;

	GetRotFromTransform(objTransform,rotM);
	GetTranslateFromTransform(objTransform,translateGM);
	rotM=rotM.Transpose();
	translateGM = -rotM*translateGM;
	TransformFromRotAndTranslate(viewTransform,rotM,translateGM);
}