#include "stdafx.h"

bool gRenderDebugHelpers = false;

MyConvex::MyConvex() :
	mNbVerts	(0),
	mVerts		(NULL),
	mNbPolys	(0),
	mPolys		(NULL)
{
	mTransform.setIdentity();
	mLocalCenter.setValue(0, 0, 0);
#ifdef TEST_INTERNAL_OBJECTS
	mRadius	= 0.0f;
	mExtents[0] = mExtents[1] = mExtents[2] = 0.0f;
#endif
}

MyConvex::~MyConvex()
{
	delete[]mPolys;
	delete[]mVerts;
}

bool MyConvex::LoadFromFile(const char* filename)
{
	FILE* fp = fopen(filename, "rb");
	if(!fp)	return false;

	fread(&mNbVerts, sizeof(int), 1, fp);

	mVerts = new btVector3[mNbVerts];
	float Scale = 0.0f;
	for(int i=0;i<mNbVerts;i++)
	{
		float vals[3];
		fread(vals, sizeof(float)*3, 1, fp);
		mVerts[i].setX(vals[0]);
		mVerts[i].setY(vals[1]);
		mVerts[i].setZ(vals[2]);
		if(fabsf(vals[0])>Scale)	Scale = fabsf(vals[0]);
		if(fabsf(vals[1])>Scale)	Scale = fabsf(vals[1]);
		if(fabsf(vals[2])>Scale)	Scale = fabsf(vals[2]);
	}
Scale = 1.0f;
	Scale = 1.0f/Scale;
	for(int i=0;i<mNbVerts;i++)
		mVerts[i] *= Scale;

	fread(&mNbPolys, sizeof(int), 1, fp);
	mPolys = new MyPoly[mNbPolys];

	for(int i=0;i<mNbPolys;i++)
	{
		fread(&mPolys[i].mNbVerts, sizeof(short), 1, fp);
		mPolys[i].mIndices = new char[mPolys[i].mNbVerts];
		fread(mPolys[i].mIndices, mPolys[i].mNbVerts, 1, fp);
		fread(mPolys[i].mPlane, sizeof(float)*4, 1, fp);
		mPolys[i].mPlane[3] *= Scale;
	}
	fclose(fp);

	float TotalArea = 0.0f;
	mLocalCenter.setValue(0, 0, 0);
	for(int i=0;i<mNbPolys;i++)
	{
		int NbTris = mPolys[i].mNbVerts-2;
		const btVector3& p0 = mVerts[mPolys[i].mIndices[0]];
		for(int j=1;j<=NbTris;j++)
		{
			int k = (j+1)%mPolys[i].mNbVerts;
			const btVector3& p1 = mVerts[mPolys[i].mIndices[j]];
			const btVector3& p2 = mVerts[mPolys[i].mIndices[k]];
			float Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
			btVector3 Center = (p0+p1+p2)/3.0f;
			mLocalCenter += Area * Center;
			TotalArea += Area;
		}
	}
	mLocalCenter /= TotalArea;

#ifdef TEST_INTERNAL_OBJECTS
	if(1)
	{
		mRadius = FLT_MAX;
		for(int i=0;i<mNbPolys;i++)
		{
			const btVector3 Normal(mPolys[i].mPlane[0], mPolys[i].mPlane[1], mPolys[i].mPlane[2]);
			const float dist = fabsf(mLocalCenter.dot(Normal) + mPolys[i].mPlane[3]);
			if(dist<mRadius)
				mRadius = dist;
		}

		ComputeAABBNoTransform();
//		const float r = mRadius / sqrtf(2.0f);
		const float r = mRadius / sqrtf(3.0f);
		const int LargestExtent = mE.maxAxis();
		const float Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
		mExtents[0] = mExtents[1] = mExtents[2] = r;
		mExtents[LargestExtent] = mE[LargestExtent]*0.5f;
		bool FoundBox = false;
		for(int j=0;j<1024;j++)
		{
			if(TestContainment())
			{
				FoundBox = true;
				break;
			}

			mExtents[LargestExtent] -= Step;
		}
		if(!FoundBox)
		{
			mExtents[0] = mExtents[1] = mExtents[2] = r;
		}
		else
		{
			// Refine the box
			const float Step = (mRadius - r)/1024.0f;
			const int e0 = (1<<LargestExtent) & 3;
			const int e1 = (1<<e0) & 3;

			for(int j=0;j<1024;j++)
			{
				const float Saved0 = mExtents[e0];
				const float Saved1 = mExtents[e1];
				mExtents[e0] += Step;
				mExtents[e1] += Step;

				if(!TestContainment())
				{
					mExtents[e0] = Saved0;
					mExtents[e1] = Saved1;
					break;
				}
			}
		}
	}
#endif
	return true;
}

#ifdef TEST_INTERNAL_OBJECTS
bool MyConvex::TestContainment() const
{
	for(int p=0;p<8;p++)
	{
		btVector3 LocalPt;
		if(p==0)		LocalPt = mLocalCenter + btVector3(mExtents[0], mExtents[1], mExtents[2]);
		else if(p==1)	LocalPt = mLocalCenter + btVector3(mExtents[0], mExtents[1], -mExtents[2]);
		else if(p==2)	LocalPt = mLocalCenter + btVector3(mExtents[0], -mExtents[1], mExtents[2]);
		else if(p==3)	LocalPt = mLocalCenter + btVector3(mExtents[0], -mExtents[1], -mExtents[2]);
		else if(p==4)	LocalPt = mLocalCenter + btVector3(-mExtents[0], mExtents[1], mExtents[2]);
		else if(p==5)	LocalPt = mLocalCenter + btVector3(-mExtents[0], mExtents[1], -mExtents[2]);
		else if(p==6)	LocalPt = mLocalCenter + btVector3(-mExtents[0], -mExtents[1], mExtents[2]);
		else if(p==7)	LocalPt = mLocalCenter + btVector3(-mExtents[0], -mExtents[1], -mExtents[2]);

		for(int i=0;i<mNbPolys;i++)
		{
			const btVector3 Normal(mPolys[i].mPlane[0], mPolys[i].mPlane[1], mPolys[i].mPlane[2]);
			const float d = LocalPt.dot(Normal) + mPolys[i].mPlane[3];
			if(d>0.0f)
				return false;
		}
	}
	return true;
}
#endif

void MyConvex::Render(bool only_wireframe, const btVector3& wire_color) const
{
	const float Scale = 1.0f;
	glPushMatrix();

	float glmat[16];	//4x4 column major matrix for OpenGL.
	mTransform.getOpenGLMatrix(glmat);
	glMultMatrixf(&(glmat[0]));

	if(!only_wireframe)
	{
		btVector3 color(0.0f, 0.5f, 1.0f);
		for(int i=0;i<mNbPolys;i++)
		{
			glNormal3f(mPolys[i].mPlane[0], mPolys[i].mPlane[1], mPolys[i].mPlane[2]);

			int NbTris = mPolys[i].mNbVerts-2;
			const btVector3& p0 = mVerts[mPolys[i].mIndices[0]]*Scale;
			for(int j=1;j<=NbTris;j++)
			{
				int k = (j+1)%mPolys[i].mNbVerts;

				const btVector3& p1 = mVerts[mPolys[i].mIndices[j]]*Scale;
				const btVector3& p2 = mVerts[mPolys[i].mIndices[k]]*Scale;

				DrawTriangle(p0, p1, p2, color);
			}
		}
	}

	{
		btVector3 color;
		if(only_wireframe)
			color = wire_color;
		else
			color = btVector3(0.0f, 0.0f, 0.0f);

		for(int i=0;i<mNbPolys;i++)
		{
			for(int j=0;j<mPolys[i].mNbVerts;j++)
			{
				int k = (j+1)%mPolys[i].mNbVerts;
				DrawLine(mVerts[mPolys[i].mIndices[j]]*Scale, mVerts[mPolys[i].mIndices[k]]*Scale, color, 1.0f);
			}
		}
	}

	if(gRenderDebugHelpers)
	{
		DrawSphere(mRadius);

		DrawLine(mLocalCenter, mLocalCenter+btVector3(mExtents[0], 0.0f, 0.0f), btVector3(1.0f, 0.0f, 0.0f), 1.0f);
		DrawLine(mLocalCenter, mLocalCenter+btVector3(0.0f, mExtents[1], 0.0f), btVector3(0.0f, 1.0f, 0.0f), 1.0f);
		DrawLine(mLocalCenter, mLocalCenter+btVector3(0.0f, 0.0f, mExtents[2]), btVector3(0.0f, 0.0f, 1.0f), 1.0f);

		DrawPoint(mLocalCenter + btVector3(mExtents[0], mExtents[1], mExtents[2]));
		DrawPoint(mLocalCenter + btVector3(mExtents[0], mExtents[1], -mExtents[2]));
		DrawPoint(mLocalCenter + btVector3(mExtents[0], -mExtents[1], mExtents[2]));
		DrawPoint(mLocalCenter + btVector3(mExtents[0], -mExtents[1], -mExtents[2]));
		DrawPoint(mLocalCenter + btVector3(-mExtents[0], mExtents[1], mExtents[2]));
		DrawPoint(mLocalCenter + btVector3(-mExtents[0], mExtents[1], -mExtents[2]));
		DrawPoint(mLocalCenter + btVector3(-mExtents[0], -mExtents[1], mExtents[2]));
		DrawPoint(mLocalCenter + btVector3(-mExtents[0], -mExtents[1], -mExtents[2]));
	}

	glPopMatrix();
}

void MyConvex::Project(const btVector3& dir, float& min, float& max) const
{
	min = FLT_MAX;
	max = -FLT_MAX;

	btVector3 invDir;
	InverseTransformPoint3x3(invDir, dir, mTransform);

	for(int i=0;i<mNbVerts;i++)
	{
		const float dp = mVerts[i].dot(invDir);
		if(dp < min)	min = dp;
		if(dp > max)	max = dp;
	}
	const float offset = mTransform.getOrigin().dot(dir);
	min += offset;
	max += offset;
}


void MyConvex::ComputeWorldAABB(btVector3& min, btVector3& max) const
{
	float MinX = FLT_MAX;
	float MinY = FLT_MAX;
	float MinZ = FLT_MAX;
	float MaxX = -FLT_MAX;
	float MaxY = -FLT_MAX;
	float MaxZ = -FLT_MAX;
	for(int i=0; i<mNbVerts; i++)
	{
		const btVector3 pt = mTransform * mVerts[i];
		if(pt.x()<MinX)	MinX = pt.x();
		if(pt.x()>MaxX)	MaxX = pt.x();
		if(pt.y()<MinY)	MinY = pt.y();
		if(pt.y()>MaxY)	MaxY = pt.y();
		if(pt.z()<MinZ)	MinZ = pt.z();
		if(pt.z()>MaxZ)	MaxZ = pt.z();
	}
	min.setValue(MinX, MinY, MinZ);
	max.setValue(MaxX, MaxY, MaxZ);
}

void MyConvex::ComputeAABBNoTransform() const
{
	float MinX = FLT_MAX;
	float MinY = FLT_MAX;
	float MinZ = FLT_MAX;
	float MaxX = -FLT_MAX;
	float MaxY = -FLT_MAX;
	float MaxZ = -FLT_MAX;
	for(int i=0; i<mNbVerts; i++)
	{
		const btVector3& pt = mVerts[i];
		if(pt.x()<MinX)	MinX = pt.x();
		if(pt.x()>MaxX)	MaxX = pt.x();
		if(pt.y()<MinY)	MinY = pt.y();
		if(pt.y()>MaxY)	MaxY = pt.y();
		if(pt.z()<MinZ)	MinZ = pt.z();
		if(pt.z()>MaxZ)	MaxZ = pt.z();
	}
	mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
	mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);
}
