/*
	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 class represents a soft body composed of a grid mesh style data structure. The body is a plane 
///				(xz) where by the plane is divided up into equal quad parts and whereby the sides of the plane are 
///				fixed and cannot move.
//=====================================================================================================================

#include "Precompiled.h"
#include "Physics/GridMeshSoftBody.h"
#include "Physics/ShapeHeightfield.h"

using namespace Game;

//=========================================================================================================
// Constants
//=========================================================================================================

/// Minimum stiffness at the center of the trampoline

const float GridMeshSoftBody::MIN_STIFFNESS = 5500.0f;

/// Maximum stiffness at the center of the trampoline

const float GridMeshSoftBody::MAX_STIFFNESS = 5500.0f;

/// Dampening factor for the springs

const float GridMeshSoftBody::DAMPENING = 0.0005f;

//=====================================================================================================================
/// @brief		Creates the grid mesh soft body
/// @param		shape - heightfield shape used by the soft body
/// @param		mass - mass of the soft body. This is distributed uniformly around the body
//=====================================================================================================================

GridMeshSoftBody::GridMeshSoftBody( ShapeHeightfield * shape , float mass )
:
Body
(
	shape					,
	false					,	// Not moveable: but only the heighfield vertices actually move
	mass					,
	Matrix3f::Identity()	,	// inertia tensor : not used
	0.0f					,	// restitution
	0.0f					,	// restitution cutoff velocity: not used
	0.60f					,	// static friction
	0.70f					,	// dynamic friction
	0.0f					,	// linear dampening : not used
	0.0f						// angular dampening : not used
)
{
	// Get the heightfield
	
	ShapeHeightfield & heightfield = (ShapeHeightfield&) GetShape();
	
	// Get all the points for the heightfield:
	
	Vec3f * heightfield_points = heightfield.GetVertices();

	// Get all the velocities for the heightfield:
	
	float * heightfield_velocities = heightfield.GetVelocities();

	// Get the number of squares in the heighfield:
	
	unsigned num_squares = heightfield.GetNumSquares();	

	// Arrays for carrying out the RK4 integration algorithm:

	m_rk4_k1_velocities = new float[ (num_squares+1)*(num_squares+1) ];
	m_rk4_k2_velocities = new float[ (num_squares+1)*(num_squares+1) ];
	m_rk4_k3_velocities = new float[ (num_squares+1)*(num_squares+1) ];
	m_rk4_k4_velocities = new float[ (num_squares+1)*(num_squares+1) ];

	/// Create the array of trampoline points
	
	m_points = new TrampolinePoint[ ( num_squares + 1 ) * ( num_squares + 1 ) ];
	
	// Fill in the trampoline point array:
	
	{
		// Current point we are on:
		
		unsigned i = 0;
		
		for ( unsigned r = 0 ; r <= num_squares ; r++ )
		{
			for ( unsigned c = 0 ; c <= num_squares ; c++ )
			{
				// Set the pointer to the shape point:
				
				m_points[i].m_position = heightfield_points + i;
				
				// Set the stiffness:
				
				{
					// Calculate range in between min and max stiffness:
				
					float t1 = Float::Abs( float( int(r) - int(num_squares>>1) ) * 2.0f ) / float(num_squares);
					float t2 = Float::Abs( float( int(c) - int(num_squares>>1) ) * 2.0f ) / float(num_squares);
					
					float t = t1 * t2;

					// Make sure this is in range

					ENSURE( t > -0.00001f && t < 1.00001f , "Invalid stiffness" )
					
					// Set stiffness:
					
					m_points[i].m_stiffness = ( 1.0f - t ) * MIN_STIFFNESS + t * MAX_STIFFNESS;
				}

				// Set the velocity pointer:

				m_points[i].m_velocity = heightfield_velocities + i;
				
				// Set the initial velocity:
				
				* ( m_points[i].m_velocity ) = 0;

				// Initialize the RK4 velocities:

				m_rk4_k1_velocities[ r * (num_squares+1) + c ] = 0;
				m_rk4_k2_velocities[ r * (num_squares+1) + c ] = 0;
				m_rk4_k3_velocities[ r * (num_squares+1) + c ] = 0;
				m_rk4_k4_velocities[ r * (num_squares+1) + c ] = 0;
				
				// Next point
				
				i++;
			}
		}		
	}
}

//=====================================================================================================================
/// @brief		Destroys the grid mesh soft body
//=====================================================================================================================

GridMeshSoftBody::~GridMeshSoftBody()
{
	// Done with our points:
	
	delete[] m_points;

	// Get rid of these RK4 arrays:

	delete[] m_rk4_k1_velocities;
	delete[] m_rk4_k2_velocities;
	delete[] m_rk4_k3_velocities;
	delete[] m_rk4_k4_velocities;
}

//=====================================================================================================================
///	@brief		Updates all the dynamics and forces acting on the body
/// @param		time_delta - amount of time between frames
/// @todo		Implement this function
//=====================================================================================================================

void GridMeshSoftBody::Integrate( float time_delta )
{
	// Get the heightfield
	
	ShapeHeightfield & heightfield = (ShapeHeightfield&) GetShape();
	
	// Get the number of squares in the heighfield:
	
	unsigned num_squares = heightfield.GetNumSquares();

	// Number of squares in the heightfield plus 1:

	unsigned num_squares_plus_1 = num_squares + 1;

	// World gravity value:

	Vec3f world_gravity = GetWorldGravity();

	// Zero these:

	SetWorldVelocity		( Vec3f::Zero() );
	SetWorldAngularVelocity	( Vec3f::Zero() );

	//-----------------------------------------------------------------------------------------------------------------
	// RK4 INTEGRATION
	//-----------------------------------------------------------------------------------------------------------------

	// Get the velocities for heightfield: copy to the rk4 k1 array

	{	
		// Get them:
			
		float * velocities = heightfield.GetVelocities();

		// Copy into this array:

		for ( unsigned r = 0 ; r <= num_squares ; r++ )
		for ( unsigned c = 0 ; c <= num_squares ; c++ )
		{
			// Get the velocity:

			float v = velocities[ num_squares_plus_1 * r + c ];

			// Save:

			m_rk4_k1_velocities[ ( num_squares_plus_1 * r ) + c ] = v;

			// Note: if along the edges then ignore. No velocity in this case.

			if ( r == 0 || r == num_squares ) 
			{
				m_rk4_k1_velocities[ num_squares_plus_1 * r + c ] = 0;

				// Take this opportunity to clear the actual velocity value at the edges also:

				velocities[ num_squares_plus_1 * r + c ] = 0;
			}

			if ( c == 0 || c == num_squares ) 
			{
				m_rk4_k1_velocities[ num_squares_plus_1 * r + c ] = 0;

				// Take this opportunity to clear the actual velocity value at the edges also:

				velocities[ num_squares_plus_1 * r + c ] = 0;
			}
		}
	}

	// Now: need to evaluate k2, or the derivative at the halfway point of timestep 

	for ( unsigned r = 1 ; r < num_squares ; r++ )
	for ( unsigned c = 1 ; c < num_squares ; c++ )
	{
		// Get the current velocity of the point and of it's neighbours:

		float v		= m_rk4_k1_velocities[ ( r     ) * num_squares_plus_1 + ( c     ) ];
		float v1	= m_rk4_k1_velocities[ ( r - 1 ) * num_squares_plus_1 + ( c     ) ];
		float v2	= m_rk4_k1_velocities[ ( r + 1 ) * num_squares_plus_1 + ( c     ) ];
		float v3	= m_rk4_k1_velocities[ ( r     ) * num_squares_plus_1 + ( c - 1 ) ];
		float v4	= m_rk4_k1_velocities[ ( r     ) * num_squares_plus_1 + ( c + 1 ) ];

		// Get the current y value of the point and of it's neighbours:

		float y		= m_points[ ( r     ) * num_squares_plus_1 + ( c     ) ].m_position->y;
		float y1	= m_points[ ( r - 1 ) * num_squares_plus_1 + ( c     ) ].m_position->y;
		float y2	= m_points[ ( r + 1 ) * num_squares_plus_1 + ( c     ) ].m_position->y;
		float y3	= m_points[ ( r     ) * num_squares_plus_1 + ( c - 1 ) ].m_position->y;
		float y4	= m_points[ ( r     ) * num_squares_plus_1 + ( c + 1 ) ].m_position->y;

		// Move along both the point and it's neighbours by half the timestep interval:

		y	+= v  * time_delta * 0.5f;
		y1  += v1 * time_delta * 0.5f;
		y2  += v2 * time_delta * 0.5f;
		y3  += v3 * time_delta * 0.5f;
		y4  += v4 * time_delta * 0.5f;

		// Apply gravity to the point:

		v += world_gravity.y * time_delta * 0.5f;

		// Get average y position of all four points:

		float y_avg = ( y1 + y2 + y3 + y4 ) * 0.25f;

		// Calculate distance from rest position:

		float rest_dist = y - y_avg;
		
		// Calculate spring forces:

		float f = ( m_points[ r * num_squares_plus_1 + c ].m_stiffness * - rest_dist ) - ( v * DAMPENING );

		// Apply force:

		v += f * time_delta * 0.5f * GetInverseMass();

		// Save as K2:

		m_rk4_k2_velocities[ num_squares_plus_1 * r + c ] = v;
	}

	// Now: need to evaluate k3, or the derivative at the halfway point of timestep using k2 as velocity

	for ( unsigned r = 1 ; r < num_squares ; r++ )
	for ( unsigned c = 1 ; c < num_squares ; c++ )
	{
		// Get the current velocity of the point and of it's neighbours:

		float v		= m_rk4_k2_velocities[ ( r     ) * num_squares_plus_1 + ( c     ) ];
		float v1	= m_rk4_k2_velocities[ ( r - 1 ) * num_squares_plus_1 + ( c     ) ];
		float v2	= m_rk4_k2_velocities[ ( r + 1 ) * num_squares_plus_1 + ( c     ) ];
		float v3	= m_rk4_k2_velocities[ ( r     ) * num_squares_plus_1 + ( c - 1 ) ];
		float v4	= m_rk4_k2_velocities[ ( r     ) * num_squares_plus_1 + ( c + 1 ) ];

		// Get the current y value of the point and of it's neighbours:

		float y		= m_points[ ( r     ) * num_squares_plus_1 + ( c     ) ].m_position->y;
		float y1	= m_points[ ( r - 1 ) * num_squares_plus_1 + ( c     ) ].m_position->y;
		float y2	= m_points[ ( r + 1 ) * num_squares_plus_1 + ( c     ) ].m_position->y;
		float y3	= m_points[ ( r     ) * num_squares_plus_1 + ( c - 1 ) ].m_position->y;
		float y4	= m_points[ ( r     ) * num_squares_plus_1 + ( c + 1 ) ].m_position->y;

		// Move along both the point and it's neighbours by half the timestep interval:

		y	+= v  * time_delta * 0.5f;
		y1  += v1 * time_delta * 0.5f;
		y2  += v2 * time_delta * 0.5f;
		y3  += v3 * time_delta * 0.5f;
		y4  += v4 * time_delta * 0.5f;

		// Apply gravity to the point:

		v += world_gravity.y * time_delta * 0.5f;

		// Get average y position of all four points:

		float y_avg = ( y1 + y2 + y3 + y4 ) * 0.25f;

		// Calculate distance from rest position:

		float rest_dist = y - y_avg;
		
		// Calculate spring forces:

		float f = ( m_points[ r * num_squares_plus_1 + c ].m_stiffness * - rest_dist ) - ( v * DAMPENING );

		// Apply force:

		v += f * time_delta * 0.5f * GetInverseMass();

		// Save as K3:

		m_rk4_k3_velocities[ num_squares_plus_1 * r + c ] = v;
	}

	// Now: need to evaluate k4, or the derivative at the end of timestep using k3 as velocity

	for ( unsigned r = 1 ; r < num_squares ; r++ )
	for ( unsigned c = 1 ; c < num_squares ; c++ )
	{
		// Get the current velocity of the point and of it's neighbours:

		float v		= m_rk4_k3_velocities[ ( r     ) * num_squares_plus_1 + ( c     ) ];
		float v1	= m_rk4_k3_velocities[ ( r - 1 ) * num_squares_plus_1 + ( c     ) ];
		float v2	= m_rk4_k3_velocities[ ( r + 1 ) * num_squares_plus_1 + ( c     ) ];
		float v3	= m_rk4_k3_velocities[ ( r     ) * num_squares_plus_1 + ( c - 1 ) ];
		float v4	= m_rk4_k3_velocities[ ( r     ) * num_squares_plus_1 + ( c + 1 ) ];

		// Get the current y value of the point and of it's neighbours:

		float y		= m_points[ ( r     ) * num_squares_plus_1 + ( c     ) ].m_position->y;
		float y1	= m_points[ ( r - 1 ) * num_squares_plus_1 + ( c     ) ].m_position->y;
		float y2	= m_points[ ( r + 1 ) * num_squares_plus_1 + ( c     ) ].m_position->y;
		float y3	= m_points[ ( r     ) * num_squares_plus_1 + ( c - 1 ) ].m_position->y;
		float y4	= m_points[ ( r     ) * num_squares_plus_1 + ( c + 1 ) ].m_position->y;

		// Move along both the point and it's neighbours by the timestep interval:

		y	+= v  * time_delta;
		y1  += v1 * time_delta;
		y2  += v2 * time_delta;
		y3  += v3 * time_delta;
		y4  += v4 * time_delta;

		// Apply gravity to the point:

		v += world_gravity.y * time_delta;

		// Get average y position of all four points:

		float y_avg = ( y1 + y2 + y3 + y4 ) * 0.25f;

		// Calculate distance from rest position:

		float rest_dist = y - y_avg;
		
		// Calculate spring forces:

		float f = ( m_points[ r * num_squares_plus_1 + c ].m_stiffness * - rest_dist ) - ( v * DAMPENING );

		// Apply force:

		v += f * time_delta * GetInverseMass();

		// Save as K4:

		m_rk4_k4_velocities[ num_squares_plus_1 * r + c ] = v;
	}

	{
		// Get the heightfield velocities:
			
		float * velocities = heightfield.GetVelocities();

		// Now using all four derviates, update the positions of all the vertices:

		for ( unsigned r = 1 ; r < num_squares ; r++ )
		for ( unsigned c = 1 ; c < num_squares ; c++ )
		{
			// Get all the 4 derivates of RK4

			float d1 = m_rk4_k1_velocities[ num_squares_plus_1 * r + c ];
			float d2 = m_rk4_k2_velocities[ num_squares_plus_1 * r + c ];
			float d3 = m_rk4_k3_velocities[ num_squares_plus_1 * r + c ];
			float d4 = m_rk4_k4_velocities[ num_squares_plus_1 * r + c ];

			// Calculate the final velocity to use for the update as per the RK4 method:

			float v = ( 1.0f / 6.0f ) * ( d1 + ( 2.0f * d2 ) + ( 2.0f * d3 ) + d4 );

			// Update the position of the vertex:

			m_points[ num_squares_plus_1 * r + c ].m_position->y += v * time_delta;

			// Now calculate the actual velocity update: begin with gravity

			velocities[ r * num_squares_plus_1 + c ] += world_gravity.y * time_delta;

			// Get the current y value of the point and of it's neighbours:

			float y		= m_points[ ( r     ) * num_squares_plus_1 + ( c     ) ].m_position->y;
			float y1	= m_points[ ( r - 1 ) * num_squares_plus_1 + ( c     ) ].m_position->y;
			float y2	= m_points[ ( r + 1 ) * num_squares_plus_1 + ( c     ) ].m_position->y;
			float y3	= m_points[ ( r     ) * num_squares_plus_1 + ( c - 1 ) ].m_position->y;
			float y4	= m_points[ ( r     ) * num_squares_plus_1 + ( c + 1 ) ].m_position->y;

			// Get average y position of all four points:

			float y_avg = ( y1 + y2 + y3 + y4 ) * 0.25f;

			// Calculate distance from rest position:

			float rest_dist = y - y_avg;
			
			// Calculate spring forces:

			float f = ( m_points[ r * num_squares_plus_1 + c ].m_stiffness * - rest_dist ) - ( v * DAMPENING );

			// Apply the force:

			velocities[ r * num_squares_plus_1 + c ] += f * time_delta * GetInverseMass();
		}
	}

	//-----------------------------------------------------------------------------------------------------------------
	// EULER INTEGRATION: NOT USED ANY MORE
	//-----------------------------------------------------------------------------------------------------------------

	// Apply gravity to all points:
	
	/*{
		// Amount to increase velocity by:
		
		Vec3f gravity = GetWorldGravity() * time_delta;
		
		// Current point we are on:
		
		TrampolinePoint * point = m_points;
		
		// Go through each row and column:
		
		for ( unsigned r = 0 ; r <= num_squares ; ++r )
		{
			for ( unsigned c = 0 ; c <= num_squares ; ++c )
			{
				// Only apply if on the edge:
				
				if ( r > 0 && c > 0 && r < num_squares && c < num_squares )
				{
					// Apply gravity:
				
					* ( point->m_velocity ) += gravity.y;
					
					// Calculate the average position of neigbours: this is the position we will try to reach:
					
					float y1 = m_points[ ( r - 1 ) * ( num_squares + 1 ) + c + 0 ].m_position->y;
					float y2 = m_points[ ( r + 1 ) * ( num_squares + 1 ) + c + 0 ].m_position->y;
					float y3 = m_points[ ( r + 0 ) * ( num_squares + 1 ) + c - 1 ].m_position->y;
					float y4 = m_points[ ( r + 0 ) * ( num_squares + 1 ) + c + 1 ].m_position->y;
					
					float avg_y = ( y1 + y2 + y3 + y4 ) * 0.25f;
					
					// Get the distance it is from rest position:
					
					float rest_dist = point->m_position->y - avg_y;
					
					// Calculate spring forces:
					
					float force = ( point->m_stiffness * - rest_dist ) - ( (*(point->m_velocity)) * DAMPENING );
					
					// Apply spring force to the point:
					
					* ( point->m_velocity ) += force * time_delta * GetInverseMass();
					
					// Now move the point:
					
					point->m_position->y += ( * ( point->m_velocity ) ) * time_delta;
				}
				else
				{
					// Zero velocity: on the edge

					* ( point->m_velocity ) = 0;
				}
				
				// Next point
				
				point++;
			}
		}
	}*/
}

//=====================================================================================================================
/// @brief		Collides this body with the given body and does collision response:
/// @param		time_delta - amount of time between frames
/// @param		other - other body to collide with
//=====================================================================================================================

void GridMeshSoftBody::Collide( float time_delta , Body & other )
{
	// See if we are moveable:
	
	if ( IsMoveable() )
	{
		if ( other.IsMoveable() )
		{
			DynamicDynamicCollision(*this,other,time_delta);
		}
		else
		{
			StaticDynamicCollision(other,*this,time_delta);
		}
	}
	else
	{
		if ( other.IsMoveable() )
		{
			StaticDynamicCollision(*this,other,time_delta);
		}
	}
}

//=====================================================================================================================				
/// @brief		Called after the end of each update to the object: after Integrate() and Collide()
//=====================================================================================================================
				
void GridMeshSoftBody::EndUpdate()
{
	// Save the previous linear acceleration for later stabilisation of resting contacts:
	
	SetPreviousWorldAcceleration( GetWorldAcceleration() );
	
	// Reset all forces acting on the rigid body
	
	ResetAccelerations();
}

//=====================================================================================================================
/// @bfief		Applies a given force through the center of mass of the body. Force is given in world coordinates.
/// @param		force - force to apply
/// @remarks	This function is ingored for this type of body
//=====================================================================================================================

#pragma warning(disable:4100)
					
void GridMeshSoftBody::ApplyWorldForce( const Vec3f & force )
{
	// Ignore this. We only deal with force at a point for this type of body.
}

#pragma warning(default:4100)

//=====================================================================================================================		
/// @brief		Applies a given force from a given point in world coordinates. Force is given in world coordinates also.
/// @param		force - force to apply
/// @param		position - position to apply the force at
/// @todo		Implement this function
///=====================================================================================================================

#pragma warning(disable:4100)

void GridMeshSoftBody::ApplyWorldForce( const Vec3f & force , const Vec3f & position )
{
	Errors::IssueError("Not implemented");
}

#pragma warning(default:4100)

//=====================================================================================================================
/// @brief		Applies torque to the body: this is torque in world coordinates
/// @param		torque - amount of torque toa apply
/// @remarks	This function is ingored for this type of body
//=====================================================================================================================

#pragma warning(disable:4100)
					
void GridMeshSoftBody::ApplyWorldTorque( const Vec3f & torque )
{
	// Ignore this. We only deal with force at a point for this type of body. No angular stuff.
}

#pragma warning(default:4100)

//=====================================================================================================================	
/// @brief		Applies a given linear acceleration to the body in world coordinates
/// @param		acceleration - acceleration to apply 
/// @remarks	This function is ingored for this type of body
//=====================================================================================================================

#pragma warning(disable:4100)
					
void GridMeshSoftBody::ApplyWorldAcceleration( const Vec3f & acceleration )
{
	// Ignore this. We only deal with force at a point for this type of body.
}

#pragma warning(default:4100)

//=====================================================================================================================
/// @brief		Applies a given linear acceleration to the body at the given position in world coordinates. 
///				Acceleration is in world coords also.
/// @param		acceleration - acceleration to apply
/// @param		position - position to apply it at
/// @todo		Implement this function
//=====================================================================================================================

#pragma warning(disable:4100)

void GridMeshSoftBody::ApplyWorldAcceleration( const Vec3f & acceleration , const Vec3f & position )
{
	Errors::IssueError("Not implemented");
}

#pragma warning(default:4100)

//=====================================================================================================================
/// @brief		Applies a given angular acceleration to the body in world coordinates
/// @param		angular_acceleration - amount of acceleration toa apply
/// @remarks	This function is ingored for this type of body
//=====================================================================================================================

#pragma warning(disable:4100)
					
void GridMeshSoftBody::ApplyWorldAngularAcceleration( const Vec3f & angular_acceleration )
{
	// Ignore this. We only deal with force at a point for this type of body. No angular stuff.
}

#pragma warning(default:4100)

//=====================================================================================================================					
/// @brief		Applies an impulse through the center of the body in world coordinates. 
/// @param		impulse - impulse to apply
/// @remarks	This function is ingored for this type of body
//=====================================================================================================================

#pragma warning(disable:4100)					
					
void GridMeshSoftBody::ApplyWorldImpulse( const Vec3f & impulse )
{
	// Ignore this. We only deal with force at a point for this type of body.
}

#pragma warning(default:4100)

//=====================================================================================================================
/// @brief		Applies an impulse to the body at the given world position. Impulse is also in world coordinates.
/// @param		impulse - impulse to apply
/// @param		position - position to apply the impulse at
/// @todo		Implement this function
//=====================================================================================================================
					
void GridMeshSoftBody::ApplyWorldImpulse( const Vec3f & impulse , const Vec3f & position )
{
	// Get the heightfield shape

	ShapeHeightfield & heightfield = ((ShapeHeightfield&)(GetShape()));

	// Get the top left corner of the heightmap
	
	Vec2f tl_corner = Vec2f( GetWorldPosition().x , GetWorldPosition().z );

	// Get the xz scale of the heightfield:

	Vec2f xz_scale = heightfield.GetXZScale();

	// Figure out the top left corner of the heightfield
	
	tl_corner.x -= 1.0f * xz_scale.x;
	tl_corner.y -= 1.0f * xz_scale.y;

	// Get the xz position of the impulse:

	Vec2f xz_position(position.x,position.z);

	// Get the position relative to the top left corner of the heightmap
	
	Vec2f rel_xz = xz_position - tl_corner;

	// Get the number of squares in the heightfield

	unsigned num_squares = heightfield.GetNumSquares();
	
	// Calculate the size of one square on xz axes:
	
	float sq_size_x = ( xz_scale.x * 2.0f ) / float(num_squares);
	float sq_size_z = ( xz_scale.y * 2.0f ) / float(num_squares);

	// 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(num_squares) ) r = num_squares;
	if ( c < 0 ) c = 0; else if ( c > int(num_squares) ) c = 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(num_squares) ) rb = num_squares;
	if ( cr > int(num_squares) ) cr = num_squares;
	
	// Clamp the relative position to within range of the heightfield:
	
	rel_xz.x = Float::Clamp( rel_xz.x , 0 , 2.0f * xz_scale.x );
	rel_xz.y = Float::Clamp( rel_xz.y , 0 , 2.0f * 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" )

	// Scale the impulse by the mass of the heightfield:

	float y_impulse = impulse.y * GetInverseMass();

	// Get the velocities of the heightfield

	float * velocities = heightfield.GetVelocities();

	// Get the number of squares in the heightfield plus 1

	unsigned num_squares_plus_1 = num_squares + 1;
	
	// Now use bilinear interpolation to apply across points:

	velocities[ ( r  * num_squares_plus_1 ) + c  ]	+= ( 1.0f - tx ) * ( 1.0f - tz ) * y_impulse;
	velocities[ ( r  * num_squares_plus_1 ) + cr ]	+= (        tx ) * ( 1.0f - tz ) * y_impulse;
	velocities[ ( rb * num_squares_plus_1 ) + cr ]	+= (        tx ) * (        tz ) * y_impulse;
	velocities[ ( rb * num_squares_plus_1 ) + c  ]	+= ( 1.0f - tx ) * (        tz ) * y_impulse;
}
