///////////////////////////////////////////////////////////////////////////////
//  S3D.cpp

#include <cassert>
#include "S3D.h"
#include "SMatrix.h"

S3D::S3D(void) :
	fX(0.0f), fY(0.0f), fZ(0.0f)
{
}

S3D::S3D(float fX0, float fY0, float fZ0) :
	fX(fX0), fY(fY0), fZ(fZ0)
{
}

bool S3D::operator==(const S3D& Obj) const
{
	return fX == Obj.fX && fY == Obj.fY && fZ == Obj.fZ;
}

bool S3D::operator!=(const S3D& Obj) const
{
	return fX != Obj.fX || fY != Obj.fY || fZ != Obj.fZ;
}

bool S3D::operator<(const S3D& Obj) const
{
	if (fX < Obj.fX) return true;
	if (fX > Obj.fX) return false;

	if (fY < Obj.fY) return true;
	if (fY > Obj.fY) return false;

	if (fZ < Obj.fZ) return true;
	if (fZ > Obj.fZ) return false;

	return false;
}

bool S3D::operator>(const S3D& Obj) const
{
	if (fX > Obj.fX) return true;
	if (fX < Obj.fX) return false;

	if (fY > Obj.fY) return true;
	if (fY < Obj.fY) return false;

	if (fZ > Obj.fZ) return true;
	if (fZ < Obj.fZ) return false;

	return false;
}

S3D& S3D::operator*=(float f)
{
	fX *= f;
	fY *= f;
	fZ *= f;

	return *this;
}

S3D& S3D::operator/=(float f)
{
	const float fInv = 1 / f;

	fX *= fInv;
	fY *= fInv;
	fZ *= fInv;

	return *this;
}

S3D& S3D::operator+=(const S3D& a)
{
	fX += a.fX;
	fY += a.fY;
	fZ += a.fZ;

	return *this;
}

S3D& S3D::operator-=(const S3D& a)
{
	fX -= a.fX;
	fY -= a.fY;
	fZ -= a.fZ;

	return *this;
}

const S3D& S3D::operator*=(const S3D& Obj)
{
	fX *= Obj.fX;
	fY *= Obj.fY;
	fZ *= Obj.fZ;

	return *this;
}

S3D& S3D::operator*=(const SMatrix& m)
{
	const float fNewX = fX * m.m[0][0] + fY * m.m[1][0] + fZ * m.m[2][0] + m.m[3][0];
	const float fNewY = fX * m.m[0][1] + fY * m.m[1][1] + fZ * m.m[2][1] + m.m[3][1];
	const float fNewZ = fX * m.m[0][2] + fY * m.m[1][2] + fZ * m.m[2][2] + m.m[3][2];

	const float fH = fX * m.m[0][3] + fY * m.m[1][3] + fZ * m.m[2][3] + m.m[3][3];

	if (fH == 1.0f) {
		fX = fNewX;
		fY = fNewY;
		fZ = fNewZ;
	}
	else if (fH != 0.0f) {
		const float fInvH = 1 / fH;

		fX = fNewX * fInvH;
		fY = fNewY * fInvH;
		fZ = fNewZ * fInvH;
	}

	return *this;
}

S3D S3D::operator*(const SMatrix& m) const
{
	S3D vRet(
		fX * m.m[0][0] + fY * m.m[1][0] + fZ * m.m[2][0] + m.m[3][0],
		fX * m.m[0][1] + fY * m.m[1][1] + fZ * m.m[2][1] + m.m[3][1],
		fX * m.m[0][2] + fY * m.m[1][2] + fZ * m.m[2][2] + m.m[3][2]);

	const float fH = fX * m.m[0][3] + fY * m.m[1][3] + fZ * m.m[2][3] + m.m[3][3];

	if (fH == 1.0f) {
	}
	else if (fH != 0.0f) {
		const float fInvH = 1 / fH;
		vRet.fX *= fInvH;
		vRet.fY *= fInvH;
		vRet.fZ *= fInvH;
	}

	return vRet;
}

S3D S3D::operator*(float f) const
{
	return S3D(fX * f, fY * f, fZ * f);
}

S3D S3D::operator/(float f) const
{
	return S3D(fX / f, fY / f, fZ / f);
}

S3D S3D::operator+(const S3D& a) const
{
	return S3D(fX + a.fX, fY + a.fY, fZ + a.fZ);
}

S3D S3D::operator-(const S3D& a) const
{
	return S3D(fX - a.fX, fY - a.fY, fZ - a.fZ);
}

S3D S3D::operator*(const S3D& Data) const
{
	return S3D(fX * Data.fX, fY * Data.fY, fZ * Data.fZ);
}

S3D operator*(float f, const S3D& Pos)
{
	return S3D(f * Pos.fX, f * Pos.fY, f * Pos.fZ);
}

S3D S3D::operator-() const
{
	return S3D(-fX, -fY, -fZ);
}

float
S3D::operator[](size_t idx)const {
	switch (idx) {
	case 0:
		return fX;
	case 1:
		return fY;
	case 2:
		return fZ;
	}

	assert(!"S3D[] out of range");

	return 0.0f;
}

//
void S3D::Set(float fX0, float fY0, float fZ0)
{
	fX = fX0; fY = fY0; fZ = fZ0;
}

//
S3D S3D::GetXZOnly() const
{
	return S3D(fX, 0.0f, fZ);
}

//
float S3D::Length() const
{
	return sqrtf(fX * fX + fY * fY + fZ * fZ);
}

float S3D::Length2() const
{
	return fX * fX + fY * fY + fZ * fZ;
}

float S3D::Distance(const S3D& pos1, const S3D& pos2)
{
	return sqrtf(POW2(pos1.fX - pos2.fX) + POW2(pos1.fY - pos2.fY) + POW2(pos1.fZ - pos2.fZ));
}

float S3D::Distance2(const S3D& pos1, const S3D& pos2)
{
	return POW2(pos1.fX - pos2.fX) + POW2(pos1.fY - pos2.fY) + POW2(pos1.fZ - pos2.fZ);
}

S3D S3D::Sub(const S3D& a, const S3D& b)
{
	return S3D(a.fX - b.fX, a.fY - b.fY, a.fZ - b.fZ);
}

S3D S3D::CrossProduct(const S3D& vec1, const S3D& vec2)
{
	return S3D(
		vec1.fY * vec2.fZ - vec1.fZ * vec2.fY,
		vec1.fZ * vec2.fX - vec1.fX * vec2.fZ,
		vec1.fX * vec2.fY - vec1.fY * vec2.fX);
}

float S3D::DotProduct(const S3D& vec1, const S3D& vec2)
{
	return vec1.fX * vec2.fX + vec1.fY * vec2.fY + vec1.fZ * vec2.fZ;
}

S3D S3D::GetOpenGLPos() const
{
	return S3D(fX, fY, -fZ);
}

// FromからToに回転する回転軸と角度（ラジアン）を求める
// ベクトルは正規化されているものとする
bool S3D::CalcRotateAxisAndAngle(S3D& RotateAxis, float& Angle, const S3D& FromVector, const S3D& ToVector)
{
	RotateAxis = S3D::CrossProduct(FromVector, ToVector).GetNormalize();
	const float dot = S3D::DotProduct(FromVector, ToVector);

	if (dot >= 1.0f) return false;

	// acosで、-1〜0〜1 -> π(180度)〜0に変換される
	Angle = acosf(dot);

	if (Angle < 1.0e-6f) return false;

	return true;
}

// 正規化
void S3D::Normalize()
{
	float fDenom = sqrtf(fX * fX + fY * fY + fZ * fZ);
	if (fDenom != 0.0f) {
		fDenom = 1.0f / fDenom;
		fX *= fDenom;
		fY *= fDenom;
		fZ *= fDenom;
	}
}

S3D S3D::GetNormalize() const
{
	float fDenom = sqrtf(fX * fX + fY * fY + fZ * fZ);
	if (fDenom != 0.0f) {
		fDenom = 1 / fDenom;
		return S3D(fX * fDenom, fY * fDenom, fZ * fDenom);
	}

	return *this;
}

//
S3D S3D::MulMatrixRot(const SMatrix& m) const
{
	return S3D(
		fX * m.m[0][0] + fY * m.m[1][0] + fZ * m.m[2][0],
		fX * m.m[0][1] + fY * m.m[1][1] + fZ * m.m[2][1],
		fX * m.m[0][2] + fY * m.m[1][2] + fZ * m.m[2][2]);
}

// ラジアン変換
S3D S3D::GetRadToDeg() const
{
	return S3D(RADtoDEG(fX), RADtoDEG(fY), RADtoDEG(fZ));
}

S3D S3D::GetDegToRad() const
{
	return S3D(DEGtoRAD(fX), DEGtoRAD(fY), DEGtoRAD(fZ));
}

// 座標系変換
S3D S3D::GetOpenGL() const
{
	return S3D(fX, fY, -fZ);
}