#include "bound.h"
#include "../utilities/util.h"

BoundingMesh::BoundingMesh(const AABB& aabb)
{
	m_TriangleCount = 6 * 2;
	m_Points = new DXVector3[m_TriangleCount * 3];

	//DXVector3 maxWorld = obb.GetMax();
	//maxWorld.TransformCoord(obb.GetOrientation());

	//DXVector3 minWorld = obb.GetMin();
	//minWorld.TransformCoord(obb.GetOrientation());

	//init points array
	DXVector3* vb = m_Points;

	//bottom face
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMin().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMin().z);

	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMax().y, aabb.GetMin().z);

	//top face
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMin().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMax().z);

	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMax().y, aabb.GetMax().z);

	//min x-side face
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMin().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMin().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMin().z);

	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMin().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMax().z);

	//max x-side face
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMax().y, aabb.GetMin().z);

	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMax().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMax().y, aabb.GetMax().z);

	//min y-side face
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMin().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMin().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMin().z);

	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMin().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMax().z);

	//max y-side face
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMax().y, aabb.GetMin().z);

	*vb++ = DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMax().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMax().y, aabb.GetMin().z);
	*vb++ = DXVector3(aabb.GetMax().x, aabb.GetMax().y, aabb.GetMax().z);
}

BoundingMesh::BoundingMesh(const OBB& obb)
{
	m_TriangleCount = 6 * 2;
	m_Points = new DXVector3[m_TriangleCount * 3];

	//DXVector3 maxWorld = obb.GetMax();
	//maxWorld.TransformCoord(obb.GetOrientation());

	//DXVector3 minWorld = obb.GetMin();
	//minWorld.TransformCoord(obb.GetOrientation());

	//init points array
	DXVector3* vb = m_Points;

	//bottom face
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMin().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMin().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMax().y, obb.GetMin().z);

	*vb++ = DXVector3(obb.GetMax().x, obb.GetMin().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMax().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMax().y, obb.GetMin().z);

	//top face
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMin().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMin().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMax().y, obb.GetMax().z);

	*vb++ = DXVector3(obb.GetMax().x, obb.GetMin().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMax().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMax().y, obb.GetMax().z);

	//min x-side face
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMin().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMin().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMax().y, obb.GetMin().z);

	*vb++ = DXVector3(obb.GetMin().x, obb.GetMin().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMax().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMax().y, obb.GetMax().z);

	//max x-side face
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMin().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMin().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMax().y, obb.GetMin().z);

	*vb++ = DXVector3(obb.GetMax().x, obb.GetMin().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMax().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMax().y, obb.GetMax().z);

	//min y-side face
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMin().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMin().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMin().y, obb.GetMin().z);

	*vb++ = DXVector3(obb.GetMin().x, obb.GetMin().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMin().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMin().y, obb.GetMax().z);

	//max y-side face
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMax().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMin().x, obb.GetMax().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMax().y, obb.GetMin().z);

	*vb++ = DXVector3(obb.GetMin().x, obb.GetMax().y, obb.GetMax().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMax().y, obb.GetMin().z);
	*vb++ = DXVector3(obb.GetMax().x, obb.GetMax().y, obb.GetMax().z);

	TransformPoints(obb.GetOrientation());
}

void ViewFrustum::Update(const Camera &cam)
{
	Orig = cam.GetPosition();
	Dir = cam.GetViewDirVector();
	Up = cam.GetViewUpVector();
	Right = cam.GetViewRightVector();
	Far = cam.GetFarPlane();
	Near = cam.GetNearPlane();
	UBound = cam.GetProjMatrix()._11;
	RBound = cam.GetProjMatrix()._22;

	DRatio = Far / Near;
	MTwoUF = -2.f * UBound * Far;
	MTwoRF = -2.f * RBound * Far;

	static const DXVector3 Box[9] = 
		{DXVector3(-1,-1,0),DXVector3(1,-1,0),DXVector3(1,1,0),DXVector3(-1,1,0),
		DXVector3(-1,-1,1),DXVector3(1,-1,1),DXVector3(1,1,1),DXVector3(-1,1,1), DXVector3(0,0,0.5)};

	//build matrix
	D3DXMATRIX matInvViewProj;
	D3DXMatrixInverse(&matInvViewProj,NULL,D3DXMatrixMultiply(&matInvViewProj,(D3DXMATRIX*)&cam.GetViewMatrix(),(D3DXMATRIX*)&cam.GetProjMatrix()));

	//transform all points
	D3DXVec3TransformCoordArray((D3DXVECTOR3*)FrustumPoints, sizeof(DXVector3), (D3DXVECTOR3*)Box, sizeof(DXVector3), &matInvViewProj, 9);

	//calculate the clipplanes
	//DXVector3 dir[4],v[4];
	//D3DXMATRIX matInvView;
	//D3DXMatrixInverse(&matInvView, NULL, (D3DXMATRIX*)&cam.GetViewMatrix() );

	//float rcp1 = 1/cam.GetProjMatrix()._11;
	//float rcp2 = 1/cam.GetProjMatrix()._22;

	//v[0] = DXVector3(-rcp1,+rcp2,1);//topleft
	//v[1] = DXVector3(+rcp1,+rcp2,1);//topright
	//v[2] = DXVector3(-rcp1,-rcp2,1);//bottomleft
	//v[3] = DXVector3(+rcp1,-rcp2,1);//bottomright

	//D3DXVec3TransformNormalArray((D3DXVECTOR3*)dir, sizeof(DXVector3), (D3DXVECTOR3*)v, sizeof(DXVector3), &matInvView, 4);

	////0 = left, 1 = right, 2 = top, 3 = bottom, 4 = near, 5 = far
	//ClipPlanes[0] = DXPlane::FromPointNormal(Orig, DXVector3::NormalizeVec(dir[0].Cross(dir[2])));
	//ClipPlanes[1] = DXPlane::FromPointNormal(Orig, DXVector3::NormalizeVec(dir[3].Cross(dir[1])));
	//ClipPlanes[2] = DXPlane::FromPointNormal(Orig, DXVector3::NormalizeVec(dir[1].Cross(dir[0])));
	//ClipPlanes[3] = DXPlane::FromPointNormal(Orig, DXVector3::NormalizeVec(dir[2].Cross(dir[3])));
	//ClipPlanes[4] = DXPlane::FromPointNormal(Orig + cam.GetViewDirVector() * cam.GetNearPlane(), cam.GetViewDirVector());
	//ClipPlanes[5] = DXPlane::FromPointNormal(Orig + cam.GetViewDirVector() * cam.GetFarPlane(), -cam.GetViewDirVector());

	ClipPlanes[0] = DXPlane::FromPoints(FrustumPoints[3], FrustumPoints[7], FrustumPoints[4]); //left
	ClipPlanes[1] = DXPlane::FromPoints(FrustumPoints[2], FrustumPoints[5], FrustumPoints[6]); //right
	ClipPlanes[2] = DXPlane::FromPoints(FrustumPoints[3], FrustumPoints[6], FrustumPoints[7]); //top
	ClipPlanes[3] = DXPlane::FromPoints(FrustumPoints[1], FrustumPoints[4], FrustumPoints[5]); //bottom
	ClipPlanes[4] = DXPlane::FromPoints(FrustumPoints[0], FrustumPoints[1], FrustumPoints[2]); //near
	ClipPlanes[5] = DXPlane::FromPoints(FrustumPoints[7], FrustumPoints[6], FrustumPoints[5]); //far

	/*for(int i = 0; i < 6; i++)
	{
		if(ClipPlanes[i].DotCoord(FrustumPoints[8]) < 0) 
			ClipPlanes[i] = -ClipPlanes[i]; 
	}*/
}

BoundingMesh::BoundingMesh(const ViewFrustum& frustum)
{
	m_TriangleCount = 6 * 2;
	m_Points = new DXVector3[m_TriangleCount * 3];
	
	//init points array
	DXVector3* vb = m_Points;

	//front face
	*vb++ = frustum.FrustumPoints[0];
	*vb++ = frustum.FrustumPoints[2];
	*vb++ = frustum.FrustumPoints[1];

	*vb++ = frustum.FrustumPoints[0];
	*vb++ = frustum.FrustumPoints[3];
	*vb++ = frustum.FrustumPoints[2];

	//backface
	*vb++ = frustum.FrustumPoints[4];
	*vb++ = frustum.FrustumPoints[5];
	*vb++ = frustum.FrustumPoints[6];

	*vb++ = frustum.FrustumPoints[4];
	*vb++ = frustum.FrustumPoints[6];
	*vb++ = frustum.FrustumPoints[7];

	//bottomface
	*vb++ = frustum.FrustumPoints[0];
	*vb++ = frustum.FrustumPoints[1];
	*vb++ = frustum.FrustumPoints[4];

	*vb++ = frustum.FrustumPoints[4];
	*vb++ = frustum.FrustumPoints[1];
	*vb++ = frustum.FrustumPoints[5];

	//rightface
	*vb++ = frustum.FrustumPoints[1];
	*vb++ = frustum.FrustumPoints[2];
	*vb++ = frustum.FrustumPoints[5];

	*vb++ = frustum.FrustumPoints[2];
	*vb++ = frustum.FrustumPoints[6];
	*vb++ = frustum.FrustumPoints[5];

	//topface
	*vb++ = frustum.FrustumPoints[2];
	*vb++ = frustum.FrustumPoints[3];
	*vb++ = frustum.FrustumPoints[6];

	*vb++ = frustum.FrustumPoints[3];
	*vb++ = frustum.FrustumPoints[7];
	*vb++ = frustum.FrustumPoints[6];

	//leftface
	*vb++ = frustum.FrustumPoints[0];
	*vb++ = frustum.FrustumPoints[4];
	*vb++ = frustum.FrustumPoints[7];

	*vb++ = frustum.FrustumPoints[0];
	*vb++ = frustum.FrustumPoints[7];
	*vb++ = frustum.FrustumPoints[3];
}

void BoundingMesh::ClipByAABB(const AABB& aabb)
{
	if(!m_TriangleCount)
		return;

	D3DXPLANE plane;

	D3DXPlaneFromPointNormal(&plane, aabb.GetMin(), DXVector3(-1,0,0));
	ClipByPlane(plane);

	D3DXPlaneFromPointNormal(&plane, aabb.GetMin(), DXVector3(0,-1,0));
	ClipByPlane(plane);

	D3DXPlaneFromPointNormal(&plane, aabb.GetMin(), DXVector3(0,0,-1));
	ClipByPlane(plane);

	D3DXPlaneFromPointNormal(&plane, aabb.GetMax(), DXVector3(1,0,0));
	ClipByPlane(plane);

	D3DXPlaneFromPointNormal(&plane, aabb.GetMax(), DXVector3(0,1,0));
	ClipByPlane(plane);

	D3DXPlaneFromPointNormal(&plane, aabb.GetMax(), DXVector3(0,0,1));
	ClipByPlane(plane);
}

void BoundingMesh::ClipByPlane(const D3DXPLANE &plane)
{
	if(!m_TriangleCount)
		return;

	DXVector3 planeNormal(plane.a, plane.b, plane.c);
		
	//points for new mesh, start with same size than orig mesh
	int newTriangleCount = 0;
	int maxNewTriangleCount = m_TriangleCount*10;
	int	iNewPoints = 0;
	DXVector3* newPoints = new DXVector3[maxNewTriangleCount*3];

	int maxObjIntrPoints = 50;
	int iObjIntrPoint = 0;
	DXVector3* objIntrPoints = new DXVector3[maxObjIntrPoints];

	for(int p = 0; p < m_TriangleCount; p++)
	{
		//clip triagle by plane and get intersection points

		//find out on which side of the plane the points are
		float side[3];
		for(int v = 0; v < 3; v++)
			side[v] = D3DXPlaneDotCoord(&plane, (const D3DXVECTOR3*)(m_Points + p*3 + v));

		int iNewPoly = 0;
		DXVector3 newPoly[4]; //if clip triangle with plane, a poly with max 4 vertices can be created

		//calculate triangle normal direction
		DXVector3 p0 = m_Points[p*3 + 0];
		DXVector3 p1 = m_Points[p*3 + 1];
		DXVector3 p2 = m_Points[p*3 + 2];

		//calculate nornmal with p0,p1,p2 triangulation
		DXVector3 e1 = p1 - p0;
		DXVector3 e2 = p2 - p0;
		DXVector3 triangleNormal;
		D3DXVec3Cross((D3DXVECTOR3*)&triangleNormal, (D3DXVECTOR3*)&e1, (D3DXVECTOR3*)&e2);

		int triintrpoints = 0;
		//check all 3 edges if they are split by the plane
		for(int v = 0; v < 3; v++)
		{
			int iNext = (v + 1)%3;
			//both outside
			if(side[v] > 0 && side[iNext] > 0)
				continue;

			//check start point, if it is outside
			if(side[v] > 0)
			{
				DXVector3 in = m_Points[p*3 + iNext];
				DXVector3 out = m_Points[p*3 + v];
				if(iObjIntrPoint == maxObjIntrPoints)
				{
					DXVector3* temp = new DXVector3[maxObjIntrPoints + 5];
					memcpy(temp, objIntrPoints, sizeof(DXVector3) * maxObjIntrPoints);
					maxObjIntrPoints += 5;
					delete [] objIntrPoints;
					objIntrPoints = temp;
				}
				DXVector3 d = out - in;
				float t = D3DXPlaneDotNormal(&plane, d);
				DXVector3 intrPoint = in - d / t * side[iNext];
				newPoly[iNewPoly++] = intrPoint;
				objIntrPoints[iObjIntrPoint++] = intrPoint;
				triintrpoints++;
				continue;
			}
			else //point is inside
			{
				newPoly[iNewPoly++] = m_Points[p*3 + v];
			}

			if(side[iNext] > 0)
			{
				DXVector3 in = m_Points[p*3 + v];
				DXVector3 out = m_Points[p*3 + iNext];
				if(iObjIntrPoint == maxObjIntrPoints)
				{
					DXVector3* temp = new DXVector3[maxObjIntrPoints + 5];
					memcpy(temp, objIntrPoints, sizeof(DXVector3) * maxObjIntrPoints);
					maxObjIntrPoints += 5;
					delete [] objIntrPoints;
					objIntrPoints = temp;
				}
				DXVector3 d = out - in;
				float t = D3DXPlaneDotNormal(&plane, d);
				DXVector3 intrPoint = in - d / t * side[v];
				newPoly[iNewPoly++] = intrPoint;
				objIntrPoints[iObjIntrPoint++] = intrPoint;
				triintrpoints++;
			}

			//if(iNewPoly > 4 ) // test breakpoint
			//	iNewPoly = iNewPoly;
		}

		//a plane triangle intersection can only result in none or two intersection points
		//otherwise ignore the new point(s)
		if(triintrpoints != 0 && triintrpoints != 2)
			iObjIntrPoint -= triintrpoints;

		//cover the clipped triangle with new triangles (usually 2, none if completlely clipped off)
		if(iNewPoly > 2)
		{
			//appand newPoly to mesh
			for(int v = 0; v < iNewPoly - 2; v++)
			{
				if(newTriangleCount == maxNewTriangleCount)
				{
					DXVector3* temp = new DXVector3[(maxNewTriangleCount + 5) * 3];
					memcpy(temp, newPoints, sizeof(DXVector3) * maxNewTriangleCount * 3);
					maxNewTriangleCount += 5;
					delete [] newPoints;
					newPoints = temp;
				}

				//add triangle with these three points
				DXVector3 p0 = newPoly[0];
				DXVector3 p1 = newPoly[v+1];
				DXVector3 p2 = newPoly[v+2];

				//calculate nornmal with p0,p1,p2 triangulation
				DXVector3 e1 = p1 - p0;
				DXVector3 e2 = p2 - p0;
				DXVector3 n;
				D3DXVec3Cross((D3DXVECTOR3*)&n, (D3DXVECTOR3*)&e1, (D3DXVECTOR3*)&e2);
				
				//dot with triangle normal > 0 means that the triangulation is the same way round, so keep it
				if(D3DXVec3Dot((D3DXVECTOR3*)&n, (D3DXVECTOR3*)&triangleNormal) > 0)
				{
					newPoints[iNewPoints++] = p0;
					newPoints[iNewPoints++] = p1;
					newPoints[iNewPoints++] = p2;
				}
				else //otherwise flip orientation
				{
					newPoints[iNewPoints++] = p0;
					newPoints[iNewPoints++] = p2;
					newPoints[iNewPoints++] = p1;
				}

				newTriangleCount++;
			}
		}
	}

	//cap the intersection poly with triangles
	//TODO: implement porper triangulation, current implementation only works for convex polygons
	if(iObjIntrPoint)
	{
		//the intersection points come in pairs building line segments
		int segcount = iObjIntrPoint >> 1;
		//it should result in segcount - 2 triangles
		int captriangles = segcount - 2;

		//at least 3 segments are needed to build on polygon
		if(segcount > 1)
		{
			DXVector3 refpoint = objIntrPoints[1];//we cant store index, because added segments get invalidated
			//int irefpoint = 1; //point where segment should be connected
			for(int t = 0; t < captriangles; t++)
			{
				//find connecting segment (nearest point to current reference point)
				//-1 because last segment only closes polygon
				int iminseg = -1;
				bool zo1 = false; //indicates which point connects the segment
				float dmin = FLT_MAX;
				for(int i = 1; i < segcount; i++)
				{
					int ipoint0 = i << 1;
					//test both points
				
					DXVector3 vd;
					float d;

					vd = objIntrPoints[ipoint0] - refpoint;
					d = abs(vd.x) + abs(vd.y) + abs(vd.z); //should be accurate enough

					if(d < dmin)
					{
						iminseg = i;
						dmin = d;
						zo1 = true;
					}

					vd = objIntrPoints[ipoint0 + 1] - refpoint;
					d = abs(vd.x) + abs(vd.y) + abs(vd.z); //should be accurate enough

					if(d < dmin)
					{
						iminseg = i;
						dmin = d;
						zo1 = false;
					}
				}

				if(iminseg > 0)
				{
					//attach new triangle to list
					if(newTriangleCount + 1 == maxNewTriangleCount)
					{
						DXVector3* temp = new DXVector3[(maxNewTriangleCount + 5) * 3];
						memcpy(temp, newPoints, sizeof(DXVector3) * maxNewTriangleCount * 3);
						maxNewTriangleCount += 5;
						delete [] newPoints;
						newPoints = temp;
					}

					//add triangle with these three points
					DXVector3 p0 = objIntrPoints[0];
					DXVector3 p1 = objIntrPoints[iminseg << 1];
					DXVector3 p2 = objIntrPoints[(iminseg << 1) + 1];

					//calculate nornmal with p0,p1,p2 triangulation
					DXVector3 e1 = p1 - p0;
					DXVector3 e2 = p2 - p0;
					DXVector3 n;
					D3DXVec3Cross((D3DXVECTOR3*)&n, (D3DXVECTOR3*)&e1, (D3DXVECTOR3*)&e2);
					
					//dot with plane normal > 0 means that the triangulation is the right way round, so keep it
					if(D3DXVec3Dot((D3DXVECTOR3*)&n, (D3DXVECTOR3*)&planeNormal) > 0)
					{
						newPoints[iNewPoints++] = p0;
						newPoints[iNewPoints++] = p1;
						newPoints[iNewPoints++] = p2;
					}
					else //otherwise flip orientation
					{
						newPoints[iNewPoints++] = p0;
						newPoints[iNewPoints++] = p2;
						newPoints[iNewPoints++] = p1;
					}

					newTriangleCount++;
				}
				else
				{
					//there must be any damange in the segment list when no intr poin
					break;
				}

				//zo1 == point that connects the segments, other point of segemnts becomes new reference point
				if(zo1)
					refpoint = objIntrPoints[(iminseg << 1) + 1];
				else
					refpoint = objIntrPoints[(iminseg << 1)];

				//invalidate added segment, setting x to flt_max should do that
				objIntrPoints[iminseg << 1].x = FLT_MAX;
				objIntrPoints[(iminseg << 1) + 1].x = FLT_MAX;
			}
		}
	}

	delete [] objIntrPoints;

	delete [] m_Points;

	m_Points = newPoints;
	m_TriangleCount = newTriangleCount;
}

AABB BoundingMesh::BuildAABB(void) const
{
	if(m_TriangleCount)
	{
		AABB b(m_Points[0], m_Points[0]);
		for(int i = 1; i < m_TriangleCount * 3; i++)
			b.AddPoint(m_Points[i]);
		return b;
	}
	else
	{
		return AABB();
	}
}

float Dist_Point_Plane(const DXVector3& point,const D3DXPLANE &plane)
{
	return fabs(D3DXPlaneDotNormal(&plane, point) - plane.d);
}

bool Intr_AABB_Point(const DXVector3& point,const OBB&box)
{
	if(point.x < box.GetMax().x && point.x > box.GetMin().x &&
		point.y < box.GetMax().y && point.y > box.GetMin().y &&
		point.z < box.GetMax().z && point.z > box.GetMin().z)
		return true;
	else 
		return false;
}

bool Intr_AABB_Ray(const AABB&box,const DXVector3&orig,const DXVector3&dir)
{
	DXVector3 BoxExtents, Diff;

	Diff.x = orig.x - box.GetCenter().x;
	BoxExtents.x = box.GetE().x;
	if(fabsf(Diff.x)>BoxExtents.x && Diff.x*dir.x>=0.0f)	return false;

	Diff.y = orig.y - box.GetCenter().y;
	BoxExtents.y = box.GetE().y;
	if(fabsf(Diff.y)>BoxExtents.y && Diff.y*dir.y>=0.0f)	return false;

	Diff.z = orig.z - box.GetCenter().z;
	BoxExtents.z = box.GetE().z;
	if(fabsf(Diff.z)>BoxExtents.z && Diff.z*dir.z>=0.0f)	return false;

	float fAWdU[3];
	fAWdU[0] = fabsf(dir.x);
	fAWdU[1] = fabsf(dir.y);
	fAWdU[2] = fabsf(dir.z);

	float f;
	f = dir.y * Diff.z - dir.z * Diff.y;	if(fabsf(f)>BoxExtents.y*fAWdU[2] + BoxExtents.z*fAWdU[1])	return false;
	f = dir.z * Diff.x - dir.x * Diff.z;	if(fabsf(f)>BoxExtents.x*fAWdU[2] + BoxExtents.z*fAWdU[0])	return false;
	f = dir.x * Diff.y - dir.y * Diff.x;	if(fabsf(f)>BoxExtents.x*fAWdU[1] + BoxExtents.y*fAWdU[0])	return false;

	return true;
}

bool Intr_AABB_Line(const AABB&box,const DXVector3&start,const DXVector3&end)
{
	/* ALGORITHM: Use the separating axis 
	theorem to see if the line segment 
	and the box overlap. A line 
	segment is a degenerate OBB. */

	DXVector3 dir = (end - start)/2.f;
	float hl = D3DXVec3Length(dir);
	DXVector3 mid = start + dir;
	D3DXVec3Normalize(dir, dir);
	DXVector3 T = box.GetCenter() - mid;
	DXVector3 v;
	float r;

	//do any of the principal axes
	//form a separating axis?

	if( fabs(T.x) > box.GetE().x + fabs(dir.x) )
	return false;

	if( fabs(T.y) > box.GetE().y + fabs(dir.y) )
	return false;

	if( fabs(T.z) > box.GetE().z + fabs(dir.z) )
	return false;

	/* NOTE: Since the separating axis is
	perpendicular to the line in these
	last four cases, the line does not
	contribute to the projection. */

	//l.cross(x-axis)?

	r = box.GetE().y*fabs(dir.z) + box.GetE().z*fabs(dir.y);

	if( fabs(T.y*dir.z - T.z*dir.y) > r )
		return false;

	//l.cross(y-axis)?

	r = box.GetE().x*fabs(dir.z) + box.GetE().z*fabs(dir.x);

	if( fabs(T.z*dir.x - T.x*dir.z) > r )
		return false;

	//l.cross(z-axis)?

	r = box.GetE().x*fabs(dir.y) + box.GetE().y*fabs(dir.x);

	if( fabs(T.x*dir.y - T.y*dir.x) > r )
		return false;

	return true;
}

bool Intr_AABB_Plane(const AABB &box, const D3DXPLANE& plane)
{
	const float x = box.GetE().x * plane.a;
	const float y = box.GetE().y * plane.b;
	const float z = box.GetE().z * plane.c;

	const float rad = fabs(x) + fabs(y) + fabs(z);
    const float signeddist = Dist_Point_Plane(box.GetCenter(), plane);
		
    return signeddist <= rad;
}


bool Intr_AABB_OBB(const AABB& aabb, const OBB& obb)
{
	//make culltest for each face plane of the obb against the aabb
	D3DXPLANE p;

	DXVector3 worldMin;
	D3DXVec3TransformCoord((D3DXVECTOR3*)&worldMin, (D3DXVECTOR3*)&obb.GetMin(), (D3DXMATRIX*)&obb.GetOrientation());

	if(aabb.IsCulled(DXPlane::FromPointNormal(worldMin, obb.GetOrientation().GetXAxis())))   
		return false;

	if(aabb.IsCulled(DXPlane::FromPointNormal(worldMin, obb.GetOrientation().GetYAxis())))  
		return false; 

	if(aabb.IsCulled(DXPlane::FromPointNormal(worldMin, obb.GetOrientation().GetZAxis())))  
		return false; 

	DXVector3 worldMax;
	D3DXVec3TransformCoord((D3DXVECTOR3*)&worldMax, (D3DXVECTOR3*)&obb.GetMax(), (D3DXMATRIX*)&obb.GetOrientation());

	if(aabb.IsCulled(DXPlane::FromPointNormal(worldMax, -obb.GetOrientation().GetXAxis())))  
		return false;

	if(aabb.IsCulled(DXPlane::FromPointNormal(worldMax, -obb.GetOrientation().GetYAxis())))  
		return false;

	if(aabb.IsCulled(DXPlane::FromPointNormal(worldMax, -obb.GetOrientation().GetZAxis())))  
		return false;

	/*D3DXPlaneFromPointNormal(&p, (D3DXVECTOR3*)&aabb.GetMax(), (D3DXVECTOR3*)&DXVector3(-1,0,0));
	if(obb.IsCulled(p))   
		return false;

	D3DXPlaneFromPointNormal(&p, (D3DXVECTOR3*)&aabb.GetMax(), (D3DXVECTOR3*)&DXVector3(0,-1,0));
	if(obb.IsCulled(p))
		return false;

	D3DXPlaneFromPointNormal(&p, (D3DXVECTOR3*)&aabb.GetMax(), (D3DXVECTOR3*)&DXVector3(0,0,-1));
	if(obb.IsCulled(p)) 
		return false;

	D3DXPlaneFromPointNormal(&p, (D3DXVECTOR3*)&aabb.GetMin(), (D3DXVECTOR3*)&DXVector3(1,0,0));
	if(obb.IsCulled(p))
		return false;

	D3DXPlaneFromPointNormal(&p, (D3DXVECTOR3*)&aabb.GetMin(), (D3DXVECTOR3*)&DXVector3(0,1,0));
	if(obb.IsCulled(p))
		return false;

	D3DXPlaneFromPointNormal(&p, (D3DXVECTOR3*)&aabb.GetMin(), (D3DXVECTOR3*)&DXVector3(0,0,1));
	if(obb.IsCulled(p))
		return false;*/

	return true;
}

bool Intr_AABB_OBS(const AABB& aabb, const OBS& obs)
{
	DXVector3 vDiff = obs.Center - aabb.GetCenter();
	
	if(fabs(vDiff.x) <= aabb.GetE().x)
	{
		if(fabs(vDiff.y) <= aabb.GetE().y)
		{
			if(fabs(vDiff.z) <= aabb.GetE().z)
			{
				return true;
			}
			else
			{
				// potential sphere-face intersection with face z
                return vDiff.z <= obs.Radius;
			}
		}
		else
		{
			if(fabs(vDiff.z) <= aabb.GetE().z)
			{
				// potential sphere-face intersection with face y
                return vDiff.y <= obs.Radius;
			}
			else
			{
				// potential sphere-edge intersection with edge formed
				// by faces y and z
				float radsqr = obs.Radius * obs.Radius;
				return vDiff.y*vDiff.y + vDiff.z*vDiff.z <= radsqr;
			}
		}
	}
	else
	{
		if(fabs(vDiff.y) <= aabb.GetE().y)
		{
			if(fabs(vDiff.z) <= aabb.GetE().z)
			{
				// potential sphere-face intersection with face x
                return vDiff.x <= obs.Radius;
			}
			else
			{
				// potential sphere-edge intersection with edge formed
                // by faces x and z
                float radsqr = obs.Radius * obs.Radius;
                return vDiff.x*vDiff.x + vDiff.z*vDiff.z <= radsqr;

			}
		}
		else
		{
			if(fabs(vDiff.z) <= aabb.GetE().z)
			{
				// potential sphere-edge intersection with edge formed
                // by faces x and y
                float radsqr = obs.Radius * obs.Radius;
                return vDiff.x*vDiff.x + vDiff.y*vDiff.y <= radsqr;

			}
			else
			{
				// potential sphere-vertex intersection at corner formed
                // by faces x,y,z
                float radsqr = obs.Radius * obs.Radius;
                return vDiff.x*vDiff.x + vDiff.y*vDiff.y + vDiff.z*vDiff.z <= radsqr;
			}
		}
	}
}

CONTAINMENTTESTRESULT Cont_AABB_In_Frustum(const AABB& aabb, const Frustum& f)
{
	DWORD nvis = 0,pv[6+5][8]={0,0,0,0,0,0,0};
	ZeroMemory(pv,sizeof(DWORD)*(6+5)*8);

	int nplanes = (int)f.planes.size();
	int i = 0;

	for (std::vector<DXPlane>::const_iterator it = f.planes.begin(); it != f.planes.end(); it++, i++)
	{
		DWORD n=0;
		if (it->DotCoord(aabb.GetMin()) > 0)
		{
			pv[i][0]++; n++;
		}
		if (it->DotCoord(DXVector3(aabb.GetMin().x, aabb.GetMin().y, aabb.GetMax().z)) > 0)
		{
			pv[i][1]++; n++;
		}
		if (it->DotCoord(DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMin().z)) > 0)
		{
			pv[i][2]++; n++;
		}
		if (it->DotCoord(DXVector3(aabb.GetMin().x, aabb.GetMax().y, aabb.GetMax().z)) > 0)
		{
			pv[i][3]++; n++;
		}
		if (it->DotCoord(DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMin().z)) > 0)
		{
			pv[i][4]++; n++;
		}
		if (it->DotCoord(DXVector3(aabb.GetMax().x, aabb.GetMin().y, aabb.GetMax().z)) > 0)
		{
			pv[i][5]++; n++;
		}
		if (it->DotCoord(DXVector3(aabb.GetMax().x, aabb.GetMax().y, aabb.GetMin().z)) > 0)
		{
			pv[i][6]++; n++;
		}
		if (it->DotCoord(aabb.GetMax()) > 0)
		{
			pv[i][7]++; n++;
		}
		if(!n)//all points failed this plane
		{
			return CTR_DISJOINT;
		}
	}

	int j;
	{
		//try to find point that passed all tests
		for(i=0;i<8;i++)
		{
			for(j=0;j<nplanes;j++)
			{
				if(!pv[j][i])
					break;
			}
			if(j == nplanes)
				break;
		}
		
		if(j<nplanes)
		{
			//no point is visible and the points are behind different faces, so there is a chance that parts are visible
			//this case can occur if the box is bigger than the view frustum
			//also if just a side from the box cuts the frustum
			
//			OBS sphere(node->box.Center,D3DXVec3Length(&(node->box.GetMax()-node->box.GetMin()))*0.5f);
			//this test is always true ... ?
//			if(Intr_OBS_Frustum(sphere,m_Frustum))
			if(1)
			{
				return CTR_INTERSECT;
			}
			else
			{
				return CTR_DISJOINT;
			}
		}
		else
		{
			//check if all points passed all tests
			for(i=0;i<8;i++)
			{
				for(j=0;j<nplanes;j++)
				{
					if(pv[j][i])
						break;
				}
				if(j < nplanes)
					break;
			}

			if(i==8 && j==nplanes)
			{
				return CTR_CONTAINS;
			}
			else
			{
				return CTR_INTERSECT;
			}
		}
	}
}

CONTAINMENTTESTRESULT Cont_AABB_In_AABB(const AABB& aabb1, const AABB& aabb2)
{
	//test if disjoint
	if (aabb1.GetMin().x > aabb2.GetMax().x || aabb1.GetMax().x < aabb2.GetMin().x ||
		aabb1.GetMin().y > aabb2.GetMax().y || aabb1.GetMax().y < aabb2.GetMin().y ||
		aabb1.GetMin().z > aabb2.GetMax().z || aabb1.GetMax().z < aabb2.GetMin().z)
	{
		return CTR_DISJOINT;
	}
	
	//test if contains
	if (aabb1.GetMin().x > aabb2.GetMin().x && aabb1.GetMax().x  < aabb2.GetMax().x &&
		aabb1.GetMin().y > aabb2.GetMin().y && aabb1.GetMax().y  < aabb2.GetMax().y &&
		aabb1.GetMin().z > aabb2.GetMin().z && aabb1.GetMax().z  < aabb2.GetMax().z)
	{
		return CTR_CONTAINS;
	}

	return CTR_INTERSECT;
}

CONTAINMENTTESTRESULT Cont_OBS_In_Frustum(const OBS& sphere, const Frustum& f)
{
	bool binter = false;
	for (std::vector<DXPlane>::const_iterator it = f.planes.begin(); it != f.planes.end(); it++)
	{
		float d = it->DotCoord(sphere.Center);
		if (d < -sphere.Radius)
			return CTR_DISJOINT;
		else if (d < sphere.Radius)
			binter = true;
	}

	if (binter)
		return CTR_INTERSECT;
	else 
		return CTR_CONTAINS;
}

bool Intr_OBS_ViewFrustum(const OBS &sphere,const ViewFrustum &frustum)
{
	int i;
	for(i=0;i<6;i++)
	{
		float d = frustum.ClipPlanes[i].DotNormal(sphere.Center) + frustum.ClipPlanes[i].d;
		if(d < -sphere.Radius)
			break;
	}

	if(i < 6)
		return false;
	else
		return true;
}

bool Intr_OBB_ViewFrustum(const OBB &box,const ViewFrustum &frustum)
{
/*	 // for convenience
    const DXVector3* A = box.m_rkBox.Axis;
    const float* E = m_rkBox.Extent;

    DXVector3 Diff = box.Center - frustum.Origin;// C-E

    Real afA[3];      // Dot(R,A[i])
    Real afB[3];      // Dot(U,A[i])
    Real afC[3];      // Dot(D,A[i])
    Real afD[3];      // (Dot(R,C-E),Dot(U,C-E),Dot(D,C-E))
    Real afNA[3];     // dmin*Dot(R,A[i])
    Real afNB[3];     // dmin*Dot(U,A[i])
    Real afNC[3];     // dmin*Dot(D,A[i])
    Real afND[3];     // dmin*(Dot(R,C-E),Dot(U,C-E),?)
    Real afRC[3];     // rmax*Dot(D,A[i])
    Real afRD[3];     // rmax*(?,?,Dot(D,C-E))
    Real afUC[3];     // umax*Dot(D,A[i])
    Real afUD[3];     // umax*(?,?,Dot(D,C-E))
    Real afNApRC[3];  // dmin*Dot(R,A[i]) + rmax*Dot(D,A[i])
    Real afNAmRC[3];  // dmin*Dot(R,A[i]) - rmax*Dot(D,A[i])
    Real afNBpUC[3];  // dmin*Dot(U,A[i]) + umax*Dot(D,A[i])
    Real afNBmUC[3];  // dmin*Dot(U,A[i]) - umax*Dot(D,A[i])
    Real afRBpUA[3];  // rmax*Dot(U,A[i]) + umax*Dot(R,A[i])
    Real afRBmUA[3];  // rmax*Dot(U,A[i]) - umax*Dot(R,A[i])
    Real fDdD, fRad, fP, fMin, fMax, fMTwoUF, fMTwoRF, fTmp;
    int i, j;

    // M = D
    afD[2] = kDiff.Dot(m_rkFrustum.DVector);
    for (i = 0; i < 3; i++)
    {
        afC[i] = akA[i].Dot(m_rkFrustum.DVector);
    }
    fRad =
        afE[0]*Math<Real>::FAbs(afC[0]) +
        afE[1]*Math<Real>::FAbs(afC[1]) +
        afE[2]*Math<Real>::FAbs(afC[2]);
    if (afD[2] + fRad < m_rkFrustum.DMin || afD[2] - fRad > m_rkFrustum.DMax)
    {
        return false;
    }

    // M = n*R - r*D
    for (i = 0; i < 3; i++)
    {
        afA[i] = akA[i].Dot(m_rkFrustum.RVector);
        afRC[i] = m_rkFrustum.RBound*afC[i];
        afNA[i] = m_rkFrustum.DMin*afA[i];
        afNAmRC[i] = afNA[i] - afRC[i];
    }
    afD[0] = kDiff.Dot(m_rkFrustum.RVector);
    fRad =
        afE[0]*Math<Real>::FAbs(afNAmRC[0]) +
        afE[1]*Math<Real>::FAbs(afNAmRC[1]) +
        afE[2]*Math<Real>::FAbs(afNAmRC[2]);
    afND[0] = m_rkFrustum.DMin*afD[0];
    afRD[2] = m_rkFrustum.RBound*afD[2];
    fDdD = afND[0] - afRD[2];
    fMTwoRF = m_rkFrustum.GetMTwoRF();
    if (fDdD + fRad < fMTwoRF || fDdD > fRad)
    {
        return false;
    }

    // M = -n*R - r*D
    for (i = 0; i < 3; i++)
    {
        afNApRC[i] = afNA[i] + afRC[i];
    }
    fRad =
        afE[0]*Math<Real>::FAbs(afNApRC[0]) +
        afE[1]*Math<Real>::FAbs(afNApRC[1]) +
        afE[2]*Math<Real>::FAbs(afNApRC[2]);
    fDdD = -(afND[0] + afRD[2]);
    if (fDdD + fRad < fMTwoRF || fDdD > fRad)
    {
        return false;
    }

    // M = n*U - u*D
    for (i = 0; i < 3; i++)
    {
        afB[i] = akA[i].Dot(m_rkFrustum.UVector);
        afUC[i] = m_rkFrustum.UBound*afC[i];
        afNB[i] = m_rkFrustum.DMin*afB[i];
        afNBmUC[i] = afNB[i] - afUC[i];
    }
    afD[1] = kDiff.Dot(m_rkFrustum.UVector);
    fRad =
        afE[0]*Math<Real>::FAbs(afNBmUC[0]) +
        afE[1]*Math<Real>::FAbs(afNBmUC[1]) +
        afE[2]*Math<Real>::FAbs(afNBmUC[2]);
    afND[1] = m_rkFrustum.DMin*afD[1];
    afUD[2] = m_rkFrustum.UBound*afD[2];
    fDdD = afND[1] - afUD[2];
    fMTwoUF = m_rkFrustum.GetMTwoUF();
    if (fDdD + fRad < fMTwoUF || fDdD > fRad)
    {
        return false;
    }

    // M = -n*U - u*D
    for (i = 0; i < 3; i++)
    {
        afNBpUC[i] = afNB[i] + afUC[i];
    }
    fRad =
        afE[0]*Math<Real>::FAbs(afNBpUC[0]) +
        afE[1]*Math<Real>::FAbs(afNBpUC[1]) +
        afE[2]*Math<Real>::FAbs(afNBpUC[2]);
    fDdD = -(afND[1] + afUD[2]);
    if (fDdD + fRad < fMTwoUF || fDdD > fRad)
    {
        return false;
    }

    // M = A[i]
    for (i = 0; i < 3; i++)
    {
        fP = m_rkFrustum.RBound*Math<Real>::FAbs(afA[i]) +
             m_rkFrustum.UBound*Math<Real>::FAbs(afB[i]);
        afNC[i] = m_rkFrustum.DMin*afC[i];
        fMin = afNC[i] - fP;
        if (fMin < 0.0f)
        {
            fMin *= m_rkFrustum.GetDRatio();
        }
        fMax = afNC[i] + fP;
        if (fMax > 0.0f)
        {
            fMax *= m_rkFrustum.GetDRatio();
        }
        fDdD = afA[i]*afD[0] + afB[i]*afD[1] + afC[i]*afD[2];
        if (fDdD + afE[i] < fMin || fDdD - afE[i] > fMax)
        {
            return false;
        }
    }

    // M = Cross(R,A[i])
    for (i = 0; i < 3; i++)
    {
        fP = m_rkFrustum.UBound*Math<Real>::FAbs(afC[i]);
        fMin = -afNB[i] - fP;
        if (fMin < 0.0f)
        {
            fMin *= m_rkFrustum.GetDRatio();
        }
        fMax = -afNB[i] + fP;
        if (fMax > 0.0f)
        {
            fMax *= m_rkFrustum.GetDRatio();
        }
        fDdD = afC[i]*afD[1] - afB[i]*afD[2];
        fRad =
            afE[0]*Math<Real>::FAbs(afB[i]*afC[0]-afB[0]*afC[i]) +
            afE[1]*Math<Real>::FAbs(afB[i]*afC[1]-afB[1]*afC[i]) +
            afE[2]*Math<Real>::FAbs(afB[i]*afC[2]-afB[2]*afC[i]);
        if (fDdD + fRad < fMin || fDdD - fRad > fMax)
        {
            return false;
        }
    }

    // M = Cross(U,A[i])
    for (i = 0; i < 3; i++)
    {
        fP = m_rkFrustum.RBound*Math<Real>::FAbs(afC[i]);
        fMin = afNA[i] - fP;
        if (fMin < 0.0f)
        {
            fMin *= m_rkFrustum.GetDRatio();
        }
        fMax = afNA[i] + fP;
        if (fMax > 0.0f)
        {
            fMax *= m_rkFrustum.GetDRatio();
        }
        fDdD = -afC[i]*afD[0] + afA[i]*afD[2];
        fRad =
            afE[0]*Math<Real>::FAbs(afA[i]*afC[0]-afA[0]*afC[i]) +
            afE[1]*Math<Real>::FAbs(afA[i]*afC[1]-afA[1]*afC[i]) +
            afE[2]*Math<Real>::FAbs(afA[i]*afC[2]-afA[2]*afC[i]);
        if (fDdD + fRad < fMin || fDdD - fRad > fMax)
        {
            return false;
        }
    }

    // M = Cross(n*D+r*R+u*U,A[i])
    for (i = 0; i < 3; i++)
    {
        Real fRB = m_rkFrustum.RBound*afB[i];
        Real fUA = m_rkFrustum.UBound*afA[i];
        afRBpUA[i] = fRB + fUA;
        afRBmUA[i] = fRB - fUA;
    }
    for (i = 0; i < 3; i++)
    {
        fP = m_rkFrustum.RBound*Math<Real>::FAbs(afNBmUC[i]) +
             m_rkFrustum.UBound*Math<Real>::FAbs(afNAmRC[i]);
        fTmp = -m_rkFrustum.DMin*afRBmUA[i];
        fMin = fTmp - fP;
        if (fMin < 0.0f)
        {
            fMin *= m_rkFrustum.GetDRatio();
        }
        fMax = fTmp + fP;
        if (fMax > 0.0f)
        {
            fMax *= m_rkFrustum.GetDRatio();
        }
        fDdD = afD[0]*afNBmUC[i] - afD[1]*afNAmRC[i] - afD[2]*afRBmUA[i];
        fRad = 0.0f;
        for (j = 0; j < 3; j++)
        {
            fRad += afE[j]*Math<Real>::FAbs(afA[j]*afNBmUC[i] -
                afB[j]*afNAmRC[i] - afC[j]*afRBmUA[i]);
        }
        if (fDdD + fRad < fMin || fDdD - fRad > fMax)
        {
            return false;
        }
    }

    // M = Cross(n*D+r*R-u*U,A[i])
    for (i = 0; i < 3; i++)
    {
        fP = m_rkFrustum.RBound*Math<Real>::FAbs(afNBpUC[i]) +
             m_rkFrustum.UBound*Math<Real>::FAbs(afNAmRC[i]);
        fTmp = -m_rkFrustum.DMin*afRBpUA[i];
        fMin = fTmp - fP;
        if (fMin < 0.0f)
        {
            fMin *= m_rkFrustum.GetDRatio();
        }
        fMax = fTmp + fP;
        if (fMax > 0.0f)
        {
            fMax *= m_rkFrustum.GetDRatio();
        }
        fDdD = afD[0]*afNBpUC[i] - afD[1]*afNAmRC[i] - afD[2]*afRBpUA[i];
        fRad = 0.0f;
        for (j = 0; j < 3; j++)
        {
            fRad += afE[j]*Math<Real>::FAbs(afA[j]*afNBpUC[i] -
                afB[j]*afNAmRC[i] - afC[j]*afRBpUA[i]);
        }
        if (fDdD + fRad < fMin || fDdD - fRad > fMax)
        {
            return false;
        }
    }

    // M = Cross(n*D-r*R+u*U,A[i])
    for (i = 0; i < 3; i++)
    {
        fP = m_rkFrustum.RBound*Math<Real>::FAbs(afNBmUC[i]) +
             m_rkFrustum.UBound*Math<Real>::FAbs(afNApRC[i]);
        fTmp = m_rkFrustum.DMin*afRBpUA[i];
        fMin = fTmp - fP;
        if (fMin < 0.0f)
        {
            fMin *= m_rkFrustum.GetDRatio();
        }
        fMax = fTmp + fP;
        if (fMax > 0.0f)
        {
            fMax *= m_rkFrustum.GetDRatio();
        }
        fDdD = afD[0]*afNBmUC[i] - afD[1]*afNApRC[i] + afD[2]*afRBpUA[i];
        fRad = 0.0f;
        for (j = 0; j < 3; j++)
        {
            fRad += afE[j]*Math<Real>::FAbs(afA[j]*afNBmUC[i] -
                afB[j]*afNApRC[i] + afC[j]*afRBpUA[i]);
        }
        if (fDdD + fRad < fMin || fDdD - fRad > fMax)
        {
            return false;
        }
    }

    // M = Cross(n*D-r*R-u*U,A[i])
    for (i = 0; i < 3; i++)
    {
        fP = m_rkFrustum.RBound*Math<Real>::FAbs(afNBpUC[i]) +
             m_rkFrustum.UBound*Math<Real>::FAbs(afNApRC[i]);
        fTmp = m_rkFrustum.DMin*afRBmUA[i];
        fMin = fTmp - fP;
        if (fMin < 0.0f)
        {
            fMin *= m_rkFrustum.GetDRatio();
        }
        fMax = fTmp + fP;
        if (fMax > 0.0f)
        {
            fMax *= m_rkFrustum.GetDRatio();
        }
        fDdD = afD[0]*afNBpUC[i] - afD[1]*afNApRC[i] + afD[2]*afRBmUA[i];
        fRad = 0.0f;
        for (j = 0; j < 3; j++)
        {
            fRad += afE[j]*Math<Real>::FAbs(afA[j]*afNBpUC[i] -
                afB[j]*afNApRC[i] + afC[j]*afRBmUA[i]);
        }
        if (fDdD + fRad < fMin || fDdD - fRad > fMax)
        {
            return false;
        }
    }*/

    return true;
}

bool Intr_OBB_Plane(const OBB& box, const D3DXPLANE& plane)
{
	return 0;
}

bool Intr_OBB_OBB(const OBB& box1, const OBB& box2)
{
	//make culltest for each face plane of the obb against the aabb
	if(box1.IsCulled(DXPlane::FromPointNormal(box2.GetMax(), box2.GetOrientation().GetXAxis())))
		return false;

	if(box1.IsCulled(DXPlane::FromPointNormal(box2.GetMax(), box2.GetOrientation().GetYAxis())))
		return false;

	if(box1.IsCulled(DXPlane::FromPointNormal(box2.GetMax(), box2.GetOrientation().GetZAxis())))
		return false;

	if(box1.IsCulled(DXPlane::FromPointNormal(box2.GetMin(), -box2.GetOrientation().GetXAxis())))
		return false;

	if(box1.IsCulled(DXPlane::FromPointNormal(box2.GetMin(), -box2.GetOrientation().GetYAxis())))
		return false;

	if(box1.IsCulled(DXPlane::FromPointNormal(box2.GetMin(), -box2.GetOrientation().GetZAxis())))
		return false;

	return true;
}

bool Intr_OBB_OBS(const OBB& box, const OBS& obs)
{
	// Test for intersection in the coordinate system of the box by
    // transforming the sphere into that coordinate system.

	//DXVector3 worldCenter;
	//D3DXVec3TransformCoord((D3DXVECTOR3*)&worldCenter, (D3DXVECTOR3*)&box.GetCenter(), (D3DXMATRIX*)&box.GetOrientation());

    //DXVector3 vDiff = worldCenter - obs.Center;  
	DXVector3 vDiff = box.GetCenter() - obs.Center;  

	float x = vDiff.Dot(box.GetOrientation().GetXAxis());
	float y = vDiff.Dot(box.GetOrientation().GetYAxis());
	float z = vDiff.Dot(box.GetOrientation().GetZAxis());
	DXVector3 d = DXVector3(x, y, z) - box.GetE();
	
	if(x <= box.GetE().x)
	{
		if(y <= box.GetE().y)
		{
			if(z <= box.GetE().z)
			{
				return true;
			}
			else
			{
				// potential sphere-face intersection with face z
                return d.z <= obs.Radius;
			}
		}
		else
		{
			if(z <= box.GetE().z)
			{
				// potential sphere-face intersection with face y
                return d.y <= obs.Radius;
			}
			else
			{
				// potential sphere-edge intersection with edge formed
				// by faces y and z
				float radsqr = obs.Radius * obs.Radius;
				return d.y*d.y + d.z*d.z <= radsqr;
			}
		}
	}
	else
	{
		if(y <= box.GetE().y)
		{
			if(z <= box.GetE().z)
			{
				// potential sphere-face intersection with face x
                return d.x <= obs.Radius;
			}
			else
			{
				// potential sphere-edge intersection with edge formed
                // by faces x and z
                float radsqr = obs.Radius * obs.Radius;
                return d.x*d.x + d.z*d.z <= radsqr;

			}
		}
		else
		{
			if(z <= box.GetE().z)
			{
				// potential sphere-edge intersection with edge formed
                // by faces x and y
                float radsqr = obs.Radius * obs.Radius;
                return d.x*d.x + d.y*d.y <= radsqr;

			}
			else
			{
				// potential sphere-vertex intersection at corner formed
                // by faces x,y,z
                float radsqr = obs.Radius * obs.Radius;
                return d.x*d.x + d.y*d.y + d.z*d.z <= radsqr;
			}
		}
	}
}

DXVector3 Intr_Ray_Plane(const DXPlane &plane,const DXVector3 &orig,const DXVector3 &dir)
{
	DXVector3 pn = plane.GetNormal();
	float t = -(orig.Dot(pn) + plane.d) / dir.Dot(DXVector3(pn));
	return orig + t * dir;
}

DXVector3 Intr_Ray_Plane(const DXPlane& plane,const DXVector3& orig,const DXVector3& dir, float& distout)
{
	DXVector3 pn(plane.a, plane.b, plane.c);
	distout = -(orig.Dot(pn) + plane.d) / dir.Dot(DXVector3(pn));
	return orig + distout * dir;
}

bool Intr_Point_ViewFrustum(const DXVector3 &point,const ViewFrustum &frustum)
{
	for(int i=0;i<6;i++)
		if(frustum.ClipPlanes[i].DotCoord(point) < 0.f)
			return false;
	
	return true;
}

bool Intr_Point_Frustum(const DXVector3 &point,const Frustum &frustum)
{
	for (std::vector<DXPlane>::const_iterator it = frustum.planes.begin(); it != frustum.planes.end(); it++)
	{
		if(it->DotCoord(point) < 0.f)
			return false;
	}
	
	return true;
}