Matrix 4 x 3-未テスト-実装ファイル

#include <assert.h>
#include <math.h>
#include "Vector3.h"
#include "EularAngles.h"
#include "Quaternion.h"
#include "RotationMatrix.h"
#include "Matrix4x3.h"
#include "Common.h"

void Matrix4x3 ::Identity()
{
	m11 = m22 = m33 = tz = 1 ;
	m12 = m13 = m21 = m23 = m31 = m32 = tx = ty = 0 ;
}

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

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

void Matrix4x3 ::SetUpTranslation(const Vector3& d)
{
	m11 = m22 = m33 ;
	m12 = m13 = m21 = m23 = m31 = m32 = 0 ;

	tx = d.x ;
	ty = d.y ;
	tz = d.z ;
}

void Matrix4x3 ::SetUpLocalToParent(const Vector3& pos, const EulerAngles& orient)
{
	RotationMatrix orientMatrix ;
	orientMatrix.SetUp(orient) ;

	SetUpLocalToParent(pos, orientMatrix) ;
}

void Matrix4x3 ::SetUpLocalToParent(const Vector3& pos, const RotationMatrix& orient)
{
	m11 = orient.m11 ; m12 = orient.m21 ; m13 = orient.m31 ;
	m21 = orient.m12 ; m22 = orient.m22 ; m23 = orient.m32 ;
	m31 = orient.m13 ; m32 = orient.m23 ; m33 = orient.m33 ;

	tx = pos.x ; ty = pos.y ; tz = pos.z ;
}

void Matrix4x3 ::SetUpParentToLocal(const Vector3& pos, const EulerAngles& orient)
{
	RotationMatrix orientMatrix ;
	orientMatrix.SetUp(orient) ;

	SetUpParentToLocal(pos, orientMatrix) ;
}

void Matrix4x3 ::SetUpParentToLocal(const Vector3& pos, const RotationMatrix& orient)
{
	m11 = orient.m11 ; m12 = orient.m12 ; m13 = orient.m13 ;
	m21 = orient.m21 ; m22 = orient.m22 ; m23 = orient.m23 ;
	m31 = orient.m31 ; m32 = orient.m32 ; m33 = orient.m33 ;

	tx = -(m11 * pos.x + m12 * pos.y + m13 * pos.z) ;
	ty = -(m21 * pos.x + m22 * pos.y + m23 * pos.z) ;
	tz = -(m31 * pos.x + m32 * pos.y + m33 * pos.z) ;
}

void Matrix4x3 ::SetUpRotate(AxisName axisName, float theta)
{
	float sinTheta = sin(theta) ;
	float cosTheta = cos(theta) ;

	switch(axisName)
	{
	case X:
		m11 = 1 ; m12 = 0; m13 = 0 ;
		m21 = 0 ; m22 = cosTheta ; m23 = sinTheta ;
		m31 = 0 ; m32 = -sinTheta ; m33 = cosTheta ;
		break ;
	case Y:
		m11 = cosTheta ; m12 = 0 ; m13 = -sinTheta ;
		m21 = 0 ; m22 = 1 ; m23 = 0 ;
		m31 = sinTheta ; m32 = 0 ; m33 = cosTheta ;
		break ;
	case Z:
		m11 = cosTheta ; m12 = sinTheta ; m13 = 0 ;
		m21 = -sinTheta ; m22 = cosTheta ; m23 = 0 ;
		m31 = 0 ; m32 = 0 ; m33 = 1 ;
		break ;
	default:
		assert(false) ;
	}

	tx = ty = tz = 0 ;
}

void Matrix4x3 ::SetUpRotate(const Vector3& axis, float theta)
{
	assert(fabs(axis * axis - 1) < 0.01) ;

	float sinTheta = sin(theta) ;
	float cosTheta = cos(theta) ;

	m11 = axis.x * axis.x * (1 - cosTheta) + cosTheta ;
	m12 = axis.x * axis.y * (1 - cosTheta) + axis.z * sinTheta ;
	m13 = axis.x * axis.z * (1 - cosTheta) - axis.y * sinTheta ;
	
	m21 = axis.x * axis.y * (1 - cosTheta) - axis.z * sinTheta ;
	m22 = axis.y * axis.y * (1 - cosTheta) + cosTheta ;
	m23 = axis.y * axis.z * (1 - cosTheta) + axis.x * sinTheta ;

	m31 = axis.x * axis.z * (1 - cosTheta) + axis.y * sinTheta ;
	m32 = axis.y * axis.z * (1 - cosTheta) + axis.x * sinTheta ;
	m33 = axis.z * axis.z * (1 - cosTheta) + cosTheta ;

	tx = ty = tz = 0 ;
}

void Matrix4x3 ::FromQuaternion(const Quaternion& q)
{
	m11 = 1 - 2 * q.y * 2*q.y - 2 * q.z * q.z ;	m12 = 2 * q.x * q.y + 2 * q.w * q.z ;		m13 = 2 * q.x * q.z - 2 * q.w * q.y ;
	m21 = 2 * q.x*q.y - 2 * q.w * q.z ;				m22 = 1 - 2 * q.x * q.x - 2 * q.z * q.z ;	m23 = 2 * q.y * q.z + 2 * q.w * q.x ;
	m31 = 2 * q.x * q.z + 2 * q.w * q.y ;			m32 = 2 * q.y * q.z - 2 * q.w * q.x ;		m33 = 1 - 2 * q.x * q.x - 2 * q.y * q.y ;
	tx = 0 ;															ty = 0 ;														tz = 0 ;
}

void Matrix4x3 ::SetUpScale(const Vector3& s)
{
	m11 = s.x ; m12 = 0 ; m13 = 0 ;
	m21 = 0 ; m22 = s.y ; m23 = 0 ;
	m31 = 0 ; m32 = 0 ; m33 = s.z ;

	tx = ty = tz = 0 ;
}

void Matrix4x3 ::SetUpScaleAlongAxis(const Vector3& axis, float k)
{
	assert(fabs(axis * axis - 1) < 0.01) ;

	m11 = 1 + (k - 1) * axis.x * axis.x ; m12 = (k - 1) * axis.x * axis.y ;			m13 = (k - 1) * axis.x * axis.z ;
	m21 = (k - 1) * axis.x * axis.y ;		  m22 = 1 + (k - 1) * axis.y * axis.y ;	m23 = (k - 1) * axis.y * axis.z ;
	m13 = (k - 1) * axis.x * axis.z ;		  m32 = (k - 1) * axis.z * axis.y ;			m33 = 1 + (k - 1) * axis.z * axis.z ;
}

void Matrix4x3 ::SetUpShera(AxisName axisName, float s, float t)
{
	switch (axisName)
	{
	case X:
		m11 = 1 ; m12 = s; m13 = t ;
		m21 = 0 ; m22 = 1 ; m23 = 0 ;
		m31 = 0 ; m32 = 0 ; m33 = 1 ;
		break ;
	case Y:
		m11 = 1 ; m12 = 0 ; m13 = 0 ;
		m21 = s ; m22 = 1 ; m23 = t ;
		m31 = 0 ; m32 = 0 ; m33 = 1 ;
		break ;
	case Z:
		m11 = 1 ; m12 = 0 ; m13 = 0 ;
		m21 = 0 ; m22 = 1 ; m23 = 0 ;
		m31 = s ; m32 = t ; m33 = 1 ;
		break ;
	default:
		assert(false) ;
	}

	tx = ty = tz = 0 ;
}

//		 0 , 
void Matrix4x3 ::SetUpProject(const Vector3& n)
{
	m11 = 1 + (0 - 1) * n.x * n.x ; m12 = (0 - 1) * n.x * n.y ;			m13 = (0 - 1) * n.x * n.z ;
	m21 = (0 - 1) * n.z * n.y ;		 m22 = 1+ (0 - 1) * n.y * n.y ;		m23 = (0 - 1) * n.y * n.z ;
	m31 = (0 - 1) * n.x * n.z ;		 m32 = (0 - 1) * n.z * n.y ;			m33 = 1 + (0 - 1) * n.z * n.z ;

	tx = ty = tz = 0 ;
}

void Matrix4x3 ::SetUpReflect(AxisName axisName, float k = 0)
{
	switch (axisName)
	{
	case X:
		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 Y:
		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 Z:
		m11 = 1 ; m12 = 0 ; m13 = 0 ;
		m21 = 0 ; m22 = 1 ; m23 = 0 ;
		m31 = 0 ; m32 = 0 ; m33 = -1 ;
		tx = ty = 0 ;
		tz = 2 * k ;
		break;
	default:
		assert(false) ;
	}
}

void Matrix4x3 ::SetUpReflect(const Vector3& n)
{
	m11 = 1 + (0 - 1) * n.x * n.x ;
	m12 = (0 - 1) * n.x * n.y ;
	m13 = (0 - 1) * n.x * n.z ;

	m21 = (0 - 1) * n.x * n.y ;
	m22 = 1 + (0 - 1) * n.y * n.y ;
	m23 = (0 - 1) * n.y * n.z ;

	m31 = (0 - 1) * n.x * n.z ;
	m32 = (0 - 1) * n.z * n.y ;
	m33 = 1 + (0 - 1) * n.z * n.z ;

	tx = ty = tz = 0 ;
}

float Matrix4x3 ::Determinant(const Matrix4x3& m)
{
	return m.m11 * (m.m22 * m.m33 - m.m23 * m.m32) +
		m.m12 * (m.m23 * m.m31 - m.m21 * m.m33) +
		m.m13 * (m.m21 * m.m32 - m.m22 * m.m31) ;
}

Matrix4x3 Matrix4x3 ::Inverse(const Matrix4x3& m)
{
	float det = Matrix4x3.Determinant(m) ;

	assert(fabs(det) > Zero) ;

	Matrix4x3 result ;

	result.m11 = (m.m22 * m.m33 - m.m32 * m.m23) / det ;
	result.m12 = (m.m11 * m.m33 - m.m13 * m.m31) / det ;
	result.m13 = (m.m12 * m.m23 - m.m13 * m.m22) / det ;

	result.m21 = (m.m21 * m.m33 - m.m23 * m.m31) / det ;
	result.m22 = (m.m11 * m.m33 - m.m13 * m.m31) / det ;
	result.m23 = (m.m11 * m.m23 - m.m13 * m.m21) / det ;

	result.m31 = (m.m21 * m.m32 - m.m22 * m.m31) / det ;
	result.m32 = (m.m11 * m.m23 - m.m13 * m.m21) / det ;
	result.m33 = (m.m11 * m.m22 - m.m12 * m.m21) / det ;

	result.tx = -(m.tx * result.m11 + m.ty * result.m21 + m.tz * result.m31) ;
	result.ty = -(m.tx * result.m12 + m.ty * result.m22 + m.tz * result.m32) ;
	result.tz = -(m.tx * result.m13 + m.ty * result.m23 + m.tz * result.m33) ;

	return result ;
}