#include "munga.h"
#pragma hdrstop

#include "boxsolid.h"
#include "origin.h"
#include "linmtrx.h"
#include "line.h"
#include "plane.h"
#include "vector2d.h"

//#############################################################################
//#########################    RightHandedTile    ######################
//#############################################################################

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
RightHandedTile::RightHandedTile(
	const ExtentBox &extents,
	BoxedSolid::Material material,
	Simulation *owner,
	BoxedSolid *next_solid,
	Scalar *corners,
	Type type
):
	BoxedSolid(extents, type, material, owner, next_solid)
{
	Check_Pointer(this);
	for (int i=0; i<ELEMENTS(cornerHeight); ++i)
	{
		cornerHeight[i] = corners[i];
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
RightHandedTile::~RightHandedTile()
{
	Check_Pointer(this);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	RightHandedTile::IntersectsBounded(const ExtentBox &extents)
{
	Check(this);
	Check(&extents);

	Verify(minX <= extents.minX);
	Verify(maxX >= extents.maxX);
	Verify(minY <= extents.minY);
	Verify(maxY >= extents.maxY);
	Verify(minZ <= extents.minZ);
	Verify(maxZ >= extents.maxZ);

	//
	//-------------------------------------------------------------------
	// See if the box hits the upper-right triangle anywhere on its plane
	//-------------------------------------------------------------------
	//	
	Point3D p0,p1,p2;
	p0.x = maxX;
	p0.y = cornerHeight[1];
	p0.z = minZ;

	p1.x = minX;
	p1.y = cornerHeight[0];
	p1.z = minZ;

	p2.x = maxX;
	p2.y = cornerHeight[3];
	p2.z = maxZ;

	Plane plane1(p0, p1, p2);
	if (plane1.ContainsSomeOf(extents))
	{
		//
		//-------------------------------------------------------------------
		// Make sure the XZ projections of the triangle and the box intersect
		//-------------------------------------------------------------------
		//
		return True;
	}

	//
	//------------------------------------------------------------------
	// See if the box hits the lower-left triangle anywhere on its plane
	//------------------------------------------------------------------
	//
	p0.x = minX;
	p0.y = cornerHeight[2];
	p0.z = maxZ;

	p1.x = maxX;
	p1.y = cornerHeight[3];
	p1.z = maxZ;
	
	p2.x = minX;
	p2.y = cornerHeight[0];
	p2.z = minZ;
	
	Plane plane2(p0, p1, p2);
	if (plane2.ContainsSomeOf(extents))
	{
		//
		//-------------------------------------------------------------------
		// Make sure the XZ projections of the triangle and the box intersect
		//-------------------------------------------------------------------
		//
		return True;
	}
	return False;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	RightHandedTile::ContainsBounded(const Point3D &point)
{
	Check(this);
	Check(&point);

	Verify(maxY >= point.y);
	return FindDistanceBelowBounded(point) <= 0.0f;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	RightHandedTile::FindDistanceBelowBounded(const Point3D &point)
{
	Check(this);
	Check(&point);

	Verify(minX <= point.x);
	Verify(maxX >= point.x);
	Verify(minY <= point.y);
	Verify(minZ <= point.z);
	Verify(maxZ >= point.z);

	//
	//---------------------------------------------------
	// Figure out which triangle the point will reside in
	//---------------------------------------------------
	//	
	Scalar rise = maxX - minX;
	Scalar run = maxZ - minZ;
	Verify(rise > SMALL);
	Verify(run > SMALL);

	Scalar dx = point.x - minX;
	Scalar dz = point.z - minZ;

	//
	//-----------------------------------------------------------------
	// Set up the appropriate triangle based upon which have it lies in
	//-----------------------------------------------------------------
	//	
	Point3D p0,p1,p2;
	if (dx*run > dz*rise)
	{
		p0.x = maxX;
		p0.y = cornerHeight[1];
		p0.z = minZ;

		p1.x = minX;
		p1.y = cornerHeight[0];
		p1.z = minZ;

		p2.x = maxX;
		p2.y = cornerHeight[3];
		p2.z = maxZ;
	}
	else
	{
		p0.x = minX;
		p0.y = cornerHeight[2];
		p0.z = maxZ;

		p1.x = maxX;
		p1.y = cornerHeight[3];
		p1.z = maxZ;
		
		p2.x = minX;
		p2.y = cornerHeight[0];
		p2.z = minZ;
	}

	//
	//---------------------------------------------------------------------
	// Make a plane out of the triangle, and have the plane solve for the Y
	// coordinate
	//---------------------------------------------------------------------
	//
	Plane plane(p0, p1, p2);
	Verify(!Small_Enough(plane.normal.y));
	Scalar height = point.y - plane.CalculateY(point.x, point.z);
	Check_Fpu();
	return height;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
static Scalar
	LineHitsTriangle(
		Line *line,
		const Point3D &p0,
		const Point3D &p1,
		const Point3D &p2,
		Scalar *cosine
	)
{
	//
	//--------------------------------------------------------------------------
	// Make the plane out of the three corner points, and figure out how far the
	// ray must travel to reach this plane.  Try some trivial rejections:
	// parallel lines, lines starting outside the halfspace heading away from
	// the plane, and lines starting outside the halfspace and heading towards
	// the plane but are too far away
	//--------------------------------------------------------------------------
	//
	Plane plane(p0, p1, p2);
	Scalar length = line->DistanceTo(plane, cosine);
	if (
		Small_Enough(*cosine)
		 || *cosine > 0.0f && length < 0.0f
		 || *cosine < 0.0f && length > line->length
	)
	{
		return -1.0f;
	}

	//
	//--------------------------------------------------------
	// Project the impact point and triangle unto the XZ plane
	//--------------------------------------------------------
	//	
	Point3D impact;	
	line->Project(length, &impact);
	Vector2DOf<Scalar> proj(impact.x - p0.x, impact.z - p0.z);
	Scalar x = p1.x - p0.x;
	Scalar z = p2.z - p0.z;

	//
	//-------------------------------------------------------------------------
	// Make sure that the area of the triangle made with the test point and the
	// first leg is not negative or greater than the area of the triangle made
	// by the two legs.  The area of the triangle is half the cross product of
	// the legs of that triangle
	//-------------------------------------------------------------------------
	//
	Scalar area_ratio = z*x;
	Verify(!Small_Enough(area_ratio));
	area_ratio = x*proj.y / area_ratio;
	Check_Fpu();
	if (area_ratio >= 0.0f && area_ratio <= 1.0f)
	{
		//
		//----------------------------------------------------------------------
		// The area ratio represents the height of the test triangle relative to
		// the given triangle.  One edge of the value is represented by
		// projecting the second leg a percentage equal to the ratio.  The bounds
		// of its variance is 1-area_ratio * the first leg of the triangle
		//----------------------------------------------------------------------
		//
		Scalar span = proj.x / x;
		Check_Fpu();
		if (span >= 0.0f && span+area_ratio <= 1.0f)
		{
			return length;
		}
	}
	return -1.0f;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	RightHandedTile::HitByBounded(
		Line *line,
		Scalar enters,
		Scalar leaves
	)
{
	Check(this);
	Check(line);

	Verify(enters <= leaves);
	Verify(leaves >= 0.0f);

	//
	//----------------------------------------------
	// See if the line hits the upper-right triangle
	//----------------------------------------------
	//	
	Point3D p0,p1,p2;
	p0.x = maxX;
	p0.y = cornerHeight[1];
	p0.z = minZ;

	p1.x = minX;
	p1.y = cornerHeight[0];
	p1.z = minZ;

	p2.x = maxX;
	p2.y = cornerHeight[3];
	p2.z = maxZ;

	Scalar cosine;
	Scalar length = LineHitsTriangle(line, p0, p1, p2, &cosine);

	//
	//--------------------------------------------------------------
	// If we are entering the the plane, set the new entering length
	//--------------------------------------------------------------
	//
	if (length >= 0.0f && cosine < 0.0f && length >= enters && length <= leaves)
	{
		line->length = length;
		return True;
	}

	//
	//---------------------------------------------
	// See if the line hits the lower left triangle
	//---------------------------------------------
	//
	p0.x = minX;
	p0.y = cornerHeight[2];
	p0.z = maxZ;

	p1.x = maxX;
	p1.y = cornerHeight[3];
	p1.z = maxZ;
	
	p2.x = minX;
	p2.y = cornerHeight[0];
	p2.z = minZ;
	
	length = LineHitsTriangle(line, p0, p1, p2, &cosine);

	//
	//--------------------------------------------------------------
	// If we are entering the the plane, set the new entering length
	//--------------------------------------------------------------
	//
	if (length >= 0.0f && cosine < 0.0f && length >= enters && length <= leaves)
	{
		line->length = length;
		return True;
	}

	//
	//-------------------------------------------
	// Neither triangle was entered, so we missed
	//-------------------------------------------
	//
	return False;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	RightHandedTile::TestInstance() const
{
	return solidType == RightHandedTileType;
}

//#############################################################################
//#########################    LeftHandedTile    ######################
//#############################################################################

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
LeftHandedTile::LeftHandedTile(
	const ExtentBox &extents,
	BoxedSolid::Material material,
	Simulation *owner,
	BoxedSolid *next_solid,
	Scalar *corners
):
	RightHandedTile(
		extents,
		material,
		owner,
		next_solid,
		corners,
		LeftHandedTileType
	)
{
	Check_Pointer(this);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
LeftHandedTile::~LeftHandedTile()
{
	Check_Pointer(this);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	LeftHandedTile::IntersectsBounded(const ExtentBox &extents)
{
	Check(this);
	Check(&extents);

	Verify(minX <= extents.minX);
	Verify(maxX >= extents.maxX);
	Verify(minY <= extents.minY);
	Verify(maxY >= extents.maxY);
	Verify(minZ <= extents.minZ);
	Verify(maxZ >= extents.maxZ);

	//
	//-------------------------------------------------------------------
	// See if the box hits the upper-left triangle anywhere on its plane
	//-------------------------------------------------------------------
	//	
	Point3D p0,p1,p2;
	p0.x = maxX;
	p0.y = cornerHeight[2];
	p0.z = minZ;

	p1.x = minX;
	p1.y = cornerHeight[1];
	p1.z = minZ;

	p2.x = maxX;
	p2.y = cornerHeight[0];
	p2.z = maxZ;

	Plane plane1(p0, p1, p2);
	if (plane1.ContainsSomeOf(extents))
	{
		//
		//-------------------------------------------------------------------
		// Make sure the XZ projections of the triangle and the box intersect
		//-------------------------------------------------------------------
		//
		return True;
	}

	//
	//------------------------------------------------------------------
	// See if the box hits the lower-right triangle anywhere on its plane
	//------------------------------------------------------------------
	//
	p0.x = minX;
	p0.y = cornerHeight[1];
	p0.z = maxZ;

	p1.x = maxX;
	p1.y = cornerHeight[2];
	p1.z = maxZ;
	
	p2.x = minX;
	p2.y = cornerHeight[3];
	p2.z = minZ;
	
	Plane plane2(p0, p1, p2);
	if (plane2.ContainsSomeOf(extents))
	{
		//
		//-------------------------------------------------------------------
		// Make sure the XZ projections of the triangle and the box intersect
		//-------------------------------------------------------------------
		//
		return True;
	}
	return False;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	LeftHandedTile::FindDistanceBelowBounded(const Point3D &point)
{
	Check(this);
	Check(&point);

	Verify(minX <= point.x);
	Verify(maxX >= point.x);
	Verify(minY <= point.y);
	Verify(minZ <= point.z);
	Verify(maxZ >= point.z);

	//
	//---------------------------------------------------
	// Figure out which triangle the point will reside in
	//---------------------------------------------------
	//	
	Scalar rise = maxX - minX;
	Scalar run = maxZ - minZ;
	Verify(rise > SMALL);
	Verify(run > SMALL);

	Scalar dx = point.x - minX;	// HACK - needs to be set up for other diagonal
	Scalar dz = point.z - minZ;

	//
	//-----------------------------------------------------------------
	// Set up the appropriate triangle based upon which have it lies in
	//-----------------------------------------------------------------
	//	
	Point3D p0,p1,p2;
	if (dx*run > dz*rise)
	{
		p0.x = maxX;
		p0.y = cornerHeight[1];
		p0.z = minZ;

		p1.x = minX;
		p1.y = cornerHeight[0];
		p1.z = minZ;

		p2.x = maxX;
		p2.y = cornerHeight[3];
		p2.z = maxZ;
	}
	else
	{
		p0.x = minX;
		p0.y = cornerHeight[2];
		p0.z = maxZ;

		p1.x = maxX;
		p1.y = cornerHeight[3];
		p1.z = maxZ;
		
		p2.x = minX;
		p2.y = cornerHeight[0];
		p2.z = minZ;
	}

	//
	//---------------------------------------------------------------------
	// Make a plane out of the triangle, and have the plane solve for the Y
	// coordinate
	//---------------------------------------------------------------------
	//
	Plane plane(p0, p1, p2);
	Verify(!Small_Enough(plane.normal.y));
	Scalar height = point.y - plane.CalculateY(point.x, point.z);
	Check_Fpu();
	return height;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	LeftHandedTile::HitByBounded(
		Line *line,
		Scalar enters,
		Scalar leaves
	)
{
	Check(this);
	Check(line);

	Verify(enters <= leaves);
	Verify(leaves >= 0.0f);

	//
	//----------------------------------------------
	// See if the line hits the upper-right triangle
	//----------------------------------------------
	//	
	Point3D p0,p1,p2;
	p0.x = maxX;
	p0.y = cornerHeight[2];
	p0.z = minZ;

	p1.x = minX;
	p1.y = cornerHeight[1];
	p1.z = minZ;

	p2.x = maxX;
	p2.y = cornerHeight[0];
	p2.z = maxZ;

	Scalar cosine;
	Scalar length = LineHitsTriangle(line, p0, p1, p2, &cosine);

	//
	//--------------------------------------------------------------
	// If we are entering the the plane, set the new entering length
	//--------------------------------------------------------------
	//
	if (length >= 0.0f && cosine < 0.0f && length >= enters && length <= leaves)
	{
		line->length = length;
		return True;
	}

	//
	//---------------------------------------------
	// See if the line hits the lower left triangle
	//---------------------------------------------
	//
	p0.x = minX;
	p0.y = cornerHeight[1];
	p0.z = maxZ;

	p1.x = maxX;
	p1.y = cornerHeight[2];
	p1.z = maxZ;
	
	p2.x = minX;
	p2.y = cornerHeight[3];
	p2.z = minZ;
	
	length = LineHitsTriangle(line, p0, p1, p2, &cosine);

	//
	//--------------------------------------------------------------
	// If we are entering the the plane, set the new entering length
	//--------------------------------------------------------------
	//
	if (length >= 0.0f && cosine < 0.0f && length >= enters && length <= leaves)
	{
		line->length = length;
		return True;
	}

	//
	//-------------------------------------------
	// Neither triangle was entered, so we missed
	//-------------------------------------------
	//
	return False;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	LeftHandedTile::TestInstance() const
{
	return solidType == LeftHandedTileType;
}