/*
	Trampoline Sim.
	Copyright (c) 2009, Darragh Coy.
	All rights reserved.
	Original work(s), source code & author(s) details available at: http://www.darraghcoy.com

	Redistribution and use in source and binary forms, with or without modification, are permitted 
	provided that the following conditions are met:

		* 	Redistributions of source code must retain the above copyright notice, this list of 
			conditions and the following disclaimer.
		* 	Redistributions in binary form must reproduce the above copyright notice, this list of 
			conditions and the following disclaimer in the documentation.
		* 	Neither the name of the author nor the names of contributors may be used to endorse or 
			promote products derived from this software without specific prior written permission.
		* 	The use of any third party libraries, modules or data files provided with the source 
			form of the program must follow and respect the individual licensing terms of these 
			components. You do not have permission to modify or circumvent the licensing terms for 
			provided third party components unless the third party licenses specifically allow this. 
			If you do not agree to the terms of provided third party components then you do not have 
			permission to use them in your derived work(s).
		* 	The terms of this license do not apply to separate (but related) distributions if supplied 
			by the author(s), only to the contents of this source distribution, derived works, and the 
			binaries produced by either modified or unmodified versions of the source code. The terms 
			of this license do not also apply to any third party libraries or components provided with 
			the source distribution. Such third party elements are licensed under their own separate 
			agreements.

	THIS SOFTWARE IS PROVIDED BY DARRAGH COY ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 
	BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
	ARE DISCLAIMED. IN NO EVENT SHALL DARRAGH COY BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
	EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 
	OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY 
	OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
	IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

//=====================================================================================================================
/// @brief		This shape represents a box shape
//=====================================================================================================================

#include "Precompiled.h"
#include "Physics/ShapeBox.h"
#include "Physics/ShapePlane.h"
#include "Physics/ShapeHeightfield.h"
#include "Physics/SimulationWorld.h"
#include "Game/GameCore.h"
#include "Graphics/Camera3D.h"
#include "Graphics/BufferedLineRenderer.h"

using namespace Game;

//=====================================================================================================================
/// @brief		Generates a list of collision contacts with the other shape
/// @param		other - other shape to generate a list of for 
/// @param		contacts - where to save the resulting contacts
//=====================================================================================================================

void ShapeBox::GenerateContacts( const Shape & other , std::vector<Contact> & contacts ) const
{
	// See what type of shape it is:

	switch ( other.GetType() )
	{
		case Shape::SHAPE_PLANE:
		{
			PlaneContacts( (ShapePlane&) other , contacts );

		}	break;
		
		case Shape::SHAPE_HEIGHTFIELD:
		{
			// Do the reverse way around: don't want to write this code twice
			
			((const ShapeHeightfield&)(other)).GenerateContacts(*this,contacts);
		
		}	break;
		
		case Shape::SHAPE_BOX:
		{
			BoxContacts( ((const ShapeBox&)(other)) , contacts );
		
		}	break;

		default:					
		{
			Errors::IssueError("Unsupported shape type to collide with");

		}	break;
	}
}

//=====================================================================================================================
/// @brief		Generates contacts for this shape with a box
/// @param		other - other shape to generate contacts with
/// @param		contacts - list where to save the contacts
//=====================================================================================================================

void ShapeBox::BoxContacts( const ShapeBox & other , std::vector<Contact> & contacts ) const
{
	// Get bounding sphere radiuses of both boxes:
	
	float bsr1 = GetBSRadius(); float bsr2 = other.GetBSRadius();
	
	// Get distance between both boxes:
	
	float dist = other.GetPosition().DistanceTo(GetPosition());
	
	// See if not close enough for potential contact:
	
	if ( dist > ( bsr1 + bsr2 ) * 2.0f ) return;

	// Do point face collisions:

	BoxPointFaceContacts(*this,other,contacts,true);
	BoxPointFaceContacts(other,*this,contacts,false);

	// Do edge-edge collisions:

	BoxEdgeEdgeContacts(*this,other,contacts);
}

//=====================================================================================================================
/// @brief		Generates contacts for this shape with a plane
/// @param		other - other shape to generate contacts with
/// @param		contacts - where to save the list of contacts
//=====================================================================================================================

void ShapeBox::PlaneContacts( const ShapePlane & other , std::vector<Contact> & contacts ) const
{
	// Transform all of our points into world space:

	Vec3f points[8]; GetWorldVertices(points);

	// Make sure there is room for 8 more elements:

	contacts.reserve( contacts.size() + 8 );

	// Get the normal of the plane:

	Vec3f plane_normal = other.GetNormal();

	// Get the distance of each point to the plane:

	for ( unsigned i = 0 ; i < 8 ; ++i )
	{
		// Get the distance to the plane 

		float dist = ( points[i] - other.GetPosition() ).Dot(plane_normal);

		// See if zero or less (penetration) 

		if ( dist <= 0 )
		{
			// Generate a contact

			Contact c; c.m_normal = plane_normal;

			// Save the position

			c.m_position = points[i];

			// Save the penetration distance

			c.m_penetration = -dist;

			// Add to the list of contacts

			contacts.push_back(c);
		}
	}
}

//=====================================================================================================================
/// @brief		Gets the transformed vertices of the box in world coordinates, saving them to the 
///				given array.
/// @param		vertices - array to save the vertices to
/// @pre		Given array cannot be null
/// @remarks	Vertices are returned in the following order:
/// @li			top back left
/// @li			top back right
/// @li			top front right
/// @li			top front left
/// @li			bottom back left
/// @li			bottom back right
/// @li			bottom front right
/// @li			bottom front left
//=====================================================================================================================

void ShapeBox::GetWorldVertices( Vec3f vertices[] ) const
{
	// Make sure ok

	ENSURE( vertices != 0 , "Given array cannot be null" )

	// Do rotations and translations:

	vertices[0] = GetOrientation().Transform( Vec3f( - m_size.x , m_size.y , - m_size.z ) ) + GetPosition();
	vertices[1] = GetOrientation().Transform( Vec3f( + m_size.x , m_size.y , - m_size.z ) ) + GetPosition();
	vertices[2] = GetOrientation().Transform( Vec3f( + m_size.x , m_size.y , + m_size.z ) ) + GetPosition();
	vertices[3] = GetOrientation().Transform( Vec3f( - m_size.x , m_size.y , + m_size.z ) ) + GetPosition();

	vertices[4] = GetOrientation().Transform( Vec3f( - m_size.x , - m_size.y , - m_size.z ) ) + GetPosition();
	vertices[5] = GetOrientation().Transform( Vec3f( + m_size.x , - m_size.y , - m_size.z ) ) + GetPosition();
	vertices[6] = GetOrientation().Transform( Vec3f( + m_size.x , - m_size.y , + m_size.z ) ) + GetPosition();
	vertices[7] = GetOrientation().Transform( Vec3f( - m_size.x , - m_size.y , + m_size.z ) ) + GetPosition();
}

//=====================================================================================================================
/// @brief		Raycasts against this shape and returns the result
/// @param		ray - ray to cast against the shape
/// @returns	Result of the raycast
//=====================================================================================================================

RaycastResult ShapeBox::Raycast( Ray & ray ) const
{
	// Get the world vertices of the box:

	Vec3f points[8]; GetWorldVertices(points);

	// Generate the normals for the 6 sides of the box:

	Vec3f n_fr = + Vec3f::UnitZ();	// front
	Vec3f n_bk = - Vec3f::UnitZ();	// back
	Vec3f n_tp = + Vec3f::UnitY();  // top
	Vec3f n_bt = - Vec3f::UnitY();	// bottom
	Vec3f n_rt = + Vec3f::UnitX();	// right
	Vec3f n_lf = - Vec3f::UnitX();	// left

	// Generate normalised edge vectors for all 6 sides of the box

	Vec3f n_frbk_e1 = Vec3f::UnitX();		// front and back edge 1: from bottom left point on each face
	Vec3f n_frbk_e2 = Vec3f::UnitY();		// front and back edge 2: from bottom left point on each face
	Vec3f n_tpbt_e1 = Vec3f::UnitX();		// top and bottom edge 1: from back left point on each face
	Vec3f n_tpbt_e2 = Vec3f::UnitZ();		// top and bottom edge 2: from back left point on each face
	Vec3f n_rtlf_e1 = Vec3f::UnitY();		// left and right edge 1: from back bottom point on each face
	Vec3f n_rtlf_e2 = Vec3f::UnitZ();		// left and right edge 2: from back bottom point on each face

	// Rotate all the normals and edges:

	n_fr = GetOrientation().Transform(n_fr);
	n_bk = GetOrientation().Transform(n_bk);
	n_tp = GetOrientation().Transform(n_tp);
	n_bt = GetOrientation().Transform(n_bt);
	n_rt = GetOrientation().Transform(n_rt);
	n_lf = GetOrientation().Transform(n_lf);

	n_frbk_e1 = GetOrientation().Transform(n_frbk_e1);
	n_frbk_e2 = GetOrientation().Transform(n_frbk_e2);
	n_tpbt_e1 = GetOrientation().Transform(n_tpbt_e1);
	n_tpbt_e2 = GetOrientation().Transform(n_tpbt_e2);
	n_rtlf_e1 = GetOrientation().Transform(n_rtlf_e1);
	n_rtlf_e2 = GetOrientation().Transform(n_rtlf_e2);

	// Make a vector containing the intersections of the ray with all of the planes:

	std::vector<RaycastResult> plane_intersections; plane_intersections.reserve(8);

	// TOP PLANE INTERSECTION

	{
		float n_dot_v = n_tp.Dot(ray.m_direction);

		// Only if this is not zero: co-linear case. Also if the ray is not behind the plane.

		if ( Float::Abs( n_dot_v ) > 0.00000001f )
		{
			// Calculate time of intersection:

			float t = ( - n_tp.Dot(ray.m_start) + n_tp.Dot(points[0]) ) / n_dot_v;

			// Only if the time of hit is >= 0

			if ( t >= 0 )
			{
				// Calculate point of intersection:

				Vec3f ip = ray.m_start + ray.m_direction * t;

				// Get the dot with the two edges to see how far along the edges the point is

				float e1dot = ( ip - points[0] ).Dot(n_tpbt_e1);
				float e2dot = ( ip - points[0] ).Dot(n_tpbt_e2);

				// Now see if the hit was actually on this face:

				if 
				( 
					e1dot >= 0 && e1dot <= m_size.x * 2.0f 
					&&
					e2dot >= 0 && e2dot <= m_size.z * 2.0f 
				)
				{
					// Add to results

					plane_intersections.push_back( RaycastResult( true , ip	, t	, n_tp ) );
				}
			}
		}
	}

	// BOTTOM PLANE INTERSECTION

	{
		float n_dot_v = n_bt.Dot(ray.m_direction);

		// Only if this is not zero: co-linear case. Also if the ray is not behind the plane.

		if ( Float::Abs( n_dot_v ) > 0.00000001f )
		{
			// Calculate time of intersection:

			float t = ( - n_bt.Dot(ray.m_start) + n_bt.Dot(points[4]) ) / n_dot_v;

			// Only if the time of hit is >= 0

			if ( t >= 0 )
			{
				// Calculate point of intersection:

				Vec3f ip = ray.m_start + ray.m_direction * t;

				// Get the dot with the two edges to see how far along the edges the point is

				float e1dot = ( ip - points[4] ).Dot(n_tpbt_e1);
				float e2dot = ( ip - points[4] ).Dot(n_tpbt_e2);

				// Now see if the hit was actually on this face:

				if
				( 
					e1dot >= 0 && e1dot <= m_size.x * 2.0f 
					&&
					e2dot >= 0 && e2dot <= m_size.z * 2.0f 
				)
				{
					// Add to results

					plane_intersections.push_back( RaycastResult( true , ip	, t	, n_bt ) );
				}
			}
		}
	}

	// BACK PLANE INTERSECTION

	{
		float n_dot_v = n_bk.Dot(ray.m_direction);

		// Only if this is not zero: co-linear case. Also if the ray is not behind the plane.

		if ( Float::Abs( n_dot_v ) > 0.00000001f )
		{
			// Calculate time of intersection:

			float t = ( - n_bk.Dot(ray.m_start) + n_bk.Dot(points[0]) ) / n_dot_v;

			// Only if the time of hit is >= 0

			if ( t >= 0 )
			{
				// Calculate point of intersection:

				Vec3f ip = ray.m_start + ray.m_direction * t;

				// Get the dot with the two edges to see how far along the edges the point is

				float e1dot = ( ip - points[4] ).Dot(n_frbk_e1);
				float e2dot = ( ip - points[4] ).Dot(n_frbk_e2);

				// Now see if the hit was actually on this face:

				if
				( 
					e1dot >= 0 && e1dot <= m_size.x * 2.0f 
					&&
					e2dot >= 0 && e2dot <= m_size.y * 2.0f 
				)
				{
					// Add to results

					plane_intersections.push_back( RaycastResult( true , ip	, t	, n_bk ) );
				}
			}
		}
	}

	// FRONT PLANE INTERSECTION

	{
		// Top plane:

		float n_dot_v = n_fr.Dot(ray.m_direction);

		// Only if this is not zero: co-linear case. Also if the ray is not behind the plane.

		if ( Float::Abs( n_dot_v ) > 0.00000001f )
		{
			// Calculate time of intersection:

			float t = ( - n_fr.Dot(ray.m_start) + n_fr.Dot(points[3]) ) / n_dot_v;

			// Only if the time of hit is >= 0

			if ( t >= 0 )
			{
				// Calculate point of intersection:

				Vec3f ip = ray.m_start + ray.m_direction * t;

				// Get the dot with the two edges to see how far along the edges the point is

				float e1dot = ( ip - points[7] ).Dot(n_frbk_e1);
				float e2dot = ( ip - points[7] ).Dot(n_frbk_e2);

				// Now see if the hit was actually on this face:

				if
				( 
					e1dot >= 0 && e1dot <= m_size.x * 2.0f 
					&&
					e2dot >= 0 && e2dot <= m_size.y * 2.0f 
				)
				{
					// Add to results

					plane_intersections.push_back( RaycastResult( true , ip	, t	, n_fr ) );
				}	
			}
		}
	}

	// LEFT PLANE INTERSECTION

	{
		// Left plane:

		float n_dot_v = n_lf.Dot(ray.m_direction);

		// Only if this is not zero: co-linear case. Also if the ray is not behind the plane.

		if ( Float::Abs( n_dot_v ) > 0.00000001f )
		{
			// Calculate time of intersection:

			float t = ( - n_lf.Dot(ray.m_start) + n_lf.Dot(points[0]) ) / n_dot_v;

			// Only if the time of hit is >= 0

			if ( t >= 0 )
			{
				// Calculate point of intersection:

				Vec3f ip = ray.m_start + ray.m_direction * t;

				// Get the dot with the two edges to see how far along the edges the point is

				float e1dot = ( ip - points[4] ).Dot(n_rtlf_e1);
				float e2dot = ( ip - points[4] ).Dot(n_rtlf_e2);

				// Now see if the hit was actually on this face:

				if
				( 
					e1dot >= 0 && e1dot <= m_size.y * 2.0f 
					&&
					e2dot >= 0 && e2dot <= m_size.z * 2.0f 
				)
				{
					// Add to results

					plane_intersections.push_back( RaycastResult( true , ip	, t	, n_lf ) );
				}		
			}
		}
	}

	// RIGHT PLANE INTERSECTION

	{
		// Right plane:

		float n_dot_v = n_rt.Dot(ray.m_direction);

		// Only if this is not zero: co-linear case. Also if the ray is not behind the plane.

		if ( Float::Abs( n_dot_v ) > 0.00000001f )
		{
			// Calculate time of intersection:

			float t = ( - n_rt.Dot(ray.m_start) + n_rt.Dot(points[1]) ) / n_dot_v;

			// Only if the time of hit is >= 0

			if ( t >= 0 )
			{
				// Calculate point of intersection:

				Vec3f ip = ray.m_start + ray.m_direction * t;

				// Get the dot with the two edges to see how far along the edges the point is

				float e1dot = ( ip - points[5] ).Dot(n_rtlf_e1);
				float e2dot = ( ip - points[5] ).Dot(n_rtlf_e2);

				// Now see if the hit was actually on this face:

				if
				( 
					e1dot >= 0 && e1dot <= m_size.y * 2.0f 
					&&
					e2dot >= 0 && e2dot <= m_size.z * 2.0f 
				)
				{
					// Add to results

					plane_intersections.push_back( RaycastResult( true , ip	, t	, n_rt ) );
				}			
			}
		}
	}

	// Find the penetration that is closest

	if ( plane_intersections.size() > 0 )
	{
		// Assume this is the closest first

		RaycastResult closest = plane_intersections[0];

		// Go through the rest:

		for ( unsigned i = 1 ; i < plane_intersections.size() ; ++i )
		{
			if ( plane_intersections[i].m_hit_distance < closest.m_hit_distance )
			{
				closest = plane_intersections[i];
			}
		}

		// Return the closest hit

		return closest;
	}
	else
	{
		// There was no result:

		return RaycastResult(false);
	}
}

//=====================================================================================================================
/// @brief		Does edge/edge collision between two boxes
/// @param		box1 - 1st box to collide
/// @param		box2 - 2nd box to collide
/// @param		contacts - where to save all the contacts to
//=====================================================================================================================

void ShapeBox::BoxEdgeEdgeContacts
(
	const ShapeBox &			box1		,
	const ShapeBox &			box2		,
	std::vector<Contact> &		contacts
)
{
	// Need to generate a list of edges in box 1 and 2

	static Vec3f edges1_points[12];
	static Vec3f edges2_points[12];
	static Vec3f edges1_directions[12];
	static Vec3f edges2_directions[12];

	box1.GetWorldVertices(edges1_points);
	box2.GetWorldVertices(edges2_points);

	edges1_points[8]  = edges1_points[4];
	edges1_points[9]  = edges1_points[5];
	edges1_points[10] = edges1_points[6];
	edges1_points[11] = edges1_points[7];

	edges2_points[8]  = edges2_points[4];
	edges2_points[9]  = edges2_points[5];
	edges2_points[10] = edges2_points[6];
	edges2_points[11] = edges2_points[7];

	edges1_directions[0] = edges1_points[1] - edges1_points[0];
	edges1_directions[1] = edges1_points[2] - edges1_points[1];
	edges1_directions[2] = edges1_points[3] - edges1_points[2];
	edges1_directions[3] = edges1_points[0] - edges1_points[3];

	edges1_directions[4] = edges1_points[5] - edges1_points[4];
	edges1_directions[5] = edges1_points[6] - edges1_points[5];
	edges1_directions[6] = edges1_points[7] - edges1_points[6];
	edges1_directions[7] = edges1_points[4] - edges1_points[7];

	edges1_directions[8 ] = edges1_points[0] - edges1_points[4];
	edges1_directions[9 ] = edges1_points[1] - edges1_points[5];
	edges1_directions[10] = edges1_points[2] - edges1_points[6];
	edges1_directions[11] = edges1_points[3] - edges1_points[7];

	edges2_directions[0] = edges2_points[1] - edges2_points[0];
	edges2_directions[1] = edges2_points[2] - edges2_points[1];
	edges2_directions[2] = edges2_points[3] - edges2_points[2];
	edges2_directions[3] = edges2_points[0] - edges2_points[3];

	edges2_directions[4] = edges2_points[5] - edges2_points[4];
	edges2_directions[5] = edges2_points[6] - edges2_points[5];
	edges2_directions[6] = edges2_points[7] - edges2_points[6];
	edges2_directions[7] = edges2_points[4] - edges2_points[7];

	edges2_directions[8 ] = edges2_points[0] - edges2_points[4];
	edges2_directions[9 ] = edges2_points[1] - edges2_points[5];
	edges2_directions[10] = edges2_points[2] - edges2_points[6];
	edges2_directions[11] = edges2_points[3] - edges2_points[7];

	// Right: need to compare all the edge pairs:

	for ( unsigned i = 0 ; i < 12 ; ++i )
	{
		// Find the shallowest contact with the second box for this edge:

		Contact c; bool found_contact = false;

		for ( unsigned j = 0 ; j < 12 ; ++j )
		{
			// Get the closest points on these two edges to each other

			Vec3f p1;
			Vec3f p2;

			bool found_closest = ClosestLinePoints
			(
				edges1_points[i]		,
				edges1_directions[i]	,
				edges2_points[j]		,
				edges2_directions[j]	,
				p1						,
				p2
			);

			// See if found:

			if ( found_closest == false ) continue;

			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Only consider if these edges are intersecting, not separated: if there is an intersection then
			// point 1 on edge 1 will be closer to box2's center than the edge point on box 2 etc.
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			if 
			( 
				p1.DistanceTo(box1.GetPosition()) > p2.DistanceTo(box1.GetPosition()) && 
				p2.DistanceTo(box2.GetPosition()) > p1.DistanceTo(box2.GetPosition())
			)
			{
				// Need to transform both points into the coord system of the other box:
				
				Vec3f p1t = box2.GetOrientation().ToMatrix3().Transpose().Transform(( p1 - box2.GetPosition() ));
				Vec3f p2t = box1.GetOrientation().ToMatrix3().Transpose().Transform(( p2 - box1.GetPosition() ));

				// Stability fix: allow close calls. Reduces on/off contacts between frames for resting contacts

				p1t *= 0.9999f;
				p2t *= 0.9999f;
				
				// Now make sure both points lie within the opposite box:
				
				if 
				( 
					p1t.x < - box2.GetSize().x || p1t.x > box2.GetSize().x ||
					p1t.y < - box2.GetSize().y || p1t.y > box2.GetSize().y ||
					p1t.z < - box2.GetSize().z || p1t.z > box2.GetSize().z ||
					p2t.x < - box1.GetSize().x || p2t.x > box1.GetSize().x ||
					p2t.y < - box1.GetSize().y || p2t.y > box1.GetSize().y ||
					p2t.z < - box1.GetSize().z || p2t.z > box1.GetSize().z					
				)
				{
					continue;
				}
			
				// Debug stuff:

				#ifdef USE_DEBUG_CONSOLE

					if ( SimulationWorld::DebugContacts() )
					{
						Stack & s					= GameCore::GetSingleton()->GetStack();
						BufferedLineRenderer * r	= GameCore::GetSingleton()->GetBufferedLineRenderer();

						// Render this contact:

						s.Push( p1				);
						s.Push( 0xffff0000		);
						s.Push( p2				);
						s.Push( 0xff0000ff		);

						// Render:

						r->Draw( GameCore::GetSingleton()->GetCamera()->GetViewProjectionMatrix() , 1 , s );
					}

				#endif

				// Good: got a contact

				if ( found_contact == true )
				{
					// Get penetration depth;

					float d = (p2 - p1).Length();

					// See if smaller:

					if ( c.m_penetration > d )
					{
						c.m_normal		= p2 - p1;
						c.m_penetration = c.m_normal.Normalize();
						c.m_position	= p1;
					}
				}
				else
				{			
					// Save:
					
					c.m_normal		= p2 - p1;
					c.m_penetration = c.m_normal.Normalize();
					c.m_position	= p1;
					
					// Disregard big penetrations: they cannot be right
					
					if 
					( 
						c.m_penetration >= box1.GetSize().x * 0.5f ||
						c.m_penetration >= box1.GetSize().y * 0.5f ||
						c.m_penetration >= box1.GetSize().z * 0.5f 
					)
					{
						continue;
					}
				}

				found_contact = true;
			}
		}

		// Put contact into the list

		if ( found_contact ) contacts.push_back(c);
	}
}

//=====================================================================================================================
/// @brief		Does point/face collision between two boxes. This only does the collision between box1 and box2, not the 
///				other way around.
/// @param		box1 - 1st box
/// @param		box2 - 2nd box
/// @param		contacts - where to save the contacts to
/// @param		reverse - if true then the normals will be reversed
//=====================================================================================================================

void ShapeBox::BoxPointFaceContacts
(
	const ShapeBox &			box1		,
	const ShapeBox &			box2		,
	std::vector<Contact> &		contacts	,
	bool						reverse	
)
{
	// Get the world vertices of box1:

	static Vec3f world_vertices[14]; box1.GetWorldVertices(world_vertices);

	// Makeup the other vertices of the center points of each face on box1 for added stability:

	world_vertices[8 ] = +Vec3f::UnitZ() * box1.GetSize().z;
	world_vertices[9 ] = -Vec3f::UnitZ() * box1.GetSize().z;
	world_vertices[10] = +Vec3f::UnitY() * box1.GetSize().y;
	world_vertices[11] = -Vec3f::UnitY() * box1.GetSize().y;
	world_vertices[12] = +Vec3f::UnitX() * box1.GetSize().x;
	world_vertices[13] = -Vec3f::UnitX() * box1.GetSize().x;

	{
		Matrix3f rot = box1.GetOrientation().ToMatrix3();

		for ( unsigned i = 8 ; i < 14 ; ++i )
		{
			world_vertices[i] = rot.Transform(world_vertices[i]) + box1.GetPosition();
		}
	}

	// Need to produce a copy of these in local space:

	static Vec3f vertices[8];

	// Get box orientation matrix and inverse of box2 for transforming to/from box2's coord system

	Matrix3f rot = box2.GetOrientation().ToMatrix3(); Matrix3f rot_inv = rot.Transpose();

	// Transform into box2 space:

	for ( unsigned i = 0 ; i < 14 ; ++i )
	{
		vertices[i] = world_vertices[i] - box2.GetPosition(); vertices[i] = rot_inv.Transform(vertices[i]);
	}

	// Get the size of the box

	Vec3f size = box2.GetSize();

	// Run through all points:
	
	for ( unsigned i = 0 ; i < 14 ; ++i )
	{
		// Find the axis of least penetration:

		int best_axis = -1; float best_depth = 0;

		// X axis

		if ( vertices[i].x > -size.x && vertices[i].x < size.x )
		{
			// Work out penetration depth:

			float depth = size.x - Float::Abs(vertices[i].x);

			// See if best axis already:

			if ( best_axis == -1 || depth < best_depth )
			{
				// See if negative:

				if ( vertices[i].x < 0 )
				{
					best_axis = 0;
				}
				else
				{
					best_axis = 3;
				}

				best_depth = depth;
			}
		}
		else
		{
			// No overlap on this axis: there can be no collision for this point

			continue;
		}

		// Y axis

		if ( vertices[i].y > -size.y && vertices[i].y < size.y )
		{
			// Work out penetration depth:

			float depth = size.y - Float::Abs(vertices[i].y);

			// See if best axis already:

			if ( best_axis == -1 || depth < best_depth )
			{
				// See if negative:

				if ( vertices[i].y < 0 )
				{
					best_axis = 1;
				}
				else
				{
					best_axis = 4;
				}

				best_depth = depth;
			}
		}
		else
		{
			// No overlap on this axis: there can be no collision for this point

			continue;
		}

		// Z axis

		if ( vertices[i].z > -size.z && vertices[i].z < size.z )
		{
			// Work out penetration depth:

			float depth = size.z - Float::Abs(vertices[i].z);

			// See if best axis already:

			if ( best_axis == -1 || depth < best_depth )
			{
				// See if negative:

				if ( vertices[i].z < 0 )
				{
					best_axis = 2;
				}
				else
				{
					best_axis = 5;
				}

				best_depth = depth;
			}
		}
		else
		{
			// No overlap on this axis: there can be no collision for this point

			continue;
		}

		// Right: see if there was a collision:

		if ( best_axis != -1 )
		{
			// Good: save the contact info

			Contact c;

			if ( reverse )
			{
				switch ( best_axis )
				{
					case 0:	c.m_normal = - Vec3f::UnitX();	break;
					case 1:	c.m_normal = - Vec3f::UnitY();	break;
					case 2:	c.m_normal = - Vec3f::UnitZ();	break;
					case 3:	c.m_normal = + Vec3f::UnitX();	break;
					case 4:	c.m_normal = + Vec3f::UnitY();	break;
					case 5:	c.m_normal = + Vec3f::UnitZ();	break;
				}
			}
			else
			{
				switch ( best_axis )
				{
					case 0:	c.m_normal = + Vec3f::UnitX();	break;
					case 1:	c.m_normal = + Vec3f::UnitY();	break;
					case 2:	c.m_normal = + Vec3f::UnitZ();	break;
					case 3:	c.m_normal = - Vec3f::UnitX();	break;
					case 4:	c.m_normal = - Vec3f::UnitY();	break;
					case 5:	c.m_normal = - Vec3f::UnitZ();	break;
				}
			}

			ENSURE( best_depth >= 0 , "Invalid CD conditions" )
			
			c.m_position	= vertices[i];
			c.m_penetration	= best_depth;

			// Need to transform normal and point into world coords:

			c.m_normal		= rot.Transform(c.m_normal);
			c.m_position	= rot.Transform(c.m_position);
			c.m_position   += box2.GetPosition();

			// Into the list you go:

			contacts.push_back(c);
		}

	}	// end for all points
}

//=====================================================================================================================
/// @brief		Computes the closest points on both given lines to each other. Returns false if there is none.
/// @param		p1 - 1st point of line 1
/// @param		e1 - edge vector of line 1
/// @param		p2 - 1st point of line 2
/// @param		e2 - edge vector of line 2
/// @param		cp1 - where to save the closest point on line 1
/// @param		cp2 - where to save the closest point on line 2
/// @returns	False if there is no closest point in between the two lines
//=====================================================================================================================

bool ShapeBox::ClosestLinePoints
(
	const Vec3f &	p1	,
	const Vec3f &	e1	,
	const Vec3f &	p2	,
	const Vec3f &	e2	,
	Vec3f &			cp1 ,
	Vec3f &			cp2
)
{
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
	// Some theory:
	//
	// Let:
	// 1 - 'A' be the closest point on 1st line
	// 2 - 'B' be the closest point on 2nd line
	// 3 - 'P1' be the 1st line's starting point
	// 4 - 'P2' be the 2nd line's starting point
	// 5 - 'E1' be the edge vector for 1st line (going from start to end point)
	// 6 - 'E2' be the edge vector for 2nd line (going from start to end point)
	//
	// The line in between the two closest points will be perpendicular to both given lines, i.e:
	//
	// (A-B).(E1) = 0
	// (A-B).(E2) = 0
	//
	// A = t1.E1 + P1
	// B = t2.E2 + P2
	//
	// t1 and t2 are unknowns ...
	// 
	// ( t1.E1 + P1 - t2.E2 - P2 ).(E1) = 0
	// ( t1.E1 + P1 - t2.E2 - P2 ).(E2) = 0
	//
	// t1.E1.E1 + P1.E1 - t2.E2.E1 - P2.E1 = 0
	// t1.E1.E2 + P1.E2 - t2.E2.E2 - P2.E2 = 0
	//
	// t1.E1.E1 - t2.E2.E1 = P2.E1 - P1.E1
	// t1.E1.E2 - t2.E2.E2 = P2.E2 - P1.E2
	// 
	// t1(E1.E1) + t2(-E2.E1) = P2.E1 - P1.E1
	// t1(E1.E2) + t2(-E2.E2) = P2.E2 - P1.E2
	//
	// This can be solved using matrices as follows: abcd are the 2nd components in the products with t1 and t2 as above:
	//
	// | a b | | t1 | = | P2.E1 - P1.E1 |
	// | c d | | t2 |   | P2.E2 - P1.E2 |
	// 
	// | t1 | = | a b | -1 | P2.E1 - P1.E1 |
	// | t2 |   | c d |    | P2.E2 - P1.E2 |
	//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

	// Calculate these dot products:

	float a =  e1.Dot(e1);
	float b = -e2.Dot(e1);
	float c =  e1.Dot(e2);
	float d = -e2.Dot(e2);

	// Calculate these also:

	float x = p2.Dot(e1) - p1.Dot(e1);
	float y = p2.Dot(e2) - p1.Dot(e2);

	// Make into a matrix:

	Matrix2f m(a,b,c,d); 

	// See if it has an inverse:

	if ( m.Determinant() != 0 )
	{
		// Ok: get the inverse:

		Matrix2f m_inv = m.Inverse();

		// Now solve for t1 and t2:

		float t1 = m_inv.GetRow(0).Dot( Vec2f(x,y) );
		float t2 = m_inv.GetRow(1).Dot( Vec2f(x,y) );

		// OLD METHOD. Clamp: 

		//if ( t1 < 0 ) t1 = 0; else if ( t1 > 1 ) t1 = 1;
		//if ( t2 < 0 ) t2 = 0; else if ( t2 > 1 ) t2 = 1;
		
		// NEW METHOD: Disregard the point if not on the edge of either object.

		if ( t1 < 0 ) return false;
		if ( t2 < 0 ) return false;
		if ( t1 > 1 ) return false;
		if ( t2 > 1 ) return false;

		// Now solve for the points:

		cp1 = p1 + e1 * t1;
		cp2 = p2 + e2 * t2;

		// Success:

		return true;
	}
	else
	{
		// No solution:

		return false;
	}
}
