///////////////////////////////////////////////////////////////////////////////
//  SMatrix.cpp

#include <math.h>
#include <string.h>
#include <algorithm>
#include "3DDefine.h"
#include "SMatrix.h"
#include "S3D.h"
#include "SQuaternion.h"

// 未初期化用コンストラクタ
SMatrix::SMatrix(int)
{
}

//
SMatrix::SMatrix(const float* pData)
{
	int i, j;
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			m[i][j] = pData[i * 4 + j];
		}
	}
}

bool SMatrix::operator==(const SMatrix& Right) const
{
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			if (m[i][j] != Right.m[i][j]) return false;
		}
	}

	return true;
}

bool SMatrix::operator!=(const SMatrix& Right) const
{
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			if (m[i][j] != Right.m[i][j]) return true;
		}
	}

	return false;
}

SMatrix	operator*(const SMatrix& b, const SMatrix& a)
{
	SMatrix Result(1);		// 未初期化

	Result.mm[ 0] =  b.mm[ 0] * a.mm[ 0];
	Result.mm[ 1] =  b.mm[ 0] * a.mm[ 1];
	Result.mm[ 2] =  b.mm[ 0] * a.mm[ 2];
	Result.mm[ 3] =  b.mm[ 0] * a.mm[ 3];

	Result.mm[ 0] += b.mm[ 1] * a.mm[ 4];
	Result.mm[ 1] += b.mm[ 1] * a.mm[ 5];
	Result.mm[ 2] += b.mm[ 1] * a.mm[ 6];
	Result.mm[ 3] += b.mm[ 1] * a.mm[ 7];

	Result.mm[ 0] += b.mm[ 2] * a.mm[ 8];
	Result.mm[ 1] += b.mm[ 2] * a.mm[ 9];
	Result.mm[ 2] += b.mm[ 2] * a.mm[10];
	Result.mm[ 3] += b.mm[ 2] * a.mm[11];

	Result.mm[ 0] += b.mm[ 3] * a.mm[12];
	Result.mm[ 1] += b.mm[ 3] * a.mm[13];
	Result.mm[ 2] += b.mm[ 3] * a.mm[14];
	Result.mm[ 3] += b.mm[ 3] * a.mm[15];

	Result.mm[ 4] =  b.mm[ 4] * a.mm[ 0];
	Result.mm[ 5] =  b.mm[ 4] * a.mm[ 1];
	Result.mm[ 6] =  b.mm[ 4] * a.mm[ 2];
	Result.mm[ 7] =  b.mm[ 4] * a.mm[ 3];

	Result.mm[ 4] += b.mm[ 5] * a.mm[ 4];
	Result.mm[ 5] += b.mm[ 5] * a.mm[ 5];
	Result.mm[ 6] += b.mm[ 5] * a.mm[ 6];
	Result.mm[ 7] += b.mm[ 5] * a.mm[ 7];

	Result.mm[ 4] += b.mm[ 6] * a.mm[ 8];
	Result.mm[ 5] += b.mm[ 6] * a.mm[ 9];
	Result.mm[ 6] += b.mm[ 6] * a.mm[10];
	Result.mm[ 7] += b.mm[ 6] * a.mm[11];

	Result.mm[ 4] += b.mm[ 7] * a.mm[12];
	Result.mm[ 5] += b.mm[ 7] * a.mm[13];
	Result.mm[ 6] += b.mm[ 7] * a.mm[14];
	Result.mm[ 7] += b.mm[ 7] * a.mm[15];

	Result.mm[ 8] =  b.mm[ 8] * a.mm[ 0];
	Result.mm[ 9] =  b.mm[ 8] * a.mm[ 1];
	Result.mm[10] =  b.mm[ 8] * a.mm[ 2];
	Result.mm[11] =  b.mm[ 8] * a.mm[ 3];

	Result.mm[ 8] += b.mm[ 9] * a.mm[ 4];
	Result.mm[ 9] += b.mm[ 9] * a.mm[ 5];
	Result.mm[10] += b.mm[ 9] * a.mm[ 6];
	Result.mm[11] += b.mm[ 9] * a.mm[ 7];

	Result.mm[ 8] += b.mm[10] * a.mm[ 8];
	Result.mm[ 9] += b.mm[10] * a.mm[ 9];
	Result.mm[10] += b.mm[10] * a.mm[10];
	Result.mm[11] += b.mm[10] * a.mm[11];

	Result.mm[ 8] += b.mm[11] * a.mm[12];
	Result.mm[ 9] += b.mm[11] * a.mm[13];
	Result.mm[10] += b.mm[11] * a.mm[14];
	Result.mm[11] += b.mm[11] * a.mm[15];

	Result.mm[12] =  b.mm[12] * a.mm[ 0];
	Result.mm[13] =  b.mm[12] * a.mm[ 1];
	Result.mm[14] =  b.mm[12] * a.mm[ 2];
	Result.mm[15] =  b.mm[12] * a.mm[ 3];

	Result.mm[12] += b.mm[13] * a.mm[ 4];
	Result.mm[13] += b.mm[13] * a.mm[ 5];
	Result.mm[14] += b.mm[13] * a.mm[ 6];
	Result.mm[15] += b.mm[13] * a.mm[ 7];

	Result.mm[12] += b.mm[14] * a.mm[ 8];
	Result.mm[13] += b.mm[14] * a.mm[ 9];
	Result.mm[14] += b.mm[14] * a.mm[10];
	Result.mm[15] += b.mm[14] * a.mm[11];

	Result.mm[12] += b.mm[15] * a.mm[12];
	Result.mm[13] += b.mm[15] * a.mm[13];
	Result.mm[14] += b.mm[15] * a.mm[14];
	Result.mm[15] += b.mm[15] * a.mm[15];

	return Result;
}

SMatrix& SMatrix::operator*=(const SMatrix& a)
{
	*this = *this * a;

	return *this;
}

void SMatrix::MulMatrix(const SMatrix& a)
{
	*this = *this * a;
}

SMatrix SMatrix::CreateMove(const S3D& Pos)
{
	return SMatrix(
			1,  0,  0, 0,
			0,  1,  0, 0,
			0,  0,  1, 0,
		Pos.fX, Pos.fY, Pos.fZ, 1);

}

SMatrix SMatrix::CreateScale(const S3D& Scale)
{
	return SMatrix(
		Scale.fX,        0,        0, 0,
		       0, Scale.fY,        0, 0,
		       0,        0, Scale.fZ, 0,
		       0,        0,        0, 1);

}

SMatrix SMatrix::CreateRotAxis(const S3D& Vec, float Radian)
{
	const float x = Vec.fX;
	const float y = Vec.fY;
	const float z = Vec.fZ;

	const float c = cosf(Radian);
	const float s = sinf(Radian);

	return SMatrix(
		x * x * (1 - c) + c,		x * y * (1 - c) + z * s,	x * z * (1 - c) - y * s,	0,
		x * y * (1 - c) - z * s,	y * y * (1 - c) + c,		y * z * (1 - c) + x * s,	0,
		x * z * (1 - c) + y * s,	y * z * (1 - c) - x * s,	z * z * (1 - c) + c,		0,
		0, 0, 0, 1);

}

SMatrix SMatrix::CreateLookAt(const S3D& EyePos, const S3D& AtPos, const S3D& UpVec)
{
	const S3D ZAxis = (AtPos - EyePos).GetNormalize();
	const S3D XAxis = S3D::CrossProduct(UpVec, ZAxis).GetNormalize();
	const S3D YAxis = S3D::CrossProduct(ZAxis, XAxis);

	return SMatrix(
		XAxis.fX, XAxis.fY, XAxis.fZ, 0.0f,
		YAxis.fX, YAxis.fY, YAxis.fZ, 0.0f,
		ZAxis.fX, ZAxis.fY, ZAxis.fZ, 0.0f,
		-S3D::DotProduct(XAxis, EyePos),
		-S3D::DotProduct(YAxis, EyePos),
		-S3D::DotProduct(ZAxis, EyePos),
		1.0f);
}

SMatrix SMatrix::CreateXYZVector(const S3D& XVector, const S3D& YVector, const S3D& ZVector)
{
	return SMatrix(
		XVector.fX, XVector.fY, XVector.fZ, 0.0f,
		YVector.fX, YVector.fY, YVector.fZ, 0.0f,
		ZVector.fX, ZVector.fY, ZVector.fZ, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f);

}

void SMatrix::Identity(void)
{
	m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f;
	m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f; m[1][3] = 0.0f;
	m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f; m[2][3] = 0.0f;
	m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f;

}

void SMatrix::Trans(void)
{
	std::swap(m[0][1], m[1][0]);
	std::swap(m[0][2], m[2][0]);
	std::swap(m[0][3], m[3][0]);

	std::swap(m[1][2], m[2][1]);
	std::swap(m[1][3], m[3][1]);
	std::swap(m[2][3], m[3][2]);
}

float SMatrix::GetScaleX() const
{
	return sqrtf(POW2(m[0][0]) + POW2(m[0][1]) + POW2(m[0][2]));
}

void SMatrix::SetRotHRad(float fAngle)
{
	const float fCos = cosf(fAngle);
	const float fSin = sinf(fAngle);

	// ヘディング（ヨー）　垂直軸（Direct3DではY軸）回転
	Set(fCos, 0, -fSin, 0,
		   0, 1,     0, 0,
		fSin, 0,  fCos, 0,
		   0, 0,     0, 1);
}

void SMatrix::SetRotPRad(float fAngle)
{
	const float fCos = cosf(fAngle);
	const float fSin = sinf(fAngle);

	// ピッチ　水平軸（Direct3DではX軸）回転
	Set(1,     0,    0, 0,
		0,  fCos, fSin, 0,
		0, -fSin, fCos, 0,
		0,     0,    0, 1);
}

void SMatrix::SetRotBRad(float fAngle)
{
	const float fCos = cosf(fAngle);
	const float fSin = sinf(fAngle);

	// バンク（ロール）　奥行き軸（Direct3DではZ軸）回転
	Set( fCos, fSin, 0, 0,
		-fSin, fCos, 0, 0,
		    0,    0, 1, 0,
		    0,    0, 0, 1);
}

void SMatrix::SetRotH(float fAngle)
{
	SetRotHRad(DEGtoRAD(fAngle));
}

void SMatrix::SetRotP(float fAngle)
{
	SetRotPRad(DEGtoRAD(fAngle));
}

void SMatrix::SetRotB(float fAngle)
{
	SetRotBRad(DEGtoRAD(fAngle));
}

void SMatrix::Inverse(void)
{
	*this = GetInverse();
}

float SMatrix::det2x2(const float a1, const float a2, const float b1, const float b2) const
{
	return a1*b2 - b1*a2;
}

/*	| a1 a2 a3 |
	| b1 b2 b3 |
	| c1 c2 c3 | calculate the determinent of a 3x3 matrix*/
float SMatrix::det3x3(const float a1, const float a2, const float a3,
	const float b1, const float b2, const float b3, const float c1, const float c2, const float c3) const
{
	return a1*det2x2(b2,b3,c2,c3) - b1*det2x2(a2,a3,c2,c3) +
			c1*det2x2(a2,a3,b2,b3);
}

SMatrix SMatrix::GetInverse(void) const
{
	float a11 =  det3x3(m[1][1],m[2][1],m[3][1],m[1][2],m[2][2],m[3][2],m[1][3],m[2][3],m[3][3]);
	float a21 = -det3x3(m[1][0],m[2][0],m[3][0],m[1][2],m[2][2],m[3][2],m[1][3],m[2][3],m[3][3]);
	float a31 =  det3x3(m[1][0],m[2][0],m[3][0],m[1][1],m[2][1],m[3][1],m[1][3],m[2][3],m[3][3]);
	float a41 = -det3x3(m[1][0],m[2][0],m[3][0],m[1][1],m[2][1],m[3][1],m[1][2],m[2][2],m[3][2]);

	float a12 = -det3x3(m[0][1],m[2][1],m[3][1],m[0][2],m[2][2],m[3][2],m[0][3],m[2][3],m[3][3]);
	float a22 =  det3x3(m[0][0],m[2][0],m[3][0],m[0][2],m[2][2],m[3][2],m[0][3],m[2][3],m[3][3]);
	float a32 = -det3x3(m[0][0],m[2][0],m[3][0],m[0][1],m[2][1],m[3][1],m[0][3],m[2][3],m[3][3]);
	float a42 =  det3x3(m[0][0],m[2][0],m[3][0],m[0][1],m[2][1],m[3][1],m[0][2],m[2][2],m[3][2]);

	float a13 =  det3x3(m[0][1],m[1][1],m[3][1],m[0][2],m[1][2],m[3][2],m[0][3],m[1][3],m[3][3]);
	float a23 = -det3x3(m[0][0],m[1][0],m[3][0],m[0][2],m[1][2],m[3][2],m[0][3],m[1][3],m[3][3]);
	float a33 =  det3x3(m[0][0],m[1][0],m[3][0],m[0][1],m[1][1],m[3][1],m[0][3],m[1][3],m[3][3]);
	float a43 = -det3x3(m[0][0],m[1][0],m[3][0],m[0][1],m[1][1],m[3][1],m[0][2],m[1][2],m[3][2]);

	float a14 = -det3x3(m[0][1],m[1][1],m[2][1],m[0][2],m[1][2],m[2][2],m[0][3],m[1][3],m[2][3]);
	float a24 =  det3x3(m[0][0],m[1][0],m[2][0],m[0][2],m[1][2],m[2][2],m[0][3],m[1][3],m[2][3]);
	float a34 = -det3x3(m[0][0],m[1][0],m[2][0],m[0][1],m[1][1],m[2][1],m[0][3],m[1][3],m[2][3]);
	float a44 =  det3x3(m[0][0],m[1][0],m[2][0],m[0][1],m[1][1],m[2][1],m[0][2],m[1][2],m[2][2]);

	float det = (m[0][0]*a11) + (m[0][1]*a21) + (m[0][2]*a31) + (m[0][3]*a41);
	float oodet = 1/det;

	SMatrix c;

	c.m[0][0] = a11*oodet;
	c.m[1][0] = a21*oodet;
	c.m[2][0] = a31*oodet;
	c.m[3][0] = a41*oodet;

	c.m[0][1] = a12*oodet;
	c.m[1][1] = a22*oodet;
	c.m[2][1] = a32*oodet;
	c.m[3][1] = a42*oodet;

	c.m[0][2] = a13*oodet;
	c.m[1][2] = a23*oodet;
	c.m[2][2] = a33*oodet;
	c.m[3][2] = a43*oodet;

	c.m[0][3] = a14*oodet;
	c.m[1][3] = a24*oodet;
	c.m[2][3] = a34*oodet;
	c.m[3][3] = a44*oodet;

	return c;

}

SQuaternion SMatrix::GetQuaternion(void) const
{
	const float fTr = m[0][0] + m[1][1] + m[2][2] + m[3][3];

	if (fTr >= 1.0) {
		const float fFourD = 2.0f * static_cast<float>(sqrt(fTr));

		return SQuaternion(
			(m[1][2] - m[2][1]) / fFourD,
			(m[2][0] - m[0][2]) / fFourD,
			(m[0][1] - m[1][0]) / fFourD,
			fFourD / 4.0f);
	}
	else {
		int i, j, k;

		if (m[0][0] > m[1][1]) i = 0;
		else i = 1;

		if (m[2][2] > m[i][i]) i = 2;

		j = (i + 1) % 3;
		k = (j + 1) % 3;

		const float fFourD = 2.0f * static_cast<float>(sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f));

		float fQ[4];
		fQ[i] = fFourD / 4.0f;
		fQ[j] = (m[i][j] + m[j][i]) / fFourD;
		fQ[k] = (m[i][k] + m[k][i]) / fFourD;
		fQ[3] = (m[j][k] - m[k][j]) / fFourD;

		return SQuaternion(fQ[0], fQ[1], fQ[2], fQ[3]);
	}

}

S3D SMatrix::GetMove(void) const
{
	return S3D(m[3][0], m[3][1], m[3][2]);

}

SMatrix SMatrix::GetOpenGLMatrix() const
{
	// １〜３行目はそれぞれX,Y,Z軸なので、XY軸はZのみ反転し、Z軸はそのものを反転させる

	return SMatrix(
		 m[0][0],  m[0][1], -m[0][2],  m[0][3],
		 m[1][0],  m[1][1], -m[1][2],  m[1][3],
		-m[2][0], -m[2][1],  m[2][2],  m[2][3],
		 m[3][0],  m[3][1], -m[3][2],  m[3][3]);

}

S3D SMatrix::GetXVector() const
{
	return S3D(m[0][0], m[0][1], m[0][2]);
}

S3D SMatrix::GetYVector() const
{
	return S3D(m[1][0], m[1][1], m[1][2]);
}

S3D SMatrix::GetZVector() const
{
	return S3D(m[2][0], m[2][1], m[2][2]);
}

S4D SMatrix::GetXVector4() const
{
	return S4D(m[0][0], m[0][1], m[0][2], m[0][3]);
}

S4D SMatrix::GetYVector4() const
{
	return S4D(m[1][0], m[1][1], m[1][2], m[1][3]);
}

S4D SMatrix::GetZVector4() const
{
	return S4D(m[2][0], m[2][1], m[2][2], m[2][3]);
}

// オイラー角
S3D SMatrix::GetEulerRadXYZ() const
{
	// X-Y-Z順で回転しているとする

	// X-Rot			Y-Rot			Z-Rot
	// | 1   0  0 |		| Cy 0 -Sy |	|  Cz Sz 0 |
	// | 0  Cx Sx |		|  0 1   0 |	| -Sz Cz 0 |
	// | 0 -Sx Cx |		| Sy 0  Cy |	|   0  0 1 |

	// X-Rot * Y-Rot
	// |   Cy   0  -Sy |
	// | SxSy  Cx SxCy |
	// | CxSy -Sx CxCy |

	// X-Rot * Y-Rot * Z-Rot
	// | CyCz,			CySz,			-Sy  |
	// | SxSyCz - CxSz,	SxSySz + CxCz,	SxCy |
	// | CxSyCz + SxSz,	CxSySz - SxCz,	CxCy |

	float RotH, RotP, RotB;

	//
	const float SinY = -m[0][2];

	// SinYのアークサインからY軸回転のθが求まる
	RotH = asinf(SinY);

	//
	if (SinY == 1.0f) {
		// SinY == 1.0fならCosY == 0.0fになるので、m[0][1] / m[0][0] = CySz / CyCzの式を使用できない
		// θx == 0.0fと仮定すると、

		// m[1][0] = SxCz - CxSz = S(x-z)
		// m[1][1] = SxSz + CxCz = CxCz + SxSz = C(x+z)
		// となるので、m[1][0] / m[1][1] = -Sz / Cz = -tanZとなる
		RotB = atan2f(-m[1][0], m[1][1]);

		RotP = 0.0f;
	}
	else if (SinY == -1.0f) {
		// SinY == -1.0fならCosY == 0.0fになるので、m[0][1] / m[0][0] = CySz / CyCzの式を使用できない
		// θx == 0.0fと仮定すると、

		// m[1][0] = -SxCz - CxSz = -S(x+z)
		// m[1][1] = -SxSz + CxCz = CxCz - SxSz = C(x-z)
		// となるので、m[1][0] / m[1][1] = -Sz / -Cz = tanZとなる
		RotB = atan2f(-m[1][0], -m[1][1]);

		RotP = 0.0f;
	}
	else {
		// tan = sin / cosなので、m[0][1] / m[0][0] = CySz / CyCz = Sz / Cz = tanZとなる
		// tanZのアークタンジェントからZ軸回転のθが求まる
		RotB = atan2f(m[0][1], m[0][0]);

		// 同様に、m[1][2] / m[2][2] = SxCy / CxCy = Sx / Cx = tanXとなり、X軸回転のθが求まる
		RotP = atan2f(m[1][2], m[2][2]);
	}

	return S3D(RotP, RotH, RotB);
}

S3D SMatrix::GetEulerRadYXZ() const
{
	// Y-X-Z順で回転しているとする

	// | CyCz - SxSySz,	SzCy + SxSyCz,	-CxSy |
	// | -CxSz,			CxCz,			Sx    |
	// | SyCz + SxCySz,	SySz - SxCyCz,	CxCy  |

	float RotH, RotP, RotB;

	//
	const float SinX = m[1][2];

	// SinXのアークサインからX軸回転のθが求まる
	RotP = asinf(SinX);

	//
	if (SinX == 1.0f) {
		// SinX == 1.0fならCosX == 0.0fになるので、m[1][0] / m[1][1] = -CxSz / CxSzの式を使用できない
		// θz == 0.0fと仮定すると、

		// m[0][1] = SzCy + CzSy = S(z+y)
		// m[0][0] = CyCz - SySz = C(y-z)
		// となるので、m[0][1] / m[0][0] = Sy / Cy = tanZとなる
		RotH = atan2f(m[0][1], m[0][0]);

		RotB = 0.0f;
	}
	else if (SinX == -1.0f) {
		// SinX == -1.0fならCosX == 0.0fになるので、m[1][0] / m[1][1] = -CxSz / CxSzの式を使用できない
		// θz == 0.0fと仮定すると、

		// m[0][1] = SzCy - CzSy = S(z-y)
		// m[0][0] = CyCz + SySz = C(y+z)
		// となるので、m[0][1] / m[0][0] = -Sy / Cy = -tanZとなる
		RotH = atan2f(-m[0][1], m[0][0]);

		RotB = 0.0f;
	}
	else {
		// tan = sin / cosなので、m[1][0] / m[1][1] = -CxSz / CxCz = -Sz / Cz = -tanZとなる
		// tanZのアークタンジェントからZ軸回転のθが求まる
		RotB = atan2f(-m[1][0], m[1][1]);

		// 同様に、m[0][2] / m[2][2] = -CxSy / CxCy = -Sy / Cy = -tanYとなり、Y軸回転のθが求まる
		RotH = atan2f(-m[0][2], m[2][2]);
	}

	return S3D(RotP, RotH, RotB);
}

S3D SMatrix::GetEulerRadZXY() const
{
	// Z-X-Y順で回転しているとする

	// X-Rot			Y-Rot			Z-Rot
	// | 1   0  0 |		| Cy 0 -Sy |	|  Cz Sz 0 |
	// | 0  Cx Sx |		|  0 1   0 |	| -Sz Cz 0 |
	// | 0 -Sx Cx |		| Sy 0  Cy |	|   0  0 1 |

	// Z-Rot * X-Rot
	// |  Cz CxSz SxSz |
	// | -Sz CxCz SxCz |
	// |   0  -Sx   Cx |

	// Z-Rot * X-Rot * Y-Rot

	// | CzCy + SzSxSy,		SzCx,	-CzSy + SzSxCy	|
	// | -SzCy + CzSxSy,	CzCx,	SzSy + CzSxCy	|
	// | CxSy,				-Sx,	CxCy			|

	float RotH, RotP, RotB;

	//
	const float SinX = -m[2][1];

	// SinXのアークサインからX軸回転のθが求まる
	RotP = asinf(SinX);

	//
	if (SinX == 1.0f) {
		// SinX == 1.0fならCosX == 0.0fになるので、m[1][0] / m[1][1] = -CxSz / CxSzの式を使用できない
		// θy == 0.0fと仮定すると、

		// m[1][0] = -SzCy + CzSy = -(SzCy - CzSy) = -S(z-y)
		// m[0][0] = CzCy + SzSy = C(z+y)
		// となるので、m[1][0] / m[0][0] = -Sz / Cz = -tanZとなる
		RotB = atan2f(-m[1][0], m[0][0]);

		RotH = 0.0f;
	}
	else if (SinX == -1.0f) {
		// SinX == -1.0fならCosX == 0.0fになるので、m[1][0] / m[1][1] = -CxSz / CxSzの式を使用できない
		// θy == 0.0fと仮定すると、

		// m[1][0] = -SzCy - CzSy = -(SzCy + CzSy) = -S(z+y)
		// m[0][0] = CzCy - SzSy = C(z-y)
		// となるので、m[1][0] / m[0][0] = -Sz / Cz = -tanZとなる
		RotB = atan2f(-m[1][0], m[0][0]);

		RotH = 0.0f;
	}
	else {
		// tan = sin / cosなので、m[0][1] / m[1][1] = SzCx / CzCx = Sz / Cz = tanZとなる
		// tanZのアークタンジェントからZ軸回転のθが求まる
		RotB = atan2f(m[0][1], m[1][1]);

		// 同様に、m[2][0] / m[2][2] = CxSy / CxCy = Sy / Cy = tanYとなり、Y軸回転のθが求まる
		RotH = atan2f(m[2][0], m[2][2]);
	}

	return S3D(RotP, RotH, RotB);
}

S3D SMatrix::GetEulerRadZYX() const
{
	// Z-Y-X順で回転しているとする

	// X-Rot			Y-Rot			Z-Rot
	// | 1   0  0 |		| Cy 0 -Sy |	|  Cz Sz 0 |
	// | 0  Cx Sx |		|  0 1   0 |	| -Sz Cz 0 |
	// | 0 -Sx Cx |		| Sy 0  Cy |	|   0  0 1 |

	// Z-Rot * Y-Rot
	// |  CyCz Sz -SyCz |
	// | -CySz Cz  SySz |
	// |    Sy  0    Cy |

	// Z-Rot * Y-Rot * X-Rot

	// | CyCz,	CxSz + SxSyCz,	SxSz - CxSyCz	|
	// | -CySz,	CxCz - SxSySz,	SxCz + CxSySz	|
	// | Sy,	-SxCy,			CxCy			|

	float RotH, RotP, RotB;

	//
	const float SinY = m[2][0];

	// SinYのアークサインからY軸回転のθが求まる
	RotH = asinf(SinY);

	// 未完成
	/*if (SinY == 1.0f) {
		// SinX == 1.0fならCosX == 0.0fになるので、m[1][0] / m[1][1] = -CxSz / CxSzの式を使用できない
		// θy == 0.0fと仮定すると、

		// m[1][0] = -SzCy + CzSy = -(SzCy - CzSy) = -S(z-y)
		// m[0][0] = CzCy + SzSy = C(z+y)
		// となるので、m[1][0] / m[0][0] = -Sz / Cz = -tanZとなる
		RotB = atan2(-m[1][0], m[0][0]);

		RotH = 0.0f;
	}
	else if (SinY == -1.0f) {
		// SinX == -1.0fならCosX == 0.0fになるので、m[1][0] / m[1][1] = -CxSz / CxSzの式を使用できない
		// θy == 0.0fと仮定すると、

		// m[1][0] = -SzCy - CzSy = -(SzCy + CzSy) = -S(z+y)
		// m[0][0] = CzCy - SzSy = C(z-y)
		// となるので、m[1][0] / m[0][0] = -Sz / Cz = -tanZとなる
		RotB = atan2(-m[1][0], m[0][0]);

		RotH = 0.0f;
	}
	else */{
		// tan = sin / cosなので、m[2][1] / m[2][2] = -SxCy / CxCy = -Sx / Cx = tanXとなる
		// tanXのアークタンジェントからX軸回転のθが求まる
		RotP = atan2f(-m[2][1], m[2][2]);

		// 同様に、m[1][0] / m[0][0] = -CySz / CyCz = -Sz / Cz = tanZとなり、Z軸回転のθが求まる
		RotB = atan2f(-m[1][0], m[0][0]);
	}

	return S3D(RotP, RotH, RotB);
}