///////////////////////////////////////////////////////////////////////////////
//  SPlane.cpp

#include <algorithm>
#include "SPlane.h"

SPlane::SPlane(void) : fA(0.0f), fB(0.0f), fC(0.0f), fD(0.0f)
{
}

SPlane::SPlane(float fA_, float fB_, float fC_, float fD_) :
	fA(fA_), fB(fB_), fC(fC_), fD(fD_)
{
}

SPlane::SPlane(const S3D& pos, const S3D& vecN)
{
	fA = vecN.fX;
	fB = vecN.fY;
	fC = vecN.fZ;
	fD = -fA * pos.fX - fB * pos.fY - fC * pos.fZ;
}

SPlane::SPlane(const S3D& pos1, const S3D& pos2, const S3D& pos3)
{
	const S3D vecN = SPlane::CalcNormal(pos1, pos2, pos3);

	fA = vecN.fX;
	fB = vecN.fY;
	fC = vecN.fZ;
	fD = -fA * pos1.fX - fB * pos1.fY - fC * pos1.fZ;
}

bool SPlane::CalcIntersectSegment(const S3D& Pos1, const S3D& Pos2, S3D& IntersectPos) const
{
	const S3D& AB = Pos2 - Pos1;
	const float fTemp = S3D::DotProduct(S3D(fA, fB, fC), AB);
	if (fTemp == 0.0f) return false;

	const float fT = (-fD - S3D::DotProduct(S3D(fA, fB, fC), Pos1)) / fTemp;
	if (fT < 0.0f || fT > 1.0f) return false;

	IntersectPos = Pos1 + AB * fT;

	return true;
}

bool SPlane::CalcIntersectVector(const S3D& Pos, const S3D& Vector, S3D& IntersectPos) const
{
	const float fTemp = S3D::DotProduct(S3D(fA, fB, fC), Vector);
	if (fTemp == 0.0f) return false;

	const float fT = (-fD - S3D::DotProduct(S3D(fA, fB, fC), Pos)) / fTemp;

	IntersectPos = Pos + Vector * fT;

	return true;
}

bool SPlane::TestFrontPos(const S3D& Pos) const
{
	const float Dist = S3D::DotProduct(Pos, S3D(fA, fB, fC)) + fD;

	return (Dist >= 0.0f);
}

S3D SPlane::CalcNormal(const S3D& vtx1, const S3D& vtx2, const S3D& vtx3)
{
	float fNx = (vtx2.fY - vtx1.fY) * (vtx3.fZ - vtx2.fZ) - (vtx2.fZ - vtx1.fZ) * (vtx3.fY - vtx2.fY);
	float fNy = (vtx2.fZ - vtx1.fZ) * (vtx3.fX - vtx2.fX) - (vtx2.fX - vtx1.fX) * (vtx3.fZ - vtx2.fZ);
	float fNz = (vtx2.fX - vtx1.fX) * (vtx3.fY - vtx2.fY) - (vtx2.fY - vtx1.fY) * (vtx3.fX - vtx2.fX);

	float fN;
	float fDenom = sqrtf(fNx * fNx + fNy * fNy + fNz * fNz);
	if (fDenom == 0.0f) {
		fN = 0;
	}
	else fN = 1 / fDenom;

	fNx *= fN;
	fNy *= fN;
	fNz *= fN;

	return S3D(fNx, fNy, fNz);
}

bool SPlane::LinePolyCrossPoint(S3D pos1, S3D vec1, S3D pos2, S3D vec2, S3D& pos)
{
	float    a, b, t;

	S3D vec = S3D::Sub(pos2, pos1);
	a = S3D::DotProduct(vec, vec2);
	b = S3D::DotProduct(vec1, vec2);

	// 平行なので交点なし
	if (b == 0) return false;

	// 媒介変数ｔを求める
	t = a / b;

	// 直線の方程式に代入し、交点を求める
	pos.fX = pos1.fX + vec1.fX * t;
	pos.fY = pos1.fY + vec1.fY * t;
	pos.fZ = pos1.fZ + vec1.fZ * t;

	// 交点あり
	return true;
}

bool SPlane::PointTriangleSide(S3D pos1, S3D pos2, S3D pos3, S3D pos, S3D normal)
{
	float a;

	S3D vec = SPlane::CalcNormal(pos, pos1, pos2);
	a = S3D::DotProduct(vec, normal);
	if (a < 0) return false;

	vec = SPlane::CalcNormal(pos, pos2, pos3);
	a = S3D::DotProduct(vec, normal);
	if (a < 0) return false;

	vec = SPlane::CalcNormal(pos, pos3, pos1);
	a = S3D::DotProduct(vec, normal);
	if (a < 0) return false;

	return true;
}

bool SPlane::PointPolygonSide(S3D pos1, S3D pos2, S3D pos3, S3D pos4, S3D pos, S3D normal)
{
	// 最小最大範囲チェック
	S3D minpos = pos1, maxpos = pos1;
	if (minpos.fX > pos2.fX) minpos.fX = pos2.fX;
	if (minpos.fY > pos2.fY) minpos.fY = pos2.fY;
	if (minpos.fZ > pos2.fZ) minpos.fZ = pos2.fZ;
	if (maxpos.fX < pos2.fX) maxpos.fX = pos2.fX;
	if (maxpos.fY < pos2.fY) maxpos.fY = pos2.fY;
	if (maxpos.fZ < pos2.fZ) maxpos.fZ = pos2.fZ;

	if (minpos.fX > pos3.fX) minpos.fX = pos3.fX;
	if (minpos.fY > pos3.fY) minpos.fY = pos3.fY;
	if (minpos.fZ > pos3.fZ) minpos.fZ = pos3.fZ;
	if (maxpos.fX < pos3.fX) maxpos.fX = pos3.fX;
	if (maxpos.fY < pos3.fY) maxpos.fY = pos3.fY;
	if (maxpos.fZ < pos3.fZ) maxpos.fZ = pos3.fZ;

	if (minpos.fX > pos4.fX) minpos.fX = pos4.fX;
	if (minpos.fY > pos4.fY) minpos.fY = pos4.fY;
	if (minpos.fZ > pos4.fZ) minpos.fZ = pos4.fZ;
	if (maxpos.fX < pos4.fX) maxpos.fX = pos4.fX;
	if (maxpos.fY < pos4.fY) maxpos.fY = pos4.fY;
	if (maxpos.fZ < pos4.fZ) maxpos.fZ = pos4.fZ;

	if (pos.fX < minpos.fX || pos.fX > maxpos.fX ||
		pos.fY < minpos.fY || pos.fY > maxpos.fY ||
		pos.fZ < minpos.fZ || pos.fZ > maxpos.fZ) return false;

	float a;

	S3D vec = SPlane::CalcNormal(pos, pos1, pos2);
	a = S3D::DotProduct(vec, normal);
	if (a < 0) return false;

	vec = SPlane::CalcNormal(pos, pos2, pos3);
	a = S3D::DotProduct(vec, normal);
	if (a < 0) return false;

	vec = SPlane::CalcNormal(pos, pos3, pos4);
	a = S3D::DotProduct(vec, normal);
	if (a < 0) return false;

	vec = SPlane::CalcNormal(pos, pos4, pos1);
	a = S3D::DotProduct(vec, normal);
	if (a < 0) return false;

	return true;
}

// 平面と点の表裏判定
bool SPlane::PointPolySide(const S3D& pos, const S3D& posPlane, const S3D& vecN)
{
	const S3D vec = S3D::Sub(pos, posPlane);
	const float f = S3D::DotProduct(vec, vecN);

	if (f >= 0.0f) return true;	// 表

	return false;	// 裏
}

// 平面と点の距離
float SPlane::CalcDistance(const S3D& pos, const S3D& posPlane, const S3D& vecN)
{
	const S3D vec = S3D::Sub(pos, posPlane);
	const float f = S3D::DotProduct(vec, vecN);

	return (std::max)(f, -f);
}

float SPlane::CalcHeight(S3D pos, S3D posPlane, S3D vecN)
{
	S3D vec = S3D::Sub(pos, posPlane);
	float f = S3D::DotProduct(vec, vecN);

	return f;
}

// 重心座標計算
void SPlane::CalcCenterGravity(S3D pos0, S3D pos1, S3D pos2, S3D pos, float& M0, float& M1, float& M2)
{
	S3D e0 = S3D::CrossProduct(pos1, pos2);
	S3D e1 = S3D::CrossProduct(pos2, pos0);
	S3D e2 = S3D::CrossProduct(pos0, pos1);

	float fInvD = 1 / S3D::DotProduct(e2, pos2);

	float A_M0 = e0.fX * fInvD;
	float B_M0 = e0.fY * fInvD;
	float C_M0 = e0.fZ * fInvD;

	float A_M1 = e1.fX * fInvD;
	float B_M1 = e1.fY * fInvD;
	float C_M1 = e1.fZ * fInvD;

	float A_M2 = e2.fX * fInvD;
	float B_M2 = e2.fY * fInvD;
	float C_M2 = e2.fZ * fInvD;

	M0 = A_M0 * pos.fX + B_M0 * pos.fY + C_M0 * pos.fZ;
	M1 = A_M1 * pos.fX + B_M1 * pos.fY + C_M1 * pos.fZ;
	M2 = A_M2 * pos.fX + B_M2 * pos.fY + C_M2 * pos.fZ;
}

// 反射ベクトルを求める
S3D SPlane::CalcMirrorVector(const S3D& v, const S3D& N)
{
	// r = -2 (n dot v) n + v
	return (-2.0f * S3D::DotProduct(N, v) * N + v).GetNormalize();
}