matrix.cpp

liu7788414

#include "matrix.h"
#include "..\dynamics\mymath.h"
/*

void CMatrix44::Load(const CMatrix44 *Sm)
{
	SYS_ASSERT(HALF_PRECISION == (COS_SIN_MUL >> 1));

	m[0] = Sm->m[0];
	m[1] = Sm->m[1];
	m[2] = Sm->m[2];
	m[3] = Sm->m[3];
	m[4] = Sm->m[4];
	m[5] = Sm->m[5];
	m[6] = Sm->m[6];
	m[7] = Sm->m[7];
	m[8] = Sm->m[8];
	m[9] = Sm->m[9];
	m[10] = Sm->m[10];
	m[11] = Sm->m[11];

	MEMCPY( &m, sm.m, sizeof(m));
}
*/

// matrix multiply
void CMatrix44::GetProduct(const CMatrix44 *M1, const CMatrix44 *M2)
{
	m[INDEX(0,0)] = ((((int)M1->m[INDEX(0,0)] * M2->m[INDEX(0,0)] + (int)M1->m[INDEX(0,1)] * M2->m[INDEX(1,0)]) + (int)M1->m[INDEX(0,2)] * M2->m[INDEX(2,0)] ) >> COS_SIN_SHIFT );
	m[INDEX(0,1)] = ((((int)M1->m[INDEX(0,0)] * M2->m[INDEX(0,1)] + (int)M1->m[INDEX(0,1)] * M2->m[INDEX(1,1)]) + (int)M1->m[INDEX(0,2)] * M2->m[INDEX(2,1)] ) >> COS_SIN_SHIFT );
	m[INDEX(0,2)] = ((((int)M1->m[INDEX(0,0)] * M2->m[INDEX(0,2)] + (int)M1->m[INDEX(0,1)] * M2->m[INDEX(1,2)]) + (int)M1->m[INDEX(0,2)] * M2->m[INDEX(2,2)] ) >> COS_SIN_SHIFT );
	m[INDEX(0,3)] = ((((int)M1->m[INDEX(0,0)] * M2->m[INDEX(0,3)] + (int)M1->m[INDEX(0,1)] * M2->m[INDEX(1,3)]) + (int)M1->m[INDEX(0,2)] * M2->m[INDEX(2,3)] ) >> COS_SIN_SHIFT ) + M1->m[INDEX(0,3)];

	m[INDEX(1,0)] = ((((int)M1->m[INDEX(1,0)] * M2->m[INDEX(0,0)] + (int)M1->m[INDEX(1,1)] * M2->m[INDEX(1,0)]) + (int)M1->m[INDEX(1,2)] * M2->m[INDEX(2,0)] ) >> COS_SIN_SHIFT );
	m[INDEX(1,1)] = ((((int)M1->m[INDEX(1,0)] * M2->m[INDEX(0,1)] + (int)M1->m[INDEX(1,1)] * M2->m[INDEX(1,1)]) + (int)M1->m[INDEX(1,2)] * M2->m[INDEX(2,1)] ) >> COS_SIN_SHIFT );
	m[INDEX(1,2)] = ((((int)M1->m[INDEX(1,0)] * M2->m[INDEX(0,2)] + (int)M1->m[INDEX(1,1)] * M2->m[INDEX(1,2)]) + (int)M1->m[INDEX(1,2)] * M2->m[INDEX(2,2)] ) >> COS_SIN_SHIFT );
	m[INDEX(1,3)] = ((((int)M1->m[INDEX(1,0)] * M2->m[INDEX(0,3)] + (int)M1->m[INDEX(1,1)] * M2->m[INDEX(1,3)]) + (int)M1->m[INDEX(1,2)] * M2->m[INDEX(2,3)] ) >> COS_SIN_SHIFT ) + M1->m[INDEX(1,3)];

	m[INDEX(2,0)] = ((((int)M1->m[INDEX(2,0)] * M2->m[INDEX(0,0)] + (int)M1->m[INDEX(2,1)] * M2->m[INDEX(1,0)]) + (int)M1->m[INDEX(2,2)] * M2->m[INDEX(2,0)] ) >> COS_SIN_SHIFT );
	m[INDEX(2,1)] = ((((int)M1->m[INDEX(2,0)] * M2->m[INDEX(0,1)] + (int)M1->m[INDEX(2,1)] * M2->m[INDEX(1,1)]) + (int)M1->m[INDEX(2,2)] * M2->m[INDEX(2,1)] ) >> COS_SIN_SHIFT );
	m[INDEX(2,2)] = ((((int)M1->m[INDEX(2,0)] * M2->m[INDEX(0,2)] + (int)M1->m[INDEX(2,1)] * M2->m[INDEX(1,2)]) + (int)M1->m[INDEX(2,2)] * M2->m[INDEX(2,2)] ) >> COS_SIN_SHIFT );
	m[INDEX(2,3)] = ((((int)M1->m[INDEX(2,0)] * M2->m[INDEX(0,3)] + (int)M1->m[INDEX(2,1)] * M2->m[INDEX(1,3)]) + (int)M1->m[INDEX(2,2)] * M2->m[INDEX(2,3)] ) >> COS_SIN_SHIFT ) + M1->m[INDEX(2,3)];
}

// Inverse rotate using transposed matrix
void CMatrix44::InvRotateVector(const Vector4s *Src, Vector4s *Dest)
{
	int x = Src->x;
	int y = Src->y;
	int z = Src->z;
	Dest->x = (x*m[INDEX(0,0)] + y*m[INDEX(1,0)] + z*m[INDEX(2,0)]) >> COS_SIN_SHIFT;
	Dest->y = (x*m[INDEX(0,1)] + y*m[INDEX(1,1)] + z*m[INDEX(2,1)]) >> COS_SIN_SHIFT;
	Dest->z = (x*m[INDEX(0,2)] + y*m[INDEX(1,2)] + z*m[INDEX(2,2)]) >> COS_SIN_SHIFT;
}

void CMatrix44::DefScale(int Scale)
{
	LoadIdentity();
	m[INDEX(0,0)] = (COS_SIN_MUL*Scale)>>8;  
	m[INDEX(1,1)] = (COS_SIN_MUL*Scale)>>8;  
	m[INDEX(2,2)] = (COS_SIN_MUL*Scale)>>8;  
}

// Ratio of 256 mean scale of 1.0f
// Ratio of 512 mean scale of 2.0f
void CMatrix44::Scale(int Ratio)
{
	CMatrix44 s;

	s.DefScale(Ratio);
	PostMultiply(&s);
}

void CMatrix44::DefTranslate(Type x, Type y, Type z)
{
	LoadIdentity();

	m[INDEX(0,3)] = x;
	m[INDEX(1,3)] = y;
	m[INDEX(2,3)] = z;
}



void CMatrix44::DefRotateX(int a)
{
	Type s, c;
	s = Sinus(a);
	c = Cosinus(a);

	m[INDEX(0,0)] = COS_SIN_MUL;
	m[INDEX(0,1)] = 0;
	m[INDEX(0,2)] = 0;

	m[INDEX(1,0)] = 0;
	m[INDEX(1,1)] = c;  
	m[INDEX(1,2)] = -s;

	m[INDEX(2,0)] = 0;
	m[INDEX(2,1)] = s;  
	m[INDEX(2,2)] = c;

	m[INDEX(0,3)] = 0;  
	m[INDEX(1,3)] = 0;  
	m[INDEX(2,3)] = 0;  
}

void CMatrix44::DefRotateY(int a)
{
	Type s, c;
	s = Sinus(a);
	c = Cosinus(a);

	m[INDEX(0,0)] = c;
	m[INDEX(0,1)] = 0;  
	m[INDEX(0,2)] = s;

	m[INDEX(1,0)] = 0;
	m[INDEX(1,1)] = COS_SIN_MUL;  
	m[INDEX(1,2)] = 0;

	m[INDEX(2,0)] = -s; 
	m[INDEX(2,1)] = 0;  
	m[INDEX(2,2)] = c;

	m[INDEX(0,3)] = 0;  
	m[INDEX(1,3)] = 0;  
	m[INDEX(2,3)] = 0;  
}

void CMatrix44::DefRotateZ(int a)
{
	Type s, c;
	s = (int)Sinus(a);
	c = (int)Cosinus(a);

	m[INDEX(0,0)] = c;  
	m[INDEX(0,1)] = -s;
	m[INDEX(0,2)] = 0;

	m[INDEX(1,0)] = s;  
	m[INDEX(1,1)] = c;
	m[INDEX(1,2)] = 0;

	m[INDEX(2,0)] = 0;
	m[INDEX(2,1)] = 0;  
	m[INDEX(2,2)] = COS_SIN_MUL;  

	m[INDEX(0,3)] = 0;  
	m[INDEX(1,3)] = 0;  
	m[INDEX(2,3)] = 0;  
}

void CMatrix44::RotateX(int a)
{
	CMatrix44 rx;

	rx.DefRotateX(a);

	PostMultiply(&rx);
}

void CMatrix44::RotateY(int a)
{
	CMatrix44 ry;

	ry.DefRotateY(a);

	PostMultiply(&ry);
}

void CMatrix44::RotateZ(int a)
{
	CMatrix44 rz;

	rz.DefRotateZ(a);

	PostMultiply(&rz);
}


void CMatrix44::Transpose()
{
	int		t;
	
	t = m[INDEX(0,1)];  
	m[INDEX(0,1)] = m[INDEX(1,0)];
	m[INDEX(1,0)] = t;

	t = m[INDEX(0,2)];  
	m[INDEX(0,2)] = m[INDEX(2,0)];
	m[INDEX(2,0)] = t;

	t = m[INDEX(1,2)];  
	m[INDEX(1,2)] = m[INDEX(2,1)];
	m[INDEX(2,1)] = t;
}


/*

// -------------------------------------------------
// matrix stack, used by rendered
CMatrixStack::CMatrixStack(int size)
	:m_maxStackSize(size)
{
	m_stack = new CMatrix44[m_maxStackSize];

	m_currentMatrix = m_stack;
	m_currentMatrix->LoadIdentity();
} 



CMatrixStack::~CMatrixStack()
{
	SYS_ASSERT(m_currentMatrix == m_stack);
	delete[] m_stack;
}



CMatrix44& CMatrixStack::PushMatrix(void)
{
	SYS_ASSERT(m_currentMatrix -m_stack <= m_maxStackSize);
 
	m_currentMatrix++;
	m_currentMatrix->Load(m_currentMatrix - 1);

	return *m_currentMatrix;
}


void CMatrixStack::PopMatrix(void)
{
	SYS_ASSERT(m_currentMatrix-m_stack > 0);
	m_currentMatrix--;
}

// ---------------------------------------------------------------------------
//
// ---------------------------------------------------------------------------
CMatrix44& CMatrixStack::ResetStack()
{
	SYS_ASSERT(m_currentMatrix == m_stack);

	m_currentMatrix = m_stack;
	m_currentMatrix->LoadIdentity();
	return *m_currentMatrix;
}
*/
void CMatrix44::LoadIdentity()
{
	m[INDEX(0,0)] = COS_SIN_MUL;  m[INDEX(0,1)] = 0;            m[INDEX(0,2)] = 0;            m[INDEX(0,3)] = 0;
	m[INDEX(1,0)] = 0;            m[INDEX(1,1)] = COS_SIN_MUL;  m[INDEX(1,2)] = 0;            m[INDEX(1,3)] = 0;
	m[INDEX(2,0)] = 0;            m[INDEX(2,1)] = 0;            m[INDEX(2,2)] = COS_SIN_MUL;  m[INDEX(2,3)] = 0;
}


CMatrix44::CMatrix44()
{
	m[INDEX(0,0)] = COS_SIN_MUL;  m[INDEX(0,1)] = 0;            m[INDEX(0,2)] = 0;            m[INDEX(0,3)] = 0;
	m[INDEX(1,0)] = 0;            m[INDEX(1,1)] = COS_SIN_MUL;  m[INDEX(1,2)] = 0;            m[INDEX(1,3)] = 0;
	m[INDEX(2,0)] = 0;            m[INDEX(2,1)] = 0;            m[INDEX(2,2)] = COS_SIN_MUL;  m[INDEX(2,3)] = 0;
}

CMatrix44::CMatrix44(const CMatrix44 *src)
{
	MEMCPY( &m, src->m, sizeof(m));
};

void CMatrix44::Identity()
{
	m[INDEX(0,0)] = COS_SIN_MUL;  m[INDEX(0,1)] = 0;            m[INDEX(0,2)] = 0;            m[INDEX(0,3)] = 0;
	m[INDEX(1,0)] = 0;            m[INDEX(1,1)] = COS_SIN_MUL;  m[INDEX(1,2)] = 0;            m[INDEX(1,3)] = 0;
	m[INDEX(2,0)] = 0;            m[INDEX(2,1)] = 0;            m[INDEX(2,2)] = COS_SIN_MUL;  m[INDEX(2,3)] = 0;
};
  

void CMatrix44::PostMultiply(const CMatrix44 *M2)
{
	int A, B, C;

	A = m[INDEX(0,0)];
	B = m[INDEX(0,1)];
	C = m[INDEX(0,2)];

	m[INDEX(0,0)] = (( (A*M2->m[INDEX(0,0)] + B*M2->m[INDEX(1,0)]) + C*M2->m[INDEX(2,0)] ) >> COS_SIN_SHIFT );
	m[INDEX(0,1)] = (( (A*M2->m[INDEX(0,1)] + B*M2->m[INDEX(1,1)]) + C*M2->m[INDEX(2,1)] ) >> COS_SIN_SHIFT );
	m[INDEX(0,2)] = (( (A*M2->m[INDEX(0,2)] + B*M2->m[INDEX(1,2)]) + C*M2->m[INDEX(2,2)] ) >> COS_SIN_SHIFT );
	m[INDEX(0,3)] = (( (A*M2->m[INDEX(0,3)] + B*M2->m[INDEX(1,3)]) + C*M2->m[INDEX(2,3)] ) >> COS_SIN_SHIFT ) + m[INDEX(0,3)];

	A = m[INDEX(1,0)];
	B = m[INDEX(1,1)];
	C = m[INDEX(1,2)];

	m[INDEX(1,0)] = (( (A*M2->m[INDEX(0,0)] + B*M2->m[INDEX(1,0)]) + C*M2->m[INDEX(2,0)] ) >> COS_SIN_SHIFT );
	m[INDEX(1,1)] = (( (A*M2->m[INDEX(0,1)] + B*M2->m[INDEX(1,1)]) + C*M2->m[INDEX(2,1)] ) >> COS_SIN_SHIFT );
	m[INDEX(1,2)] = (( (A*M2->m[INDEX(0,2)] + B*M2->m[INDEX(1,2)]) + C*M2->m[INDEX(2,2)] ) >> COS_SIN_SHIFT );
	m[INDEX(1,3)] = (( (A*M2->m[INDEX(0,3)] + B*M2->m[INDEX(1,3)]) + C*M2->m[INDEX(2,3)] ) >> COS_SIN_SHIFT ) + m[INDEX(1,3)];

	A = m[INDEX(2,0)];
	B = m[INDEX(2,1)];
	C = m[INDEX(2,2)];

	m[INDEX(2,0)] = (( (A*M2->m[INDEX(0,0)] + B*M2->m[INDEX(1,0)]) + C*M2->m[INDEX(2,0)] ) >> COS_SIN_SHIFT );
	m[INDEX(2,1)] = (( (A*M2->m[INDEX(0,1)] + B*M2->m[INDEX(1,1)]) + C*M2->m[INDEX(2,1)] ) >> COS_SIN_SHIFT );
	m[INDEX(2,2)] = (( (A*M2->m[INDEX(0,2)] + B*M2->m[INDEX(1,2)]) + C*M2->m[INDEX(2,2)] ) >> COS_SIN_SHIFT );
	m[INDEX(2,3)] = (( (A*M2->m[INDEX(0,3)] + B*M2->m[INDEX(1,3)]) + C*M2->m[INDEX(2,3)] ) >> COS_SIN_SHIFT ) + m[INDEX(2,3)];

};


void CMatrix44::PostTranslate(int x, int y, int z)
{

	int A, B, C;

	A = m[INDEX(0,0)];
	B = m[INDEX(0,1)];
	C = m[INDEX(0,2)];

	m[INDEX(0,3)] += ( A*x + B*y + C*z ) >> COS_SIN_SHIFT;

	A = m[INDEX(1,0)];
	B = m[INDEX(1,1)];
	C = m[INDEX(1,2)];


	m[INDEX(1,3)] += ( A*x + B*y + C*z ) >> COS_SIN_SHIFT;

	A = m[INDEX(2,0)];
	B = m[INDEX(2,1)];
	C = m[INDEX(2,2)];


	m[INDEX(2,3)] += ( A*x + B*y + C*z ) >> COS_SIN_SHIFT;

};

void CMatrix44::PostRotation(AXIS axis, int angle)
{
	switch(axis)
	{
	case X:
		RotateX(angle);
		break;
	case Y:
		RotateY(angle);
		break;
	case Z:
		RotateZ(angle);
		break;
	}
};

void CMatrix44::LookAt(const Vector4s &eye, const Vector4s &dir, const Vector4s &up)
{
	Vector4s zaxis,xaxis,yaxis;
	
//	DBGPRINTF("x:%d\ty:%d\tz:%d\t\tx:%d\ty:%d\tz:%d\t\tx:%d\ty:%d\tz:%d", dir.x, dir.y, dir.z, eye.x, eye.y, eye.z, up.x, up.y, up.z);
	zaxis = dir;
	zaxis.Normalize();
	
	up.CrossShift(&zaxis, &xaxis);
	xaxis.Normalize();

	zaxis.CrossShift(&xaxis, &yaxis);
	yaxis.Normalize();

	m[INDEX(0,0)] = (Type)-xaxis.x;
	m[INDEX(0,1)] = (Type)-xaxis.y;
	m[INDEX(0,2)] = (Type)-xaxis.z;
	m[INDEX(1,0)] = (Type)yaxis.x;
	m[INDEX(1,1)] = (Type)yaxis.y;
	m[INDEX(1,2)] = (Type)yaxis.z;
	m[INDEX(2,0)] = (Type)zaxis.x;
	m[INDEX(2,1)] = (Type)zaxis.y;
	m[INDEX(2,2)] = (Type)zaxis.z;

	m[INDEX(0,3)] = (Type)(eye*xaxis >>COS_SIN_SHIFT);
	m[INDEX(1,3)] = (Type)(-eye*yaxis >>COS_SIN_SHIFT);
	m[INDEX(2,3)] = (Type)(-eye*zaxis >>COS_SIN_SHIFT);

};

void CMatrix44::LookAt(const int* eye, const int* dir)
{
	Vector4s e(eye[0], eye[1], eye[2]);
	Vector4s d(dir[0], dir[1], dir[2]);
	Vector4s u(0, 0, 1<<COS_SIN_SHIFT);
	LookAt(e, d, u);
}

void CMatrix44::Inverse()// do not implement now 
{
	/*
	Type t;

	t = m[1];
	m[1] = m[4];
	m[4] = t;

	t = m[2];
	m[2] = m[8];
	m[8] = t;

	t = m[6];
	m[6] = m[9];
	m[9] = t;
	*/

/*
	DBGPRINTF("===========");
	DBGPRINTF("PRE");
	DBGPRINTF("m:%d %d %d %d",m[INDEX(0,0)],m[INDEX(0,1)],m[INDEX(0,2)],m[INDEX(0,3)]);
	DBGPRINTF("m:%d %d %d %d",m[INDEX(1,0)],m[INDEX(1,1)],m[INDEX(1,2)],m[INDEX(1,3)]);
	DBGPRINTF("m:%d %d %d %d",m[INDEX(2,0)],m[INDEX(2,1)],m[INDEX(2,2)],m[INDEX(2,3)]);
*/


	Matrix3x3 mat2(get(0,0),get(0,1),get(0,2),get(1,0),get(1,1),get(1,2),get(2,0),get(2,1),get(2,2));

	Matrix3x3 mat=mat2.Inverse();	

	m[INDEX(0,0)]=(Type)mat.e11;
	m[INDEX(0,1)]=(Type)mat.e12;
	m[INDEX(0,2)]=(Type)mat.e13;

	m[INDEX(1,0)]=(Type)mat.e21;
	m[INDEX(1,1)]=(Type)mat.e22;
	m[INDEX(1,2)]=(Type)mat.e23;

	m[INDEX(2,0)]=(Type)mat.e31;
	m[INDEX(2,1)]=(Type)mat.e32;
	m[INDEX(2,2)]=(Type)mat.e33;

	Vector4s vet(get(0,3),get(1,3),get(2,3));
	Vector4s vet2;

	m[INDEX(0,3)]=0;
	m[INDEX(1,3)]=0;
	m[INDEX(2,3)]=0;

	vet2=this->Transform(vet);

	m[INDEX(0,3)]=(Type)-vet2.x;
	m[INDEX(1,3)]=(Type)-vet2.y;
	m[INDEX(2,3)]=(Type)-vet2.z;


/*
	DBGPRINTF("POST");
	DBGPRINTF("m:%d %d %d %d",m[INDEX(0,0)],m[INDEX(0,1)],m[INDEX(0,2)],m[INDEX(0,3)]);
	DBGPRINTF("m:%d %d %d %d",m[INDEX(1,0)],m[INDEX(1,1)],m[INDEX(1,2)],m[INDEX(1,3)]);
	DBGPRINTF("m:%d %d %d %d",m[INDEX(2,0)],m[INDEX(2,1)],m[INDEX(2,2)],m[INDEX(2,3)]);

	DBGPRINTF("====================");
*/


};



void CMatrix44::Test()
{
	DBGPRINTF("===Test===");
	DBGPRINTF("m:%d %d %d %d",m[INDEX(0,0)],m[INDEX(0,1)],m[INDEX(0,2)],m[INDEX(0,3)]);
	DBGPRINTF("m:%d %d %d %d",m[INDEX(1,0)],m[INDEX(1,1)],m[INDEX(1,2)],m[INDEX(1,3)]);
	DBGPRINTF("m:%d %d %d %d",m[INDEX(2,0)],m[INDEX(2,1)],m[INDEX(2,2)],m[INDEX(2,3)]);
	DBGPRINTF("====================");
}