/*
	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 heightfield shape
//=====================================================================================================================

#include "Precompiled.h"
#include "Physics/ShapeHeightfield.h"
#include "Physics/ShapeBox.h"
#include "Physics/RaycastResult.h"

using namespace Game;

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

#pragma warning(disable:4100)

RaycastResult ShapeHeightfield::Raycast( Ray & ray ) const
{
	// Not supported
	
	Errors::IssueError("Not supported");
	
	// Do this to keep compiler happy:
	
	return RaycastResult(false);
}

#pragma warning(default:4100)

//=====================================================================================================================
/// @brief		Samples the height at a particular point in the heightfield
/// @param		xz_position - xz position to sample at
/// @returns	Height at given position
/// @remarks	This function will clamp to the edges of the heightfield if the xz position is out of range
/// @remarks	This function does not support rotated heightfields. The rotation must be at the identity rotation.
//=====================================================================================================================

float ShapeHeightfield::SampleHeight( const Vec2f & xz_position ) const
{
	// Get the top left corner of the heightmap
	
	Vec2f tl_corner = Vec2f( GetPosition().x , GetPosition().z );
	
	tl_corner.x -= 1.0f * m_xz_scale.x;
	tl_corner.y -= 1.0f * m_xz_scale.y;

	// Get the position relative to the top left corner of the heightmap
	
	Vec2f rel_xz = xz_position - tl_corner;
	
	// Calculate the size of one square on xz axes:
	
	float sq_size_x = ( m_xz_scale.x * 2.0f ) / m_num_squares_f;
	float sq_size_z = ( m_xz_scale.y * 2.0f ) / m_num_squares_f;

	// Calculate which square the xz position is on:
	
	int c = int( rel_xz.x / sq_size_x );
	int r = int( rel_xz.y / sq_size_z );
	
	// Clamp:
	
	if ( r < 0 ) r = 0; else if ( r > int(m_num_squares) ) r = m_num_squares;
	if ( c < 0 ) c = 0; else if ( c > int(m_num_squares) ) c = m_num_squares;
	
	// Calculate the indices of the row below and column to the right of this
	
	int rb = r + 1; 
	int cr = c + 1;
	
	// Clamp:
	
	if ( rb > int(m_num_squares) ) rb = m_num_squares;
	if ( cr > int(m_num_squares) ) cr = m_num_squares;
	
	// Clamp the relative position to within range of the heightfield:
	
	rel_xz.x = Float::Clamp( rel_xz.x , 0 , 2.0f * m_xz_scale.x );
	rel_xz.y = Float::Clamp( rel_xz.y , 0 , 2.0f * m_xz_scale.y );
		
	// Calculate how much in between squares the position is on the x and z axes

	float tx = rel_xz.x - ( float(c) * sq_size_x );
	float tz = rel_xz.y - ( float(r) * sq_size_z );
	
	tx /= sq_size_x;
	tz /= sq_size_z;
	
	// Make sure ok:
	
	ENSURE( tx > -0.001  , "Invalid sampling conditions" )
	ENSURE( tz > -0.001  , "Invalid sampling conditions" )
	ENSURE( tx < +1.001  , "Invalid sampling conditions" )
	ENSURE( tz < +1.001  , "Invalid sampling conditions" )
	
	// Now use bilinear interpolation to sample the point
	
	float height = 
	
		( ( 1.0f - tx ) * ( 1.0f - tz ) * m_vertices[ ( r  * m_num_squares_plus_1 ) + c  ].y ) +
		( (        tx ) * ( 1.0f - tz ) * m_vertices[ ( r  * m_num_squares_plus_1 ) + cr ].y ) +
		( (        tx ) * (        tz ) * m_vertices[ ( rb * m_num_squares_plus_1 ) + cr ].y ) +
		( ( 1.0f - tx ) * (        tz ) * m_vertices[ ( rb * m_num_squares_plus_1 ) + c  ].y ) ;
		
	// Take into account the position of the object:
	
	height += GetPosition().y;
		
	// Return the height
	
	return height;
}

//=====================================================================================================================
/// @brief		Samples the velocity at a particular point in the heightfield.
/// @param		xz_position - xz position to sample velocity at.
/// @returns	Velocity at a given position in the heightfield.
//=====================================================================================================================

float ShapeHeightfield::SampleVelocity( const Vec2f & xz_position ) const
{
	// Get the top left corner of the heightmap
	
	Vec2f tl_corner = Vec2f( GetPosition().x , GetPosition().z );
	
	tl_corner.x -= 1.0f * m_xz_scale.x;
	tl_corner.y -= 1.0f * m_xz_scale.y;

	// Get the position relative to the top left corner of the heightmap
	
	Vec2f rel_xz = xz_position - tl_corner;
	
	// Calculate the size of one square on xz axes:
	
	float sq_size_x = ( m_xz_scale.x * 2.0f ) / m_num_squares_f;
	float sq_size_z = ( m_xz_scale.y * 2.0f ) / m_num_squares_f;

	// Calculate which square the xz position is on:
	
	int c = int( rel_xz.x / sq_size_x );
	int r = int( rel_xz.y / sq_size_z );
	
	// Clamp:
	
	if ( r < 0 ) r = 0; else if ( r > int(m_num_squares) ) r = m_num_squares;
	if ( c < 0 ) c = 0; else if ( c > int(m_num_squares) ) c = m_num_squares;
	
	// Calculate the indices of the row below and column to the right of this
	
	int rb = r + 1; 
	int cr = c + 1;
	
	// Clamp:
	
	if ( rb > int(m_num_squares) ) rb = m_num_squares;
	if ( cr > int(m_num_squares) ) cr = m_num_squares;
	
	// Clamp the relative position to within range of the heightfield:
	
	rel_xz.x = Float::Clamp( rel_xz.x , 0 , 2.0f * m_xz_scale.x );
	rel_xz.y = Float::Clamp( rel_xz.y , 0 , 2.0f * m_xz_scale.y );
		
	// Calculate how much in between squares the position is on the x and z axes

	float tx = rel_xz.x - ( float(c) * sq_size_x );
	float tz = rel_xz.y - ( float(r) * sq_size_z );
	
	tx /= sq_size_x;
	tz /= sq_size_z;
	
	// Make sure ok:
	
	ENSURE( tx > -0.001  , "Invalid sampling conditions" )
	ENSURE( tz > -0.001  , "Invalid sampling conditions" )
	ENSURE( tx < +1.001  , "Invalid sampling conditions" )
	ENSURE( tz < +1.001  , "Invalid sampling conditions" )
	
	// Now use bilinear interpolation to sample the velocity:
	
	float velocity = 
	
		( ( 1.0f - tx ) * ( 1.0f - tz ) * m_velocities[ ( r  * m_num_squares_plus_1 ) + c  ] ) +
		( (        tx ) * ( 1.0f - tz ) * m_velocities[ ( r  * m_num_squares_plus_1 ) + cr ] ) +
		( (        tx ) * (        tz ) * m_velocities[ ( rb * m_num_squares_plus_1 ) + cr ] ) +
		( ( 1.0f - tx ) * (        tz ) * m_velocities[ ( rb * m_num_squares_plus_1 ) + c  ] ) ;		
		
	// Return the velocity:
	
	return velocity;
}

//=====================================================================================================================
/// @brief		Samples the normal at a particular point in the heightfield
/// @param		xz_position - xz position to sample at
/// @returns	Normal at given position
/// @remarks	This function will clamp to the edges of the heightfield if the xz position is out of range
/// @remarks	The normal returned will be normalized
/// @remarks	This function does not support rotated heightfields. The rotation must be at the identity rotation.
//=====================================================================================================================

Vec3f ShapeHeightfield::SampleNormal( const Vec2f & xz_position ) const
{
	// Figure out the size of one square on the x and z axes:
	
	Vec2f sq_size = Vec2f
	(
		( m_xz_scale.x * 2.0f ) * m_num_squares_f_inv ,
		( m_xz_scale.y * 2.0f ) * m_num_squares_f_inv
	);
	
	// Half it:
	
	sq_size *= 1.0f;
	
	// Sample the point:
	
	Vec3f p_c = Vec3f
	( 
		xz_position.x				,
		SampleHeight(xz_position)	,
		xz_position.y				
	);
	
	// Sample points around it to the left/right back/front:
	
	Vec3f p_l = Vec3f
	( 
		xz_position.x - sq_size.x								,
		SampleHeight(xz_position - Vec2f::UnitX() * sq_size.x )	,
		xz_position.y				
	);
	
	Vec3f p_r = Vec3f
	( 
		xz_position.x + sq_size.x								,
		SampleHeight(xz_position + Vec2f::UnitX() * sq_size.x )	,
		xz_position.y	
	);
	
	Vec3f p_b = Vec3f
	( 
		xz_position.x 											,
		SampleHeight(xz_position - Vec2f::UnitY() * sq_size.y)	,
		xz_position.y - sq_size.y			
	);
	
	Vec3f p_f = Vec3f
	( 
		xz_position.x											,
		SampleHeight(xz_position + Vec2f::UnitY() * sq_size.y)	,
		xz_position.y + sq_size.y	
	);
	
	// Get a vector from the center point to all of these points:
	
	Vec3f p_l_rel = p_l - p_c;
	Vec3f p_r_rel = p_r - p_c;
	Vec3f p_b_rel = p_b - p_c;
	Vec3f p_f_rel = p_f - p_c;
	
	// Figure out four un-normalized normals from these points:
	
	Vec3f n1 = p_b_rel.Cross(p_l_rel);
	Vec3f n2 = p_r_rel.Cross(p_b_rel);
	Vec3f n3 = p_f_rel.Cross(p_r_rel);
	Vec3f n4 = p_l_rel.Cross(p_f_rel);
	
	// Normalize all of these:
	
	n1.Normalize();
	n2.Normalize();
	n3.Normalize();
	n4.Normalize();
	
	// Add:
	
	Vec3f n = ( n1 + n2 + n3 + n4 ) * 0.25f;
	
	// Normalize:
	
	n.Normalize();
	
	// Return result
	
	return n;
}

//=====================================================================================================================
/// @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 ShapeHeightfield::GenerateContacts( const Shape & other , std::vector<Contact> & contacts ) const
{
	// See what type of shape it is:
	
	switch ( other.GetType() )
	{
		case Shape::SHAPE_PLANE:
		
			// Never collide with a plane
			
			return;
		
		break;
		
		case Shape::SHAPE_HEIGHTFIELD:
		
			// Never collide with another heightfield
			
			return;
		
		break;
		
		case Shape::SHAPE_BOX:
		
			// Generate the contacts with the box:
			
			BoxContacts((const ShapeBox &)(other),contacts);
		
		break;
	}
}

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

void ShapeHeightfield::BoxContacts( const ShapeBox & other , std::vector<Contact> & contacts ) const
{
	// Rotate and position the 8 points of the box:
	
	static Vec3f box_points[8]; other.GetWorldVertices(box_points);
	
	// Generate the bounds of the trampoline:
	
	Vec2f minb = Vec2f(-1,-1);
	Vec2f maxb = Vec2f(+1,+1);
	
	minb.x *= m_xz_scale.x;
	minb.y *= m_xz_scale.y;
	maxb.x *= m_xz_scale.x;
	maxb.y *= m_xz_scale.y;
	
	minb.x += GetPosition().x;
	minb.y += GetPosition().z;
	maxb.x += GetPosition().x;
	maxb.y += GetPosition().z;
	
	// Samaple the height of the trampoline at the 8 points:
	
	for ( unsigned i = 0 ; i < 8 ; ++i )
	{
		// Sample the height:
	
		float height = SampleHeight( Vec2f(box_points[i].x,box_points[i].z) );
		
		// See if there is a collision:
		
		if ( box_points[i].y < height )
		{
			// Collision: generate the normal
			
			Vec3f normal = SampleNormal( Vec2f(box_points[i].x,box_points[i].z) );

			// Sample the velocity of this mesh point:

			float velocity = SampleVelocity( Vec2f(box_points[i].x,box_points[i].z) );
			
			// Now create the contact:
			
			Contact c; 
			
			c.m_normal					= normal;
			c.m_position				= box_points[i];
			c.m_deformation_velocity	= Vec3f::UnitY() * velocity;
			c.m_penetration				= ( Vec3f(box_points[i].x,height,box_points[i].z) - box_points[i] ).Dot(c.m_normal);
			
			// Add to the list:
			
			contacts.push_back(c);
		}
	}
}
