#include "stdafx.h"
#include "K3DBound.h"
#include "KPrimitiveMesh.h"
#include "KViewport.h"
#include "k3dpccp.h"

namespace
{
	KColor c_WireColor[3] = 
	{ 
		KColor( 255,   0,   0, 255 ), //Red    X
		KColor(   0, 255,   0, 255 ), //Green  Y
		KColor(   0,   0, 255, 255 ), //Blue   Z
	};

	const int c_WireColorIdx[12] = { 0, 1, 0, 1, 2, 0, 2, 1, 2, 0, 2, 1 };//{ X, Y, X, Y, Z, X, Z, Y, Z, Y, Z, X }
};

bool K3DBoundSetCube( K3DBoundRotCube *cube, K3DVector *vertices, int nCount )
{
	static int cubevtxtbl[8] = { 
		0, 1, 3, 2,
			5, 4, 6, 7,
	};

	if( nCount >= 8 )
	{
		K3DVertex vtx[8];

		for( int i = 0; i < 8; i++ )
		{
			vtx[i] = vertices[cubevtxtbl[i]];
		}
		cube->SetCube( vtx );
		return true;
	}
	return false;
}
////////////////////////////////////////////////////////
// - RotCube - /////////////////////////////////////////
////////////////////////////////////////////////////////

K3DBoundRotCube::~K3DBoundRotCube()
{
	if( m_prPlan )
		delete m_prPlan;

	if ( m_pr )
		delete m_pr;
}

const K3DBoundRotCube& K3DBoundRotCube::operator=( const K3DBoundRotCube& r )
{
//	SAFE_DELETE( m_pr );
	ClearCube();
	SetTransform( r.m_matrix );
	SetWireColor( r.m_WireColor );
	SetWireDrawMode( r.m_bWireComplex );

	memcpy( m_vtx, r.m_vtx, sizeof(m_vtx) );
	updateTransform();
	return (*this);
}

void K3DBoundRotCube::AddCube( const K3DBoundRotCube &cube )
{
	K3DVertex trvtx1[8], trvtx2[8];
	memcpy(trvtx1, GetVertices(), sizeof(trvtx1));
	memcpy(trvtx2, cube.GetVertices(), sizeof(trvtx2));

	float minx, miny, minz;
	float maxx, maxy, maxz;
	minx = trvtx1[0].x;
	miny = trvtx1[0].y;
	minz = trvtx1[0].z;
	maxx = trvtx1[0].x;
	maxy = trvtx1[0].y;
	maxz = trvtx1[0].z;

	int i;
	for( i = 0; i < 8; i++ )
	{
		if( trvtx1[i].x < minx  )	minx = trvtx1[i].x;
		if( trvtx1[i].y < miny  )	miny = trvtx1[i].y;
		if( trvtx1[i].z < minz  )	minz = trvtx1[i].z;

		if( trvtx1[i].x > maxx  )	maxx = trvtx1[i].x;
		if( trvtx1[i].y > maxy  )	maxy = trvtx1[i].y;
		if( trvtx1[i].z > maxz  )	maxz = trvtx1[i].z;

		if( trvtx2[i].x < minx  )	minx = trvtx2[i].x;
		if( trvtx2[i].y < miny  )	miny = trvtx2[i].y;
		if( trvtx2[i].z < minz  )	minz = trvtx2[i].z;

		if( trvtx2[i].x > maxx  )	maxx = trvtx2[i].x;
		if( trvtx2[i].y > maxy  )	maxy = trvtx2[i].y;
		if( trvtx2[i].z > maxz  )	maxz = trvtx2[i].z;
	}

	for( i = 0; i < 8; i++ )
	{
		m_transvtx[i] = m_vtx[i] =
			KPCCP::get_cube_vertex( i, minx, maxx,
			miny, maxy,
			minz, maxz );
	}

	K3DMatrixIdentity(m_matrix);
	m_transformchanged = false;
}

void K3DBoundRotCube::Render( KViewportObject *viewport )
{
	updateTransform();


	if ( m_pr == NULL )
		m_pr = new KWireUtilPrimitive;

	if( m_pr )
	{
		m_pr->Clear();

		static int LineIndices1[] = { 0,1, 1,2, 2,3, 3,0, 0,5, 5,4, 4,1, 4,7, 7,2, 5,6, 6,3, 6,7 };
		static int LineIndices2[] = { 0,4, 1,5, 2,4, 1,7, 2,6, 3,7, 0,6, 3,5, 2,0, 3,1, 7,5, 6,4 };

		AddLines( LineIndices1, _countof( LineIndices1 ) );

		if( m_bWireComplex )
			AddLines( LineIndices2, _countof( LineIndices2 ) );

		viewport->RegisterWire( m_pr );
	}
}

/*void K3DBoundRotCube::updatePrimitive()
{
	if ( m_renderPr )
	{
		delete m_renderPr;
	}
	m_renderPr = new KMeshPrimitive;

	m_renderPr->
}*/

void K3DBoundRotCube::AddLines( const int* pLineIndices, int nCount )
{
 	int i(0);
	KColor diff(0,0,0,255);
	for ( int n = 0; n < nCount; n += 2 )
	{
		diff = m_WireColor;
		if( m_bColorGradation )
		{
			diff.r = (unsigned char)(diff.r*(1.f-((float)n/nCount))*100);
			diff.g = (unsigned char)(diff.g*(1.f-((float)n/nCount))*100);
			diff.b = (unsigned char)(diff.b*(1.f-((float)n/nCount))*100);
		}
		m_pr->AddLine( m_transvtx[ pLineIndices[n] ], m_transvtx[ pLineIndices[n + 1] ], diff/*m_WireColor*//*c_WireColorIdx[i++]*/ );
	}
}

void K3DBoundRotCube::CreatePlanPrimitive()
{
	if( m_prPlan )
	{
		delete m_prPlan;
		m_prPlan = NULL;
	}
	m_prPlan = new KPlanUtilPrimitive;
	m_prPlan->SetTransform( m_matrix );

	KColor diff(rand()%255,rand()%255,rand()%255,255);

	m_prPlan->AddVertex( m_vtx[3], m_vtx[2], m_vtx[0], diff );
	m_prPlan->AddVertex( m_vtx[0], m_vtx[2], m_vtx[1], diff );
	m_prPlan->AddVertex( m_vtx[6], m_vtx[7], m_vtx[3], diff );
	m_prPlan->AddVertex( m_vtx[3], m_vtx[7], m_vtx[2], diff );
	m_prPlan->AddVertex( m_vtx[5], m_vtx[6], m_vtx[0], diff );
	m_prPlan->AddVertex( m_vtx[0], m_vtx[6], m_vtx[3], diff );
	m_prPlan->AddVertex( m_vtx[7], m_vtx[4], m_vtx[2], diff );
	m_prPlan->AddVertex( m_vtx[2], m_vtx[4], m_vtx[1], diff );
	m_prPlan->AddVertex( m_vtx[5], m_vtx[4], m_vtx[6], diff );
	m_prPlan->AddVertex( m_vtx[6], m_vtx[4], m_vtx[7], diff );
	m_prPlan->AddVertex( m_vtx[0], m_vtx[1], m_vtx[5], diff );
	m_prPlan->AddVertex( m_vtx[5], m_vtx[1], m_vtx[4], diff );
}

////////////////////////////////////////////////////////
// - SPHERE - //////////////////////////////////////////
////////////////////////////////////////////////////////
void K3DBoundSphere::AddSphere( const K3DBoundSphere &sphere )
{
	K3DVector v;
	K3DVertex spos = sphere.GetPosition();
	K3DVertex tpos = GetPosition();
	v = K3DVector( spos.x - tpos.x, spos.y - tpos.y, spos.z - tpos.z );

	float o_slen = sphere.GetRadius();
	float o_tlen = GetRadius();
	//float slen = o_slen + o_tlen;
	//float vlen = Length(v);

	if(K3DVectorLength(v) <= 0.000001f)  {
		m_position = spos;
		m_radius = max(o_slen, o_tlen);
	}
	else  {
		Normalize(v);
		K3DVertex d1 = spos + o_slen*v, d2 = tpos - o_tlen*v;
		K3DVector d(d1 - d2);
		float newdiameter = K3DVectorLength(d);

		if(newdiameter <= 2*o_slen)  {
			// sphere에 this가 포함
			m_position = spos;
			m_radius = o_slen;
		}
		else if(newdiameter <= 2*o_tlen)  {
			// this에 sphere가 포함
			m_position = tpos;
			m_radius = o_tlen;
		}
		else  {
			// 완전포함이 아님
			m_position = .5f*d1 + .5f*d2;
			m_radius = newdiameter/2;
		}
	}

	K3DMatrixIdentity(m_matrix);
	m_needTransform = true;
	return;
}

bool K3DBoundSphere::CheckCollision(const K3DVertex &v1, const K3DVertex &v2, const K3DVertex &v3) const
{ 

	// collision check of sphere with triangle in 3D space
	// * = 54 times
	// / = 8 times
	// pow = 25 times
	// so, total mul and div is 58 + 14 + 25 = 87 times
	updateTransform();

	// step 1: distance check
	const float a = (v2.x - v1.x), b = (v3.x - v1.x);
	const float c = (v2.y - v1.y), d = (v3.y - v1.y);
	const float e = (v2.z - v1.z), f = (v3.z - v1.z);

	const float ac = a*c;
	const float cc = c*c;
	const float ce = c*e;

	const float k = ce*d - cc*f;
	const float l = ac*f - b*ce;
	const float m = b*cc - ac*d;
	const float C = k*v1.x + l*v1.y + m*v1.z;

	const float DA = k*m_transpos.x + l*m_transpos.y + m*m_transpos.z + C;
	const float DP = powf(k,2) + powf(l,2) + powf(m,2);

	const float CC = DA/DP;
	const float powerdistance = DA*CC; // the distance is equal fabs(DA)/sqrt(DP), so powerdistancd is equal distance^2

	if (powerdistance > powf(m_transrad,2)) return false;

	// step 2: Is v1 or v2 or v3 exist inside sphere?
	if (powf(v1.x - m_transpos.x,2) + powf(v1.y - m_transpos.y,2) + powf(v1.z - m_transpos.z,2) < powf(m_transrad,2) ||
		powf(v2.x - m_transpos.x,2) + powf(v2.y - m_transpos.y,2) + powf(v2.z - m_transpos.z,2) < powf(m_transrad,2) ||
		powf(v3.x - m_transpos.x,2) + powf(v3.y - m_transpos.y,2) + powf(v3.z - m_transpos.z,2) < powf(m_transrad,2))
	{
		return true;
	}

	// step 3: Is m_poisition exist inside triangle?
	K3DVertex PP; 
	PP.x = m_transpos.x - CC*k;
	PP.y = m_transpos.y - CC*l;
	PP.z = m_transpos.z - CC*m;

	K3DVector BA(v1 - v2);
	K3DVector BC(v3 - v2);
	K3DVector BP(PP - v2);
	K3DVector N(BA.y, -BA.x, 0);

	if (BC.x*N.x + BC.y*N.y == 0)
	{
		N = K3DVector(0, -BA.z, BA.y);
	}

	float T = (BP.x*N.x + BP.y*N.y + BP.z*N.z) / (BC.x*N.x + BC.y*N.y +BC.z*N.z); // because N is not perpendicularity with BC, BC*N is not zero.
	float S = 1000000000L;

	// AB Vector can not be zero, if it can be, v1, v2, v3 can not make triangle
	if (BA.x)	   S = (BP.x - T*BC.x)/(-BA.x);
	else if (BA.y) S = (BP.y - T*BC.y)/(-BA.y);
	else if (BA.z) S = (BP.z - T*BC.z)/(-BA.z);

	if (0 <= T + S && T + S <= 1) return true;

	// step 4: check line cross
#define CHECKLINECROSS(va,vb)																			\
	x1 = va.x; y1 = va.y; z1 = va.z;																	\
	x2 = vb.x; y2 = vb.y; z2 = vb.z;																	\
	\
	p = (x1 - m_transpos.x)*(x1 - x2) + (y1 - m_transpos.y)*(y1 - y2) + (z1 - m_transpos.z)*(z1 - z2);	\
	q = powf(x1 - x2, 2) + powf(y1 - y2,2) + powf(z1 - z2,2);												\
	\
	pdivq = p/q;																						\
	x = x1 - pdivq*(x1-x2);																				\
	y = y1 - pdivq*(y1-y2);																				\
	z = z1 - pdivq*(z1-z2);																				\
	\
	if (x1 > x2) { t = x1; x1 = x2; x2 = t; }															\
	if (y1 > y2) { t = y1; y1 = y2; y2 = t; }															\
	if (z1 > z2) { t = z1; z1 = z2; z2 = t; }															\
	if ((x1 < x && x < x2) && (y1 < y && y < y2) && (z1 < z && z < z2)) return true;					\

	float x,y,z;
	float x1,y1,z1;
	float x2,y2,z2;
	float p,q,t;
	register float pdivq;

	CHECKLINECROSS(v1,v2);
	CHECKLINECROSS(v2,v3);
	CHECKLINECROSS(v1,v3);

	return false;
}

bool K3DBoundSphere::CheckCollision(const K3DVertex &v1, const K3DVertex &v2) const
{
	float x,y,z;
	float x1,y1,z1;
	float x2,y2,z2;
	float p,q,t;
	register float pdivq;

	updateTransform();

	// v1 or v2 가 sphere 안에 존재할때
	if((m_transpos - v1).SquareMagnitude() < (m_transrad*m_transrad) || (m_transpos - v2).SquareMagnitude() < (m_transrad*m_transrad))
		return true;

	K3DVector va(v1 - v2); Normalize(va); // Normalize 는 꼭 필요! 
	K3DVector vb(v1 - m_transpos);
	K3DVector result;
	K3DVectorCross(result, va, vb);

	// Result 크기 값은 결국에 구의 중심에서 v1_v2 선분 까지 그은 가장 짧은 직선 t의 길이가 된다.
	// va 가 1 이라는걸 기억하자.
	// | va x vb | = |va||vb| sin(@) = |vb| * (t / |vb|) = t
	// 그러므로 이 외적의 크기가 반지름보다 크면 당연히 선분은 구 밖에 있다.
	float len = K3DVectorLength(result);
	if (len > m_transrad) return false;

	x1 = v1.x; y1 = v1.y; z1 = v1.z;
	x2 = v2.x; y2 = v2.y; z2 = v2.z;

	p = (x1 - m_transpos.x)*(x1 - x2) + (y1 - m_transpos.y)*(y1 - y2) + (z1 - m_transpos.z)*(z1 - z2);
	q = powf(x1 - x2, 2) + powf(y1 - y2,2) + powf(z1 - z2,2);

	// pdivq = (Va dot Vb) / | Va |^2
	pdivq = p/q;

	// pdivq * (V1 - V2) 는 vb를 va에 Projection한 값이다.
	// Proj Vb to Va =   (Va dot Vb) va           Vb /| 
	//                   ----------------           / |
	//                     |Va||Va|                /  |
	//                                            /--->----->  Va
	//                                        (Proj Vb to va)
	x = x1 - pdivq*(x1-x2);                      
	y = y1 - pdivq*(y1-y2);
	z = z1 - pdivq*(z1-z2);



	if (x1 > x2) { t = x1; x1 = x2; x2 = t; }
	if (y1 > y2) { t = y1; y1 = y2; y2 = t; }
	if (z1 > z2) { t = z1; z1 = z2; z2 = t; }
	if ((x1 <= x && x <= x2) && (y1 <= y && y <= y2) && (z1 <= z && z <= z2)) return true;

	return false;
}