matrix.cpp

3D赛车游戏源代码-用Visual Studio 2005

#include "StdAfx.h"
#include "MathUtil.h"
#include "Vector3.h"
#include "Matrix.h"

//-------------------------------------------------------------------------------
// 设置单位矩阵
//-------------------------------------------------------------------------------
void CMatrix44F::Identity()
{
	m[0+4*0] = 1; m[0+4*1] = 0; m[0+4*2] = 0; m[0+4*3] = 0;
	m[1+4*0] = 0; m[1+4*1] = 1; m[1+4*2] = 0; m[1+4*3] = 0;
	m[2+4*0] = 0; m[2+4*1] = 0; m[2+4*2] = 1; m[2+4*3] = 0;
	m[3+4*0] = 0; m[3+4*1] = 0; m[3+4*2] = 0; m[3+4*3] = 1;
}

//-------------------------------------------------------------------------------
// 访问行
//-------------------------------------------------------------------------------
void CMatrix44F::GetRow(int i, Vector3 *pVec) const
{
	assert(i >= 0 && i < 4);
	i *= 4;
	pVec->x = m[i++];
	pVec->y = m[i++];
	pVec->z = m[i];
}

//-------------------------------------------------------------------------------
// 访问行
//-------------------------------------------------------------------------------
void CMatrix44F::SetRow(int i, const Vector3 &vec)
{
	assert(i >= 0 && i < 4);
	i *= 4;
	m[i++] = vec.x;
	m[i++] = vec.y;
	m[i] = vec.z;
}

//-------------------------------------------------------------------------------
// 访问列
//-------------------------------------------------------------------------------
void CMatrix44F::GetCol(int i, Vector3 *pVec) const
{
	assert(i >= 0 && i < 4);
	pVec->x = m[i];
	pVec->y = m[i+4];
	pVec->z = m[i+8];
}

//-------------------------------------------------------------------------------
// 访问列
//-------------------------------------------------------------------------------
void CMatrix44F::SetCol(int i, const Vector3 &vec)
{
	assert(i >= 0 && i < 4);
	m[i] = vec.x;
	m[i+4] = vec.y;
	m[i+8] = vec.z;
}

//-------------------------------------------------------------------------------
// 移动部分置空
//-------------------------------------------------------------------------------
void CMatrix44F::ZeroTranslation()
{
	m[12] = m[13] = m[14] = 0;
}

//-------------------------------------------------------------------------------
// 直接设置移动部分
//-------------------------------------------------------------------------------
void CMatrix44F::SetTranslation(const Vector3 &d)
{
	m[12] = d.x;
	m[13] = d.y;
	m[14] = d.z;
}

//-------------------------------------------------------------------------------
// 设置移动部分并把旋转置空
//-------------------------------------------------------------------------------
void CMatrix44F::SetupTranslation(const Vector3 &d)
{
	Identity();
	SetTranslation(d);
}

//-------------------------------------------------------------------------------
// 根据当前的旋转和位置来移动矩阵
//-------------------------------------------------------------------------------
void CMatrix44F::Translate(const Vector3 &d)
{
	m[12] = m[0] * d.x + m[4] * d.y + m[8]  * d.z + m[12];
	m[13] = m[1] * d.x + m[5] * d.y + m[9]  * d.z + m[13];
	m[14] = m[2] * d.x + m[6] * d.y + m[10] * d.z + m[14];
	m[15] = 1;
}

//-------------------------------------------------------------------------------
// 构造绕坐标轴旋转的矩阵
//-------------------------------------------------------------------------------
void CMatrix44F::SetupRotate(int axis, float theta)
{
	float s, c;
	sinCos(&s, &c, theta);

	switch (axis)
	{
	case 1: //绕x轴旋转
		m[0] = 1.0f; m[1] = 0.0f; m[2] = 0.0f;
		m[4] = 0.0f; m[5] = c;    m[6] = s;
		m[8] = 0.0f; m[9] = -s;   m[10] = c;
		break;

	case 2: //绕y轴旋转
		m[0] = c;    m[1] = 0.0f; m[2] = -s;
		m[4] = 0.0f; m[5] = 1.0f; m[6] = 0.0f;
		m[8] = s;    m[9] = 0.0f; m[10] = c;
		break;

	case 3: //绕z轴旋转
		m[0] = c;    m[1] = s;    m[2] = 0.0f;
		m[4] = -s;   m[5] = c;    m[6] = 0.0f;
		m[8] = 0.0f; m[9] = 0.0f; m[10] = 1.0f;
		break;

	default:
		assert(false);
		break;
	}

	m[3]  = 0.0f;
	m[7]  = 0.0f;
	m[11] = 0.0f;
	m[12] = 0.0f; 
	m[13] = 0.0f;
	m[14] = 0.0f;
	m[15] = 1.0f;
}

//-------------------------------------------------------------------------------
// 构造绕任意轴旋转的矩阵
//-------------------------------------------------------------------------------
void CMatrix44F::SetupRotate(const Vector3 &axis, float theta)
{
	//检查旋转轴是不是单位向量
	assert(fabs(axis*axis - 1.0f) < 0.01f);
	
	float s, c;
	sinCos(&s, &c, theta);

	float a = 1.0f - c;
	float ax = a * axis.x;
	float ay = a * axis.y;
	float az = a * axis.z;

	m[0] = ax * axis.x + c;
	m[1] = ax * axis.y + axis.z * s;
	m[2] = ax * axis.z - axis.y * s;

	m[4] = ay * axis.x - axis.z * s;
	m[5] = ay * axis.y + c;
	m[6] = ay * axis.z + axis.x * s;

	m[8] = az * axis.x + axis.y * s;
	m[9] = az * axis.y - axis.x * s;
	m[10] = az * axis.z + c;

	m[3]  = 0.0f;
	m[7]  = 0.0f;
	m[11] = 0.0f;
	m[12] = 0.0f; 
	m[13] = 0.0f;
	m[14] = 0.0f;
	m[15] = 1.0f;
}

//-------------------------------------------------------------------------------
// 绕坐标轴旋转矩阵，依照当前的矩阵
//-------------------------------------------------------------------------------
void CMatrix44F::Rotate(int axis, float theta)
{
	CMatrix44F mat;
	mat.SetupRotate(axis, theta);
	Mul34(mat);
}

//-------------------------------------------------------------------------------
// 绕任意轴旋转矩阵，依照当前的矩阵
//-------------------------------------------------------------------------------
void CMatrix44F::Rotate(const Vector3 &axis, float theta)
{
	CMatrix44F mat;
	mat.SetupRotate(axis, theta);
	Mul34(mat);
}

//注意这个宏在Mul34和下面的乘号运算浮等地方都会用到。
//方便的访问矩阵成员。
#define A(row,col)  a.m[(col<<2)+row]
#define B(row,col)  b.m[(col<<2)+row]

//-------------------------------------------------------------------------------
// 用于旋转的乘法，平移部分不变。34为三行四列的意思
//-------------------------------------------------------------------------------
CMatrix44F& CMatrix44F::Mul34(const CMatrix44F &m)
{
	CMatrix44F &a = *this;
	const CMatrix44F &b = m;

	for (int i = 0; i < 3; i++) 
	{
		const float ai0=A(i,0),  ai1=A(i,1),  ai2=A(i,2),  ai3=A(i,3);
		A(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0);
		A(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1);
		A(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2);
		A(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3;
	}

	A(3,0) = 0;
	A(3,1) = 0;
	A(3,2) = 0;
	A(3,3) = 1;

	return *this;
}

//-------------------------------------------------------------------------------
// 仿射反转，用于“物体到世界”和“世界到物体”的转变
//-------------------------------------------------------------------------------
CMatrix44F& CMatrix44F::AffineInverse()
{
	float temp[16];
	memcpy(temp, m, 16 * sizeof(float));

	m[4] = temp[1];
	m[1] = temp[4];
	m[8] = temp[2];
	m[2] = temp[8];
	m[9] = temp[6];
	m[6] = temp[9];

	m[12]  = -(temp[0]*temp[12] + temp[1]*temp[13] + temp[2]*temp[14]);
	m[13]  = -(temp[4]*temp[12] + temp[5]*temp[13] + temp[6]*temp[14]);
	m[14]  = -(temp[8]*temp[12] + temp[9]*temp[13] + temp[10]*temp[14]);

	return *this;
}

//-------------------------------------------------------------------------------
// 向量乘以矩阵
//-------------------------------------------------------------------------------
Vector3 operator*(const Vector3 &v, const CMatrix44F &m)
{
	return Vector3( v.x*m.m[0] + v.y*m.m[4] + v.z*m.m[8] + m.m[12], 
					v.x*m.m[1] + v.y*m.m[5] + v.z*m.m[9] + m.m[13],
					v.x*m.m[2] + v.y*m.m[6] + v.z*m.m[10] + m.m[14]);
}

//-------------------------------------------------------------------------------
// 向量乘以矩阵
//-------------------------------------------------------------------------------
Vector3& operator*=(Vector3 &v, const CMatrix44F &m)
{
	v = v * m;
	return v;
}

//-------------------------------------------------------------------------------
// 矩阵相乘，会调用拷贝构造函数，慢一点
//-------------------------------------------------------------------------------
CMatrix44F operator*(const CMatrix44F &a, const CMatrix44F &b)
{
	CMatrix44F r(a);
	r *= b;
	return r;
}


//-------------------------------------------------------------------------------
// 矩阵乘以矩阵
//-------------------------------------------------------------------------------
CMatrix44F& operator*=(CMatrix44F &a, const CMatrix44F &b)
{
	for (int i = 0; i < 4; ++i)
	{
		const float ai0 = A(i,0), ai1 = A(i,1), ai2 = A(i,2), ai3=A(i,3);
		A(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
		A(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
		A(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
		A(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
	}

	return a;
}

//-------------------------------------------------------------------------------
// 提取矩阵的平移部分
//-------------------------------------------------------------------------------
Vector3	GetTranslation(const CMatrix44F &m)
{
	return Vector3(m.m[12], m.m[13], m.m[14]);
}