//=======================================================================//
// File:		line.cpp                                                     //
// Project: Architecture                                                 //
// Author:	J.M. Albertson                                               //
//-----------------------------------------------------------------------//
// Copyright (C) 1994, Virtual World Entertainments, All Rights reserved //
// PROPRIETARY AND CONFIDENTIAL                                          //
//=======================================================================//

#include "StuffHeaders.hpp"

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Line3D&
	Line3D::SetDirection(const Vector3D &vector)
{
	Check_Pointer(this);
	Check_Object(&vector);

	//
	//---------------------------------------
	// Make sure m_length of vector is non-zero
	//---------------------------------------
	//
	m_length = vector.GetLength();
	Verify(!Small_Enough(m_length));
	m_length = 1.0f / m_length;

	//
	//----------------------------------------------
	// Normalize the vector and put it into the line
	//----------------------------------------------
	//
	m_direction.x = vector.x*m_length;
	m_direction.y = vector.y*m_length;
	m_direction.z = vector.z*m_length;
	return *this;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	Line3D::GetDistanceTo(
		const Plane &plane,
		Scalar *product
	) const
{
	Check_Object(this);
	Check_Object(&plane);
	Check_Pointer(product);

	*product = m_direction * plane.normal;
	if (Small_Enough(*product))
		return -1.0f;
	Scalar result = -plane.GetDistanceTo(m_origin) / *product;
	return result;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	Line3D::GetDistanceTo(
		const Sphere &sphere,
		Scalar *penetration
	) const
{
	Check_Object(this);
	Check_Object(&sphere);
	Check_Pointer(penetration);

	//
	//-------------------------------------------------------------------
	// Determine if ray intersects bounding sphere of object.  If sphere
	// is (X-C)*(X-C) = R^2 and ray is X = t*D+L for t >= 0, then
	// intersection is obtained by plugging X into sphere equation to
	// get quadratic:  (D*D)t^2 + 2*(D*(L-C))t + (L-C)*(L-C) = 0
	// Define a = D*D = 1.0f, b = 2*(D*(L-C)), and c = (L-C)*(L-C).
	//-------------------------------------------------------------------
	//
	Vector3D diff;
	diff.Subtract(m_origin, sphere.center);
	Scalar b = (m_direction*diff) * 2.0f;
	Scalar c = (diff*diff) - sphere.radius*sphere.radius;

	//
	//-------------------------------------------------------------------------
	// If penetration is negative, we couldn't hit the sphere at all.  If it is
	// really small, it touches at only one place
	//-------------------------------------------------------------------------
	//
	*penetration = b*b - 4.0f*c;
	if (*penetration < -SMALL)
		return -1.0f;
	b *= -0.5f;
	if (*penetration<SMALL)
	{
		*penetration = 0.0f;
		Min_Clamp(b, 0.0f);
		return (b > m_length) ? -1.0f : b;
	}

	//
	//-------------------------------------------------------------
	// We know we hit the sphere, so figure out where it first hits
	//-------------------------------------------------------------
	//
	*penetration = 0.5f * Sqrt(*penetration);
	if (b + *penetration < -SMALL)
		return -1.0f;
	b -= *penetration;
	if (b > m_length)
		return -1.0f;
	Min_Clamp(b, 0.0f);
	return b;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	Line3D::GetDistanceTo(const OBB& box)
{
	Check_Object(this);
	Check_Object(&box);

	//
	//------------------------------------------------------------------------
	// Get the vector from the line to the centerpoint of the OBB.  All planes
	// will be generated relative to this
	//------------------------------------------------------------------------
	//
	Point3D center;
	center = box.localToParent;
	Vector3D delta;
	delta.Subtract(center, m_origin);

	//
	//--------------------------------------------------
	// Set up the loop to examine each of the three axes
	//--------------------------------------------------
	//
	Scalar enters = -100.0f - m_length;
	Scalar leaves = m_length + 100.0f;
	for (int axis=X_Axis; axis <= Z_Axis; ++axis)
	{
		UnitVector3D
			normal(
				box.localToParent(axis, X_Axis),
				box.localToParent(axis, Y_Axis),
				box.localToParent(axis, Z_Axis)
			);

		//
		//----------------------------------------------------------------------
		// Now, we have to calculate how far the line moves along the normal per
		// unit traveled down the line.  If it is perpendicular to the normal,
		// then it will hit or miss based solely upon the m_origin location
		//----------------------------------------------------------------------
		//
		Scalar drift = m_direction * normal;
		Scalar distance;
		if (Small_Enough(drift))
		{
			distance = delta * normal;
			if (Fabs(distance) > box.axisExtents[axis])
				return -1.0f;
			else
				continue;
		}

		//
		//--------------------------------------------------------------------
		// We know the line is not parallel, so we will now calculate how long
		// the line will stay inside the box.  We also will calculate how far
		// from the m_origin to the centerplane of the OBB
		//--------------------------------------------------------------------
		//
		drift = 1.0f / drift;
		Scalar span = box.axisExtents[axis] * Fabs(drift);
		distance = (delta * normal) * drift;

		//
		//--------------------------------------------------------------------
		// Now adjust where the line can enter and leave the OBB, and if it is
		// no longer possible to hit, stop checking
		//--------------------------------------------------------------------
		//
		Scalar enter = distance - span;
		Scalar leave = distance + span;
		if (enter > enters)
			enters = enter;
		if (leave < leaves)
			leaves = leave;
		if (enters > leaves)
			return -1.0f;
	}

	//
	//-------------------------------------------------------------------------
	// If we got here, then the line in theory can hit the OBB, so now we check
	// to make sure it hits it within the allowed span of the line
	//-------------------------------------------------------------------------
	//
	if (leaves < 0.0f || enters > m_length)
		return -1.0f;
	Min_Clamp(enters, 0.0f);
	return enters;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	Line3D::GetDistanceTo(
		const OBB& box,
		int *first_axis
	)
{
	Check_Object(this);
	Check_Object(&box);
	Check_Pointer(first_axis);

	//
	//------------------------------------------------------------------------
	// Get the vector from the line to the centerpoint of the OBB.  All planes
	// will be generated relative to this
	//------------------------------------------------------------------------
	//
	Point3D center;
	center = box.localToParent;
	Vector3D delta;
	delta.Subtract(center, m_origin);

	//
	//--------------------------------------------------
	// Set up the loop to examine each of the three axes
	//--------------------------------------------------
	//
	Scalar enters = -100.0f - m_length;
	Scalar leaves = m_length + 100.0f;
	for (int axis=X_Axis; axis <= Z_Axis; ++axis)
	{
		UnitVector3D
			normal(
				box.localToParent(axis, X_Axis),
				box.localToParent(axis, Y_Axis),
				box.localToParent(axis, Z_Axis)
			);

		//
		//----------------------------------------------------------------------
		// Now, we have to calculate how far the line moves along the normal per
		// unit traveled down the line.  If it is perpendicular to the normal,
		// then it will hit or miss based solely upon the m_origin location
		//----------------------------------------------------------------------
		//
		Scalar drift = m_direction * normal;
		Scalar distance;
		if (Small_Enough(drift))
		{
			distance = delta * normal;
			if (Fabs(distance) > box.axisExtents[axis])
				return -1.0f;
			else
				continue;
		}

		//
		//--------------------------------------------------------------------
		// We know the line is not parallel, so we will now calculate how long
		// the line will stay inside the box.  We also will calculate how far
		// from the m_origin to the centerplane of the OBB
		//--------------------------------------------------------------------
		//
		drift = 1.0f / drift;
		Scalar span = box.axisExtents[axis] * Fabs(drift);
		distance = (delta * normal) * drift;

		//
		//--------------------------------------------------------------------
		// Now adjust where the line can enter and leave the OBB, and if it is
		// no longer possible to hit, stop checking
		//--------------------------------------------------------------------
		//
		Scalar enter = distance - span;
		Scalar leave = distance + span;
		if (enter > enters)
		{
			*first_axis = axis;
			enters = enter;
		}
		if (leave < leaves)
			leaves = leave;
		if (enters > leaves)
			return -1.0f;
	}

	//
	//-------------------------------------------------------------------------
	// If we got here, then the line in theory can hit the OBB, so now we check
	// to make sure it hits it within the allowed span of the line
	//-------------------------------------------------------------------------
	//
	if (leaves < 0.0f || enters > m_length)
		return -1.0f;
	Min_Clamp(enters, 0.0f);
	return enters;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	Stuff::Find_Time_Till_Closest_Approach(
		const Point3D& origin1,
		const Vector3D& velocity1,
		const Point3D& origin2,
		const Vector3D& velocity2
	)
{
	Vector3D a,b;
	a.Subtract(origin1, origin2);
	b.Subtract(velocity1, velocity2);

	//
	//--------------------------------------------------------------------
	// If the velocities are identical, any point will do for the test, so
	// return time zero
	//--------------------------------------------------------------------
	//
	Scalar d = b.GetLengthSquared();
	if (Small_Enough(d))
		return 0.0f;

	//
	//-------------------------------------------------------------------------
	// The equation representing the difference in the lines is a+bt. If we dot
	// this equation with itself, we get a function representing the squared
	// distances between the lines = aa + 2tab + ttbb.  The derivative of this
	// function with respect to t is 2ab + 2tbb.  The closest approach is when
	// the derivative is zero, or when t = -a*b / b*b
	//-------------------------------------------------------------------------
	//	
	return (a * b) / -d;
}