矩陣-旋轉

class Vector3;

class Matrix3X3 {
public:
	float m11, m12, m13;
	float m21, m22, m23;
	float m31, m32, m33;

	void SetRotate(int x, float theta);
};

Matrix3X3 operator*(const Matrix3X3 &m1, const Matrix3X3 &m2);

Vector3 operator*(const Vector3 &v, const Matrix3X3 &m);

Matrix3X3 operator*(const Matrix3X3 &m1, const Matrix3X3 &m2)
{
	Matrix3X3 r;
	r.m11 = m1.m11*m2.m11 + m1.m12*m2.m21 + m1.m13*m2.m31;
	r.m12 = m1.m11*m2.m12 + m1.m12*m2.m22 + m1.m13*m2.m32;
	r.m13 = m1.m11*m2.m13 + m1.m12*m2.m23 + m1.m13*m2.m33;

	r.m21 = m1.m21*m2.m11 + m1.m22*m2.m21 + m1.m23*m2.m31;
	r.m22 = m1.m21*m2.m12 + m1.m22*m2.m22 + m1.m23*m2.m32;
	r.m23 = m1.m21*m2.m13 + m1.m22*m2.m23 + m1.m23*m2.m33;

	r.m31 = m1.m31*m2.m11 + m1.m32*m2.m21 + m1.m33*m2.m31;
	r.m32 = m1.m31*m2.m12 + m1.m32*m2.m22 + m1.m33*m2.m32;
	r.m33 = m1.m31*m2.m13 + m1.m32*m2.m23 + m1.m33*m2.m33;
	return r;
}

Vector3 operator*(const Vector3 &v, const Matrix3X3 &m)
{
	return  Vector3(v.x*m.m11 + v.y*m.m21 + v.z*m.m31, v.x*m.m12 + v.y*m.m22 + v.z*m.m32, v.x*m.m13 + v.y*m.m23 + v.z*m.m33);
}

//1,2,3代表 x y z旋轉
void Matrix3X3::SetRotate(int x, float theta)
{
	float s, c;
	SinCos(&s, &c, theta);
	switch (x)
	{
	case 1:
		m11 = 1; m12 = 0; m13 = 0;
		m21 = 0; m22 = c; m23 = s;
		m31 = 0; m32 = -s; m33 = c;
		break;
	case 2:
		m11 = c; m12 = 0; m13 = -s;
		m21 = 0; m22 = 1; m23 = 0;
		m31 = s; m32 = 0; m33 = c;
		break;
	case 3:
		m11 = c; m12 = s; m13 = 0;
		m21 = -s; m22 = c; m23 = 0;
		m31 = 0; m32 = 0; m33 = 1;
		break;
	default:
		break;
	}
}

const float PI = 3.141593;
const float Pi2 = 2 * PI;
const float PiOver2 = PI / 2;

inline void SinCos(float *Sin, float *Cos, float theta)
{
	*Sin = sin(theta);
	*Cos = cos(theta);
}

float to_zero(float n)
{
	if (abs(n) < 0.0001)
	{
		return 0;
	}
	return n;
}

void print_v(Vector3 &v)
{
	cout << "[" << to_zero(v.x) << "," << to_zero(v.y) << "," << to_zero(v.z) << "]" << endl;
}

void print_m(Matrix3X3 &m)
{
	cout << to_zero(m.m11) << "\t" << to_zero(m.m12) << "\t" << to_zero(m.m13) << endl;
	cout << to_zero(m.m21) << "\t" << to_zero(m.m22) << "\t" << to_zero(m.m23) << endl;
	cout << to_zero(m.m31) << "\t" << to_zero(m.m32) << "\t" << to_zero(m.m33) << endl;
}

int main()
{
	cout << "Hello Matrix Rotate!\n";

	Vector3 v(10, 0, 0);
	Matrix3X3 m;
	m.SetRotate(3, PiOver2);
	Vector3 r;
	r = v * m;
	print_v(r);

	m.SetRotate(2, PiOver2);
	Vector3 w;
	w = v * m;
	print_v(w);

	m.SetRotate(1, PiOver2);
	Vector3 z;
	z = v * m;
	print_v(z);

	system("pause");
	return 0;
}

解決問題：

3D旋轉矩陣 分別繞X Y Z座標軸旋轉

源代碼地址