矩陣-齊次矩陣

1.正交矩陣 一個矩陣爲正交矩陣，就不需要求逆矩陣，直接使用正交矩陣作爲逆矩陣進行變換運算。施密特正交化。

2.3X3矩陣擴展爲4X4齊次矩陣 最後一行表示矩陣的平移 線性變換+平移

class Vector3;

class Matrix4X3 {
public:
	float m11, m12, m13;
	float m21, m22, m23;
	float m31, m32, m33;
	float tx, ty, tz;

	void SetRotate(int x, float theta);
	void SetScale(const Vector3 &v);
	void SetProject(const Vector3 &v);
	void SetReflect(int x, float k);
	void SetReflect( Vector3 &v);
	void SetShear(int x,float s,float t);

	void ZeroTranslation();
	void SetTranslation(const Vector3 &v);
	void SetupTranslation(const Vector3 &v);
};

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

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

float Determinant(const Matrix4X3 &m);

Matrix4X3 Inverse(const Matrix4X3 &m);

Vector3 GetTranslation(const Matrix4X3 &m);

#include "pch.h"
#include "Matrix4X3.h"
#include "Vector3.h"
#include "MathUtil.h"
#include <assert.h>

Matrix4X3 operator*(const Matrix4X3 &m1, const Matrix4X3 &m2)
{
	Matrix4X3 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;

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

Vector3 operator*(const Vector3 &v, const Matrix4X3 &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 Matrix4X3::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;
	}
	tx = ty = tz = 0;
}

//縮放矩陣
void Matrix4X3::SetScale(const Vector3 &v)
{
	m11 = v.x; m12 = 0; m13 = 0;
	m21 = 0; m22 = v.y; m23 = 0;
	m31 = 0; m32 = 0; m33 = v.z;
	tx = ty = tz = 0;
}

//投影矩陣 v傳入單位向量 
void Matrix4X3::SetProject(const Vector3 &v)
{
	m11 = 1.0f - v.x*v.x;
	m22 = 1.0f - v.y*v.y;
	m33 = 1.0f - v.z*v.z;

	m12 = m21 = -v.x*v.y;
	m13 = m31 = -v.x*v.z;
	m23 = m32 = -v.y*v.z;

	tx = ty = tz = 0;
}

//1,2,3代表 x y z鏡像
void Matrix4X3::SetReflect(int x,float k)
{
	switch (x)
	{
	case 1:
		m11 = -1; m12 = 0; m13 = 0;
		m21 = 0; m22 = 1; m23 = 0;
		m31 = 0; m32 = 0; m33 = 1;
		tx = 2 * k;
		ty = tz = 0;
		break;
	case 2:
		m11 = 1; m12 = 0; m13 = 0;
		m21 = 0; m22 = -1; m23 = 0;
		m31 = 0; m32 = 0; m33 = 1;
		tx = tz = 0;
		ty = 2 * k;
		break;
	case 3:
		m11 = 1; m12 = 0; m13 = 0;
		m21 = 0; m22 = 1; m23 = 0;
		m31 = 0; m32 = 0; m33 = -1;
		tz = 2 * k;
		tx = ty = 0;
		break;
	default:
		break;
	}
}

void Matrix4X3::SetReflect( Vector3 &n)
{
	assert(fabs(n*n) - 1.0f < 0.01f);

	float ax = -2.0f*n.x;
	float ay = -2.0f*n.y;
	float az = -2.0f*n.z;

	m11 = 1.0f + ax * n.x;
	m22 = 1.0f + ay * n.y;
	m33 = 1.0f + az * n.z;

	m12 = m21 = ax * n.y;
	m13 = m31 = ax * n.z;
	m23 = m32 = ay * n.z;

	tx = ty = tz = 0;
}

//1，2，3代表用x，y，z軸切變
void Matrix4X3::SetShear(int x,float s,float t)
{
	switch (x)
	{
	case 1:
		m11 = 1; m12 = s; m13 = t;
		m21 = 0; m22 = 1; m23 = 0;
		m31 = 0; m32 = 0; m33 = 1;
		break;
	case 2:
		m11 = 1; m12 = 0; m13 = 0;
		m21 = s; m22 = 1; m23 = t;
		m31 = 0; m32 = 0; m33 = 1;
		break;
	case 3:
		m11 = 1; m12 = 0; m13 = 0;
		m21 = 0; m22 = 1; m23 = 0;
		m31 = s; m32 = t; m33 = 1;
		break;
	default:
		break;
	}
	tx = ty = tz = 0.0f;
}

//行列式值的幾何意義是 立方體體積
float Determinant(const Matrix4X3 &m)
{
	return m.m11*m.m22*m.m33 + m.m12*m.m23*m.m31 + m.m21*m.m32*m.m13 - m.m13*m.m22*m.m31 - m.m21*m.m12*m.m33 - m.m23*m.m32*m.m11;
}

//矩陣的逆
Matrix4X3 Inverse(const Matrix4X3 &m)
{
	float det = Determinant(m);
	assert(fabs(det) > 0.00001f); 

	float oneOverDet = 1 / det;

	Matrix4X3 r;

	r.m11 = (m.m22*m.m33 - m.m23*m.m32)*oneOverDet;
	r.m12 = (m.m32*m.m13 - m.m12*m.m33)*oneOverDet;  
	r.m13 = (m.m12*m.m23 - m.m13*m.m22)*oneOverDet;

	r.m21 = (m.m31*m.m23 - m.m33*m.m21)*oneOverDet;
	r.m22 = (m.m11*m.m33 - m.m31*m.m13)*oneOverDet;
	r.m23 = (m.m21*m.m13 - m.m11*m.m23)*oneOverDet;

	r.m31 = (m.m21*m.m32 - m.m31*m.m22)*oneOverDet;
	r.m32 = (m.m31*m.m12 - m.m11*m.m32)*oneOverDet;
	r.m33 = (m.m11*m.m22 - m.m12*m.m21)*oneOverDet;

	return r;
}

void Matrix4X3::ZeroTranslation()
{
	tx = ty = tz = 0;
}

void Matrix4X3::SetTranslation(const Vector3 &v)
{
	tx = v.x;
	ty = v.y;
	tz = v.z;
}

void Matrix4X3::SetupTranslation(const Vector3 &v)
{
	m11 = 1; m12 = 0; m13 = 0;
	m21 = 0; m22 = 1; m23 = 0;
	m31 = 0; m32 = 0; m33 = 1;
	tx = v.x;
	ty = v.y;
	tz = v.z;
}

Vector3 GetTranslation(const Matrix4X3 &m)
{
	return Vector3(m.tx, m.ty, m.tz);
}