#include "munga.h"
#pragma hdrstop

#include "ray.h"
#include "plane.h"
#include "sphere.h"

//
//#############################################################################
//#############################################################################
//
void Ray::Project(Scalar length, Point3D *result)
{
	Check(this);
	Check(result);

	Vector3D temp;

	temp.Multiply(direction,length);
	result->Add(origin,temp);
}

//
//#############################################################################
//#############################################################################
//
Scalar Ray::LengthToClosestPointTo(const Point3D &point)
{
	Check(this);
	Check(&point);

	Vector3D temp;

	temp.Subtract(point,origin);
	return temp*direction;
}

//
//#############################################################################
//#############################################################################
//
Scalar Ray::DistanceTo(const Plane &plane, Scalar *product) const
{
	Scalar t;

	//
	//------------------------------------------------------------------
	// Compute the dot product of the ray and plane normal, and find the
	// distance from the origin of the ray to the plane
	//------------------------------------------------------------------
	//
	*product = plane.normal * direction;
	t = plane.DistanceTo(origin);

	//
	//----------------------------------------------------------------------
	// If the ray is not parallel to the plane, determine how far to proceed
	// along the ray until we hit the plane
	//----------------------------------------------------------------------
	//
	if (!Small_Enough(*product))
		t /= -*product;
	return t;
}

//
//#############################################################################
//#############################################################################
//
Scalar Ray::DistanceTo(const Sphere &sphere, Scalar *penetration) const
{
	Scalar b, c;
	Vector3D temp;

	//
	//-------------------------------------------------------------------------
	// Set up to solve a quadratic equation for the intersection of the ray and
	// sphere.  The solution is based on finding the closest point on the line
	// to the sphere, and then calculating the interval between the entry and
	// exit points of the ray
	//-------------------------------------------------------------------------
	//
	temp.Subtract(origin,sphere.center);
	b = 2.0f * (direction * temp);
	c = temp.LengthSquared() - sphere.radius*sphere.radius;

	//
	//--------------------------------------------------------------------------
	// Compute the squared interval to use for the solution.  If it is negative,
	// then the ray misses the sphere
	//--------------------------------------------------------------------------
	//
	*penetration = b*b - 4.0f*c;
	if (*penetration<SMALL)
		return 0.0f;
	else
	{
		//-------------------------------------------------------------------------
		// Otherwise, find the linear distance along the line of the entry point by
		// subtracting half the interval between entry and exit points from the
		// distance to the closest point on the sphere
		//-------------------------------------------------------------------------
		*penetration = Sqrt(*penetration);
		return -0.5f*(b+*penetration);
	}
}

//
//#############################################################################
//#############################################################################
//
Logical Ray::TestInstance() const
{
	return true;
}

//
//#############################################################################
//#############################################################################
//
Scalar Find_Closest_Approach(const Point3D& origin1, const Vector3D& velocity1, Point3D *result1, const Point3D& origin2, const Vector3D& velocity2, Point3D *result2, Scalar *time, Logical *constant)
{
	Vector3D a,b;
	a.Subtract(origin1, origin2);
	b.Subtract(velocity1, velocity2);

	//
	//--------------------------------------------------------------------
	// If the velocities are identical, any point will do for the test, so
	// simply return the difference between the starting points
	//--------------------------------------------------------------------
	//
	Scalar d = b.LengthSquared();
	if (Small_Enough(d))
	{
		*constant = True;
		d = a.Length();
		Check_Fpu();
		return d;
	}

	//
	//-------------------------------------------------------------------------
	// The velocities are not parallel, so figure out when the closest approach
	// is via the derivative
	//-------------------------------------------------------------------------
	//	
	*constant = False;
	*time = (a * b) / -d;
	Check_Fpu();

	//
	//------------------------------------------------------
	// Now, plot the resultant points of both line equations
	//------------------------------------------------------
	//
	Vector3D closest;
	closest.AddScaled(a, b, *time);
	result1->AddScaled(origin1, velocity1, *time);
	result2->AddScaled(origin2, velocity2, *time);
	d = closest.Length();
	Check_Fpu();
	return d;
}

#if defined(TEST_CLASS)
#	include "ray.tcp"
#endif