////////////////////////////////////////////////////////
// filename: common.cpp
// author: Chafumi Touji
// version: 1.0.0
// date: 2020/03/05
////////////////////////////////////////////////////////

#include "defcha3dcore.h"

float ToRadian(float angle) {

	angle /= 360.000f;
	angle *= 2.00f;
	angle *= 3.14159f;

	return angle;
}

float ToAngle(float radian) {

	radian /= 2.00f;
	radian /= 3.14159f;
	radian *= 360.000f;

	return radian;
}

void MatrixSet(matrix* mtx, float m0, float m1, float m2, float m3, float m4, float m5, float m6, float m7, float m8, float m9, float m10, float m11, float m12, float m13, float m14, float m15) {

	mtx->m11 = m0;
	mtx->m12 = m1;
	mtx->m13 = m2;
	mtx->m14 = m3;
	mtx->m21 = m4;
	mtx->m22 = m5;
	mtx->m23 = m6;
	mtx->m24 = m7;
	mtx->m31 = m8;
	mtx->m32 = m9;
	mtx->m33 = m10;
	mtx->m34 = m11;
	mtx->m41 = m12;
	mtx->m42 = m13;
	mtx->m43 = m14;
	mtx->m44 = m15;
}

void MatrixInit(matrix* mtx) {

	mtx->m11 = 1;
	mtx->m12 = 0;
	mtx->m13 = 0;
	mtx->m14 = 0;
	mtx->m21 = 0;
	mtx->m22 = 1;
	mtx->m23 = 0;
	mtx->m24 = 0;
	mtx->m31 = 0;
	mtx->m32 = 0;
	mtx->m33 = 1;
	mtx->m34 = 0;
	mtx->m41 = 0;
	mtx->m42 = 0;
	mtx->m43 = 0;
	mtx->m44 = 1;
}

bool GetParam(char* line, char* param1, char* param2, char* c1) {

	memset(param1, 0, SIZE_CHAR_MAX);
	memset(param2, 0, SIZE_CHAR_MAX);

	char* ctx;
	char* tmp = strtok_s(line, c1, &ctx);
	if (tmp != NULL) {
		strcpy_s(param1, SIZE_CHAR_MAX, tmp);
		tmp = strtok_s(NULL, c1, &ctx);
		if (tmp != NULL) {
			strcpy_s(param2, SIZE_CHAR_MAX, tmp);
		}
	}

	return true;
}

bool GetParamW(wchar_t* w_line, wchar_t* param1, wchar_t* param2, wchar_t* c1) {

	wmemset(param1, 0, SIZE_CHAR_MAX);
	wmemset(param2, 0, SIZE_CHAR_MAX);

	wchar_t* ctx;
	wchar_t* tmp = wcstok_s(w_line, c1, &ctx);
	if (tmp != NULL) {
		wcscpy_s(param1, SIZE_CHAR_MAX, tmp);
		tmp = wcstok_s(NULL, c1, &ctx);
		if (tmp != NULL) {
			wcscpy_s(param2, SIZE_CHAR_MAX, tmp);
		}
	}

	return true;
}

bool SetVertex(CVertex* vtx, float pos_x, float pos_y, float pos_z, float normal_x, float normal_y, float normal_z, float color_r, float color_g, float color_b, float color_a, float u, float v) {

	vtx->Pos.x = pos_x;
	vtx->Pos.y = pos_y;
	vtx->Pos.z = pos_z;
	vtx->Normal.x = normal_x;
	vtx->Normal.y = normal_y;
	vtx->Normal.z = normal_z;
	vtx->Color.x = color_r;
	vtx->Color.y = color_g;
	vtx->Color.z = color_b;
	vtx->Color.w = color_a;
	vtx->UV.x = u;
	vtx->UV.y = v;

	return true;
}

XMFLOAT4 CalcCenterPos(XMFLOAT4 pos1, XMFLOAT4 pos2, XMFLOAT4 pos3) {

	//三角形の重心を計算する。

	XMFLOAT4 out_pos;
	out_pos.x = (pos1.x + pos2.x + pos3.x) / 3.0f;
	out_pos.y = (pos1.y + pos2.y + pos3.y) / 3.0f;
	out_pos.z = (pos1.z + pos2.z + pos3.z) / 3.0f;

	return out_pos;
}

bool CalcDistPointPoint(XMFLOAT4 pos1, XMFLOAT4 pos2, XMFLOAT4* near_point, float* dist) {

	XMVECTOR vec1 = XMVectorSet(pos1.x, pos1.y, pos1.z, 1.0f);
	XMVECTOR vec2 = XMVectorSet(pos2.x, pos2.y, pos2.z, 1.0f);
	float D = sqrt(pow(pos2.x - pos1.x, 2.0f) + pow(pos2.y - pos1.y, 2.0f) + pow(pos2.z - pos1.z, 2.0f));

	XMFLOAT4 out_pos = XMFLOAT4(0, 0, 9999.0f, 0);
	*dist = 9999.0f;
	if (D < pos1.w + pos2.w) {
		XMVECTOR v = XMVector3Normalize(vec2 - vec1);
		float d = pos2.w / (pos1.w + pos2.w);
		out_pos.x = pos1.x + XMVectorGetX(v) * d;
		out_pos.y = pos1.y + XMVectorGetY(v) * d;
		out_pos.z = pos1.z + XMVectorGetZ(v) * d;
		*dist = sqrt(pow(out_pos.x - pos1.x, 2.0f) + pow(out_pos.y - pos1.y, 2.0f) + pow(out_pos.z - pos1.z, 2.0f));
		*near_point = out_pos;

		return true;
	}

	return false;
}

bool CalcDistPointLine(XMFLOAT4 pos, XMFLOAT4 p0, XMFLOAT4 p1, XMFLOAT4* near_point, float* dist) {

	XMVECTOR v_pos = XMVectorSet(pos.x, pos.y, pos.z, 1.0f);
	XMVECTOR v_p0 = XMVectorSet(p0.x, p0.y, p0.z, 1.0f);
	XMVECTOR v_p1 = XMVectorSet(p1.x, p1.y, p1.z, 1.0f);

	XMVECTOR vec1 = v_pos - v_p0;
	XMVECTOR vec2 = v_p1 - v_p0;
	XMVECTOR vec3 = v_pos - v_p1;
	XMVECTOR vec4 = v_p0 - v_p1;

	XMFLOAT4 out_pos = XMFLOAT4(0, 0, -9999.0f, 0);
	*dist = -9999.0f;
	XMVECTOR dot1 = XMVector3Dot(XMVector3Normalize(vec1), XMVector3Normalize(vec2));
	XMVECTOR dot2 = XMVector3Dot(XMVector3Normalize(vec3), XMVector3Normalize(vec4));
	XMVECTOR n_vec = XMVector3Normalize(vec2);
	XMVECTOR P = v_p0 + XMVector3Dot(vec1, n_vec) * n_vec;
	float D = sqrt(pow(XMVectorGetX(v_pos) - XMVectorGetX(P), 2.0f) + pow(XMVectorGetY(v_pos) - XMVectorGetY(P), 2.0f) + pow(XMVectorGetZ(v_pos) - XMVectorGetZ(P), 2.0f));

	out_pos.x = XMVectorGetX(P);
	out_pos.y = XMVectorGetY(P);
	out_pos.z = XMVectorGetZ(P);

	if (XMVectorGetX(dot1) < 0.0f) {
		float len = sqrt(pow(pos.x - p0.x, 2.0f) + pow(pos.y - p0.y, 2.0f) + pow(pos.z - p0.z, 2.0f));
		if (len < pos.w) {
			*near_point = out_pos;
			*dist = len;
			return true;
		}
		return false;
	}

	if (XMVectorGetX(dot2) < 0.0f) {
		float len = sqrt(pow(pos.x - p1.x, 2.0f) + pow(pos.y - p1.y, 2.0f) + pow(pos.z - p1.z, 2.0f));
		if (len < pos.w) {
			*near_point = out_pos;
			*dist = len;
			return true;
		}
		return false;
	}

	if (D < pos.w) {
		*near_point = out_pos;
		*dist = D;
		return true;
	}

	return false;
}

bool CalcDistLineLine(XMFLOAT4 p0, XMFLOAT4 p1, XMFLOAT4 pos0, XMFLOAT4 pos1, XMFLOAT4* near_point, XMFLOAT4* near_vector, float* dist) {

	XMVECTOR v_pos0 = XMVectorSet(pos0.x, pos0.y, pos0.z, 1.0f);
	XMVECTOR v_pos1 = XMVectorSet(pos1.x, pos1.y, pos1.z, 1.0f);
	XMVECTOR v_p0 = XMVectorSet(p0.x, p0.y, p0.z, 1.0f);
	XMVECTOR v_p1 = XMVectorSet(p1.x, p1.y, p1.z, 1.0f);

	XMVECTOR m1 = XMVector3Normalize(v_pos1 - v_pos0);
	XMVECTOR m2 = XMVector3Normalize(v_p1 - v_p0);
	XMVECTOR cross = XMVector3Cross(m1, m2);
	XMVECTOR dot = XMVector3Dot(m1, m2);

	float L = sqrt(pow(XMVectorGetX(cross), 2.0f) + pow(XMVectorGetY(cross), 2.0f) + pow(XMVectorGetZ(cross), 2.0f));
	float d = abs(XMVectorGetX((XMVector3Dot(v_pos0 - v_p0, cross)) / L));

	float k = XMVectorGetX(XMVector3Dot(m1 - dot * m2, v_p0 - v_pos0)) / (1.0f - pow(XMVectorGetX(dot), 2.0f));
	XMVECTOR P1 = v_pos0 + k * m1;

	XMFLOAT4 out_pos = XMFLOAT4(0, 0, 9999.0f, 0);
	*dist = 9999.0f;
	float len = sqrt(pow(XMVectorGetX(v_pos1 - v_pos0), 2.0f) + pow(XMVectorGetY(v_pos1 - v_pos0), 2.0f) + pow(XMVectorGetZ(v_pos1 - v_pos0), 2.0f));
	if (k > -pos1.w && k < len + pos1.w) {

		*dist = d;
		out_pos.x = XMVectorGetX(P1);
		out_pos.y = XMVectorGetY(P1);
		out_pos.z = XMVectorGetZ(P1);
		*near_point = out_pos;

		return true;
	}

	bool col;
	XMFLOAT4 P2, P3;
	float dist2, dist3;
	bool col2 = CalcDistPointLine(pos0, p0, p1, &P2, &dist2);
	bool col3 = CalcDistPointLine(pos1, p0, p1, &P3, &dist3);

	if (dist2 < dist3 && col2 == true) {
		*dist = dist2;
		*near_point = P2;
		return true;
	}
	else if (col3 == true) {
		*dist = dist3;
		*near_point = P3;
		return true;
	}

	return false;
}

bool CalcDistPointPolygon(XMFLOAT4 pos, XMFLOAT4 p1, XMFLOAT4 p2, XMFLOAT4 p3, XMFLOAT4 normal, XMFLOAT4* near_point, float* dist) {

	XMFLOAT4 near_pos1, near_pos2, near_pos3;
	float dist1, dist2, dist3;

	XMFLOAT4 center = CalcCenterPos(p1, p2, p3);

	XMVECTOR vec;
	XMVECTOR n_vec = XMVectorSet(normal.x, normal.y, normal.z, 1.0);
	vec = XMVectorSet(pos.x - center.x, pos.y - center.y, pos.z - center.z, 1.0f);
	float D = XMVectorGetX(XMVector3Dot(vec, n_vec)) / sqrt(pow(normal.x, 2.0f) + pow(normal.y, 2.0f) + pow(normal.z, 2.0f));

	XMFLOAT4 out_pos = XMFLOAT4(0, 0, -9999.0f, 0);
	*dist = -9999.0f;
	XMVECTOR cross_pos = XMVectorSet(pos.x, pos.y, pos.z, 1.0f) + XMVector3Normalize(n_vec) * abs(D);
	out_pos.x = XMVectorGetX(cross_pos);
	out_pos.y = XMVectorGetY(cross_pos);
	out_pos.z = XMVectorGetZ(cross_pos);

	XMVECTOR v1 = XMVectorSet(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z, 1.0f);
	XMVECTOR v2 = XMVectorSet(out_pos.x - p2.x, out_pos.y - p2.y, out_pos.z - p2.z, 1.0f);
	XMVECTOR vn = XMVector3Cross(v1, v2);
	float D1 = XMVectorGetX(XMVector3Dot(vn, n_vec));

	v1 = XMVectorSet(p3.x - p2.x, p3.y - p2.y, p3.z - p2.z, 1.0f);
	v2 = XMVectorSet(out_pos.x - p3.x, out_pos.y - p3.y, out_pos.z - p3.z, 1.0f);
	vn = XMVector3Cross(v1, v2);
	float D2 = XMVectorGetX(XMVector3Dot(vn, n_vec));

	v1 = XMVectorSet(p1.x - p3.x, p1.y - p3.y, p1.z - p3.z, 1.0f);
	v2 = XMVectorSet(out_pos.x - p1.x, out_pos.y - p1.y, out_pos.z - p1.z, 1.0f);
	vn = XMVector3Cross(v1, v2);
	float D3 = XMVectorGetX(XMVector3Dot(vn, n_vec));

	*near_point = out_pos;
	*dist = abs(D);

	if (D1 * D2 > 0.0f && D2 * D3 > 0.0f && D3 * D1 > 0.0f && abs(D) < pos.w) {
		return true;
	}

	return false;
}

bool CalcDistLinePolygon(XMFLOAT4 pos1, XMFLOAT4 pos2, XMFLOAT4 p1, XMFLOAT4 p2, XMFLOAT4 p3, XMFLOAT4 normal, XMFLOAT4* near_point, float* dist) {

	XMVECTOR vec1 = XMVectorSet(pos1.x - p1.x, pos1.y - p1.y, pos1.z - p1.z, 1.0f);
	XMVECTOR vec2 = XMVectorSet(pos2.x - p1.x, pos2.y - p1.y, pos2.z - p1.z, 1.0f);
	XMVECTOR n_vec = XMVector3Normalize(XMVectorSet(normal.x, normal.y, normal.z, 1.0f));
	XMVECTOR dot1 = XMVector3Dot(vec1, n_vec);
	XMVECTOR dot2 = XMVector3Dot(vec2, n_vec);

	XMFLOAT4 out_pos = XMFLOAT4(0, 0, -9999.0f, 0);
	*dist = -9999.0f;

	XMFLOAT4 Pos1, Pos2;
	float d1, d2;
	CalcDistPointPolygon(XMFLOAT4(pos1.x, pos1.y, pos1.z, 0.0f), p1, p2, p3, normal, &Pos1, &d1);
	CalcDistPointPolygon(XMFLOAT4(pos2.x, pos2.y, pos2.z, 0.0f), p1, p2, p3, normal, &Pos2, &d2);
	float d = d1 / (d1 + d2);

	if (XMVectorGetX(dot1) * XMVectorGetX(dot2) > 0.0f && min(d1, d2) > pos1.w) {
		return false;
	}

	XMVECTOR vec_line = XMVectorSet(pos2.x, pos2.y, pos2.z, 1.0f) - XMVectorSet(pos1.x, pos1.y, pos1.z, 1.0f);
	XMFLOAT4 cross_pos = XMFLOAT4(0, 0, 0, 0);
	cross_pos.x = pos1.x + d * XMVectorGetX(vec_line);
	cross_pos.y = pos1.y + d * XMVectorGetY(vec_line);
	cross_pos.z = pos1.z + d * XMVectorGetZ(vec_line);

	XMVECTOR v1 = XMVectorSet(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z, 1.0f);
	XMVECTOR v2 = XMVectorSet(cross_pos.x - p2.x, cross_pos.y - p2.y, cross_pos.z - p2.z, 1.0f);
	XMVECTOR vn = XMVector3Cross(v1, v2);
	float D1 = XMVectorGetX(XMVector3Dot(vn, n_vec));

	v1 = XMVectorSet(p3.x - p2.x, p3.y - p2.y, p3.z - p2.z, 1.0f);
	v2 = XMVectorSet(cross_pos.x - p3.x, cross_pos.y - p3.y, cross_pos.z - p3.z, 1.0f);
	vn = XMVector3Cross(v1, v2);
	float D2 = XMVectorGetX(XMVector3Dot(vn, n_vec));

	v1 = XMVectorSet(p1.x - p3.x, p1.y - p3.y, p1.z - p3.z, 1.0f);
	v2 = XMVectorSet(cross_pos.x - p1.x, cross_pos.y - p1.y, cross_pos.z - p1.z, 1.0f);
	vn = XMVector3Cross(v1, v2);
	float D3 = XMVectorGetX(XMVector3Dot(vn, n_vec));

	if (D1 * D2 > 0.0f && D2 * D3 > 0.0f && D3 * D1 > 0.0f) {
		*near_point = cross_pos;
		if (XMVectorGetX(dot1) > 0.0f)
			*dist = d1;
		else
			*dist = d2;
		return true;
	}

	return false;
}

bool InsArea(CChaColliderObject* obj1, CChaColliderObject* obj2) {

	float len = sqrt(pow(obj1->CenterPos.x - obj2->CenterPos.x, 2.0f) + pow(obj1->CenterPos.y - obj2->CenterPos.y, 2.0f) + pow(obj1->CenterPos.z - obj2->CenterPos.z, 2.0f));
	if (len < obj1->Length + obj2->Length)
		return true;

	return false;
}

bool InsArea(CChaColliderObject* obj, CColliderVertex vtx1, CColliderVertex vtx2, CColliderVertex vtx3) {

	if (obj->MinPos.x > vtx1.Pos.x && obj->MinPos.x > vtx2.Pos.x && obj->MinPos.x > vtx3.Pos.x)
		return false;
	if (obj->MinPos.y > vtx1.Pos.y && obj->MinPos.y > vtx2.Pos.y && obj->MinPos.y > vtx3.Pos.y)
		return false;
	if (obj->MinPos.z > vtx1.Pos.z && obj->MinPos.z > vtx2.Pos.z && obj->MinPos.z > vtx3.Pos.z)
		return false;
	if (obj->MaxPos.x < vtx1.Pos.x && obj->MaxPos.x < vtx2.Pos.x && obj->MaxPos.x < vtx3.Pos.x)
		return false;
	if (obj->MaxPos.y < vtx1.Pos.y && obj->MaxPos.y < vtx2.Pos.y && obj->MaxPos.y < vtx3.Pos.y)
		return false;
	if (obj->MaxPos.z < vtx1.Pos.z && obj->MaxPos.z < vtx2.Pos.z && obj->MaxPos.z < vtx3.Pos.z)
		return false;

	return true;
}
