#include "munga.h"
#pragma hdrstop

#include "boxsolid.h"
#include "line.h"

//#############################################################################
//###############################    BoxedCone    #############################
//#############################################################################

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
BoxedCone::BoxedCone(
	const ExtentBox &extents,
	BoxedSolid::Material material,
	Simulation *owner,
	BoxedSolid *next_solid
):
	BoxedSolid(extents, ConeType, material, owner, next_solid)
{
	Check_Pointer(this);
}

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

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	BoxedCone::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);

	//
	//------------------------------------------------------------------------
	// Find the center point in the XZ plane of the cone, and find the biggest
	// radius of the cone by measuring in the X axis, along with its height
	//------------------------------------------------------------------------
	//
	Vector3D center;
	center.x = (minX + maxX) * 0.5f;
	center.y = minY;
	center.z = (minZ + maxZ) * 0.5f;
	Scalar radius = maxX - center.x;
	Verify(!Small_Enough(radius));

	//
	//--------------------------------------------------------------------------
	// Convert the closest point in the slice to the coordinates of the cone,
	// putting the apex of the cone at the origin, then make sure that the point
	// isn't in one of the corners of the box
	//--------------------------------------------------------------------------
	//
	Vector3D nearest(center);
	extents.Constrain(&nearest);
	center.y = maxY;
	nearest.Subtract(center,nearest);
	Scalar r = nearest.x*nearest.x + nearest.z*nearest.z;
	if (r > radius*radius)
	{
		return False;
	}

	//
	//--------------------------------------------------------------------------
	// Compute the distance from the axis, and then see if the slope of the line
	// from the cone apex to the test point is less than the slope of the cone
	//--------------------------------------------------------------------------
	//
	r = Sqrt(r);
	Scalar height = maxY - minY;
	return r*height <= nearest.y*radius;
}

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

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

	//
	//------------------------------------------------------------------------
	// Find the center point in the XZ plane of the cone, and find the biggest
	// radius of the cone by measuring in the X axis, along with its height
	//------------------------------------------------------------------------
	//
	Vector3D center;
	center.x = (minX + maxX) * 0.5f;
	center.y = maxY;
	center.z = (minZ + maxZ) * 0.5f;
	Scalar radius = maxX - center.x;
	Verify(!Small_Enough(radius));

	//
	//--------------------------------------------------------------------------
	// Convert the closest point in the slice to the coordinates of the cone,
	// putting the apex of the cone at the origin, then make sure that the point
	// isn't in one of the corners of the box
	//--------------------------------------------------------------------------
	//
	Vector3D nearest;
	nearest.Subtract(center, point);
	Scalar r = nearest.x*nearest.x + nearest.z*nearest.z;
	if (r > radius*radius)
	{
		return False;
	}

	//
	//--------------------------------------------------------------------------
	// Compute the distance from the axis, and then see if the slope of the line
	// from the cone apex to the test point is less than the slope of the cone
	//--------------------------------------------------------------------------
	//
	r = Sqrt(r);
	Scalar height = maxY - minY;
	return r*height <= nearest.y*radius;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	BoxedCone::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);

	//
	//------------------------------------------------------------------------
	// Find the center point in the XZ plane of the cone, and find the biggest
	// radius of the cone by measuring in the X axis, along with its height
	//------------------------------------------------------------------------
	//
	Vector3D center;
	center.x = (minX + maxX) * 0.5f;
	center.y = maxY;
	center.z = (minZ + maxZ) * 0.5f;
	Scalar radius = maxX - center.x;
	Verify(!Small_Enough(radius));

	//
	//--------------------------------------------------------------------------
	// Convert the closest point in the slice to the coordinates of the cone,
	// putting the apex of the cone at the origin, then make sure that the point
	// isn't in one of the corners of the box
	//--------------------------------------------------------------------------
	//
	Vector3D nearest;
	nearest.Subtract(point, center);      
	Scalar r = nearest.x*nearest.x + nearest.z*nearest.z;
	if (r > radius*radius)
	{
		return -1.0f;
	}

	//
	//--------------------------------------------------------------------------
	// Compute the distance from the axis, and then see if the slope of the line
	// from the cone apex to the test point is less than the slope of the cone
	//--------------------------------------------------------------------------
	//
	r = Sqrt(r);
	Scalar height = maxY - minY;
	nearest.y += r*(height/radius);
	return Max(nearest.y,0.0f);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
// This function is a helper function used in the derivation for the terms of
// the quadratic equation to find t when colliding a line and a cone.  I'm not
// real clear on the physical representation of this kind of dot product
//
static Scalar
	Special_Cone_Dot_Product(
		const Vector3D &v1,
		const Vector3D &v2,
		Scalar squared_tan
	)
{
	return v1.x*v2.x - squared_tan*v1.y*v2.y + v1.z*v2.z;
}

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

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

	//
	//--------------------------------------------------------------------------
	// Find out the size of the box, and set up the height and maximum radius of
	// the cone, along with the squared tangent of the spread angle
	//--------------------------------------------------------------------------
	//
	Vector3D center;
	center.x = (minX + maxX) * 0.5f;
	center.y = maxY;
	center.z = (minZ + maxZ) * 0.5f;
	Scalar radius = maxX - center.x;
	Scalar height = maxY - minY;
	Verify(!Small_Enough(height));
	Scalar squared_tan = radius*radius/(height*height);

	//
	//-------------------------------------------------------------------------
	// Find the line between the point of the cone and the origin of our line,
	// then set up the conditions for the quadratic equation.  The following
	// equation sets up a*t^2 + b*t + c = 0.  The terms a, b, and c are derived
	// by solving the following equations for t:
	//
	//   p == line->origin + line->direction * t
	//   r == p - cone->apex
	//   r.x^2 + r.z^2 == tan^2(cone_angle)*r.y^2
	//
	// The special cone dot product function is a handy way to reduce the
	// complexity of the problem
	//-------------------------------------------------------------------------
	//
	Vector3D v;
	v.Subtract(line->origin,center);

	//
	//--------------------------------------------------------------------------
	// If the line diverges from the axis at the same angle as the spread angle,
	// we need to find the closest point on the line to the axis.  We then drop
	// a vertical plane containing the line through the cone at this point, thus
	// generating a hyperbola.  We then solve the hyperbola vs. line equation to
	// get our intersection point
	//--------------------------------------------------------------------------
	//
	Scalar a =
		Special_Cone_Dot_Product(line->direction, line->direction, squared_tan);
	if (Small_Enough(a))
	{
	}

	//
	//-----------------------------------------------
	// Otherwise, continue solving the first equation
	//-----------------------------------------------
	//
	else
	{
		Scalar b =
			2.0f * Special_Cone_Dot_Product(v, line->direction, squared_tan);
		Scalar c = Special_Cone_Dot_Product(v, v, squared_tan);

		//
		//-----------------------------------------------------------------------
		// Now, use the quadratic equation to determine where the two points
		// intersect the cone.  If there is no solution, than the line missed the
		// cone
		//-----------------------------------------------------------------------
		//
		Verify(!Small_Enough(a));
		Scalar t = -b / (2.0f * a);
		Scalar i = b*b - 4.0f*a*c;

		if (i < 0.0f)
		{
			return False;
		}

		//
		//----------------------------------------------------------------------
		// If the interval is zero, then the line hits the cone in one spot only
		// (a tangential hit).  Check to see if hits the lower half of the conic
		// equation (our cone)
		//----------------------------------------------------------------------
		//
		if (Small_Enough(i))
		{
			Scalar y = v.y + line->direction.y * t;
			if (y > 0.0f)
			{
				return False;
			}

			//
			//----------------------------------------------------------------
			// It hit the lower cone, so see if it was entering or leaving the
			// cone
			//----------------------------------------------------------------
			//
			if (line->direction.y > 0.0f)
			{
				//
				//-------------------------------------------------------------
				// The line is leaving the cone, so see if we need to reset the
				// leaving distance
				//-------------------------------------------------------------
				//
				if (t < leaves)
				{
					leaves = t;
				}
			}

			//
			//------------------------------------------------------------
			// The line is entering the cone, so see if we need to set the
			// entering distance
			//------------------------------------------------------------
			//
			else if (t > enters)
			{
				enters = t;
			}

			//
			//-------------------------------------------------------------------
			// See if we still have a collision with the cone, and if so, set the
			// new line length
			//-------------------------------------------------------------------
			//
			if (enters > leaves || enters > line->length || leaves < 0.0f)
			{
				return False;
			}
			line->length = Max(enters, 0.0f);
			return True;
		}

		//
		//----------------------------------------------------------------------
		// If the interval is non-zero, then the conic section was struck in two
		// places.  Find out the lengths to those places and the y values for
		// those points
		//----------------------------------------------------------------------
		//
		i = Sqrt(i) / (2.0f * a);
		Scalar t1,t2;
		if (i > 0.0f)
		{
			t1 = t - i;
			t2 = t + i;
		}
		else
		{
			t1 = t + i;
			t2 = t - i;
		}
		Scalar y1 = v.y + line->direction.y * t1;
		Scalar y2 = v.y + line->direction.y * t2;

		//
		//----------------------------------------------------------------------
		// There are four combinations of the signs of the y values.  Each has a
		// different effect on missing/hitting, entering or leaving distances.
		// First check to see if only the upper conic was hit
		//----------------------------------------------------------------------
		//
		if (y1 > 0.0f)
		{
			if (y2 > 0.0f)
			{
				return False;
			}

			//
			//-----------------------------------------------------------------
			// The ray is entering our conic, so set the distance appropriately
			//-----------------------------------------------------------------
			//
			if (t2 > enters)
			{
				enters = t2;
			}
		}

		//
		//----------------------------------------------------------------------
		// See if the ray is leaving the lower conic, and if so, set the leaving
		// distance accordingly
		//----------------------------------------------------------------------
		//
		else
		{
			if (y2 > 0.0f)
			{
				if (t1 < leaves)
				{
					leaves = t1;
				}
			}

			//
			//--------------------------------------------------------------------
			// Both spots are in the lower conic, so set both leaving and entering
			// distances
			//--------------------------------------------------------------------
			//
			else
			{
				if (t1 > enters)
				{
					enters = t1;
				}
				if (t2 < leaves)
				{
					leaves = t2;
				}
			}
		}

		//
		//-----------------------------------------------------------------------
		// See if we still have a collision with the cone, and if so, set the new
		// line length
		//-----------------------------------------------------------------------
		//
		if (enters > leaves || enters > line->length || leaves < 0.0f)
		{
			return False;
		}
		line->length = Max(enters, 0.0f);
	}
	return True;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	BoxedCone::TestInstance() const
{
	return solidType == ConeType;
}