//
// Asura_Physics_Vehicle_Speedboat.cpp
//
// Created by Andrew Weinkove - 30/08/05
//
// Copyright (C) 2005 Rebellion, All Rights Reserved.
//

#include "Core/Asura.h"

// KJFL - 13 November 2006 - I've commented out this code because with recent changes to the Physics
// (getting angular and linear velocities, etc) I've removed an invalid angular velocity method that was 
// added to the Physics for use by this class, meaning the code will no longer compile.
//
// Investigating the method revealed that it did not generate correct angular velocities, but obviously
// this speedboat code must have relied upon these values. However, any other speedboats made using this
// code may not have worked (e.g. if they were a different size, etc) so for now I've commented it out
// so it's just here for reference.
//
// This code can be reinstated and refactored if a project comes along that requires driveable boats.
//
#if 0

#include "Physics/Vehicles/Asura_Physics_Vehicle_Speedboat.h"

#include "Core/Asura_Timers.h"
#include "Debugging/Asura_DebugText.h"
#include "Physics/Asura_Physics.h"
#include "Physics/Asura_Physics_Object.h"
#include "Physics/Asura_Physics_RigidBody.h"
#include "Physics/Vehicles/Asura_Physics_Vehicle_Speedboat_Description.h"


/////////////////////////////////////
// Asura_Physics_Vehicle_Speedboat //
/////////////////////////////////////
float Asura_Physics_Vehicle_Speedboat::s_fWaterLevel = 0.0f;

void Asura_Physics_Vehicle_Speedboat::AdjustPositionAndOrientation(Asura_Vector_3& xPosition, Asura_Matrix_3x3& xOrientation)
{
	Asura_Euler_3 xOriEuler;
	xOriEuler.FromMatrix(xOrientation);
	xOriEuler.x += m_fCurrentEulerXOffset;
	xOriEuler.z += m_fCurrentEulerZOffset;
	xOriEuler.ToMatrix(xOrientation);
}


void Asura_Physics_Vehicle_Speedboat::CalculateBouyancyForce(float& fBouyancyForce, float fYPosition, float fEulerX) const
{
	fBouyancyForce = m_pxSpeedboatDescription->m_fSpeedboatGravity;
	float fDepth = GetDepth(fYPosition, fEulerX);
	if (fDepth > m_pxSpeedboatDescription->m_fMaxBouyancyDepth)
	{
		fBouyancyForce += m_pxSpeedboatDescription->m_fMaxBouyancyForce;
	}
	else if (fDepth > 0.0f)
	{
		Asura_Assert(m_pxSpeedboatDescription->m_fMaxBouyancyDepth > 0.0f, "Invalid max bouyancy depth");
		fBouyancyForce += m_pxSpeedboatDescription->m_fMaxBouyancyForce * (fDepth / m_pxSpeedboatDescription->m_fMaxBouyancyDepth);
	}
}

DEBUGVAR float dbg_fMaxTiltTurbulenceX = 50.0f; 
DEBUGVAR float dbg_fMaxTiltTurbulenceZ = 100.0f; 

void Asura_Physics_Vehicle_Speedboat::CalculateTiltForce(float& fEulerXForce, float& fEulerZForce, float fYPosition, const Asura_Euler_3& xOriEuler, const Asura_Vector_3& xVelocity, const Asura_Vector_3& xAcceleration, bool bInvertTurnTilt) const
{
	// Initialise the forces to zero.
	fEulerXForce = 0.0f;
	fEulerZForce = 0.0f;
	// Correct any tilt with damped harmonic motion. Force of this is reduced if we're out of the water.
	float fTiltCorrectionAmount = m_pxSpeedboatDescription->m_fTiltCorrectionMax; 
	float fDepth = GetDepth(fYPosition, xOriEuler.x);
	if (fDepth < m_pxSpeedboatDescription->m_fMinTiltCorrectionDepth)
	{
		fTiltCorrectionAmount = m_pxSpeedboatDescription->m_fTiltCorrectionMin;
	}
	else if (fDepth < m_pxSpeedboatDescription->m_fMaxTiltCorrectionDepth)
	{
		const float fProportion = ((fDepth - m_pxSpeedboatDescription->m_fMinTiltCorrectionDepth) / (m_pxSpeedboatDescription->m_fMaxTiltCorrectionDepth - m_pxSpeedboatDescription->m_fMinTiltCorrectionDepth));
		fTiltCorrectionAmount = (m_pxSpeedboatDescription->m_fTiltCorrectionMin * (1.0f - fProportion)) + (fProportion * m_pxSpeedboatDescription->m_fTiltCorrectionMax);
	}
	fEulerXForce -= xOriEuler.x * fTiltCorrectionAmount;
	fEulerZForce -= xOriEuler.z * fTiltCorrectionAmount;
	// Accelerations make us tilt.
	const Asura_Vector_3 xForwardVector(Asura_Maths::Sine(xOriEuler.y), 0.0f, Asura_Maths::Cosine(xOriEuler.y));
	float fForwardAcceleration = xForwardVector * xAcceleration;

	// Apply some noise to the tilt force
	fForwardAcceleration += Asura_Random::GetNormalFloat() * m_pxSpeedboatDescription->m_fNoiseProportionAngularX * fForwardAcceleration; 

	if (fForwardAcceleration > 0.0f &&			// Avoid tilting forwards.
		xVelocity * xForwardVector > 0.0f)		// Avoid tilting back when slowing down in reverse.
	{
		fEulerXForce += fForwardAcceleration * m_pxSpeedboatDescription->m_fTiltAcceleratingForward;
	}
  	Asura_Vector_3 xRightVector(-xForwardVector.z, 0.0f, xForwardVector.x);
  	if (bInvertTurnTilt)
	{
		// (AW - 01/09/05) - Big hack that deserves a proper explanation.
		// In this model, player controlled boats lean into corners because when they turn
		// they rotate about the y-axis first and then their velocity changes. It is the sliding 
		// that causes the lean into the corners.
		// Unfortunately for boats on splines, rotation about the y-axis is generally a RESULT of
		// the change in velocity. Because there is no sliding, spline-boats are leaning the wrong
		// way when they're cornering. This hack makes it look vaguely correct.
		xRightVector = -xRightVector;
	}
	else
	{
		float fRightAcceleration = xRightVector * xAcceleration;
		fEulerZForce += fRightAcceleration * m_pxSpeedboatDescription->m_fTiltAcceleratingSideways;
	}

	// Fairly unique to boats is the fact that velocities also cause tilting due to 
	// the push of water against the hull. When accelerating and travelling forwards
	// the 'hull effect' is in the same direction as the 'acceleration effect' so you
	// just get a bigger wheelie. When turning, however, the hull effect is in the 
	// opposite direction to the acceleration effect so you often get the counter-intuitive
	// result that the boat will lean into corners.
	float fForwardVelocity = xForwardVector * xVelocity;
  	if (fForwardVelocity >= 0.0f) // Avoid tilting back.
	{
		fEulerXForce += fForwardVelocity * m_pxSpeedboatDescription->m_fTiltTravellingForward;
	}
	float fRightVelocity = xRightVector * xVelocity;
	fEulerZForce += fRightVelocity * m_pxSpeedboatDescription->m_fTiltTravellingSideways;

	// Apply a random tilt to the boat, it will be more noticeable the slower we move
	fEulerXForce += Asura_Random::GetNormalFloat() * dbg_fMaxTiltTurbulenceX * Asura_Physics_System::GetTimeSlice(); 
	Asura_Maths::ClampToRange(fEulerXForce, 0.0f, dbg_fMaxTiltTurbulenceX); 

	fEulerZForce += Asura_Random::GetNormalFloat() * dbg_fMaxTiltTurbulenceZ * Asura_Physics_System::GetTimeSlice(); 
	Asura_Maths::ClampToRange(fEulerZForce, 0.0f, dbg_fMaxTiltTurbulenceZ); 


}

float Asura_Physics_Vehicle_Speedboat::GetDepth(float fYPosition, float fEulerX) const
{
	return (fYPosition - s_fWaterLevel) + m_pxSpeedboatDescription->m_fDepthOffset + (fEulerX * m_pxSpeedboatDescription->m_fTiltDepthOffset);
}

//////////////////////////////////////////////
// Asura_Physics_Vehicle_Speedboat_External //
//////////////////////////////////////////////
Asura_Physics_Vehicle_Speedboat_External* Asura_Physics_Vehicle_Speedboat_External::Create( const Asura_Physics_Vehicle_Speedboat_Description& xSpeedboatDesc, Asura_Physics_Object* pxPhysicsObject)
{
	return new Asura_Physics_Vehicle_Speedboat_External(xSpeedboatDesc, pxPhysicsObject);
}

DEBUGVAR float dbg_fPhysicsSpeedboatExternalMinTimeBetweenRecords = 0.05f;
void Asura_Physics_Vehicle_Speedboat_External::AdjustPositionAndOrientation(Asura_Vector_3& xPosition, Asura_Matrix_3x3& xOrientation)
{
	// Update our records.
	// Guard against calls to this function being too close together.
	if (m_uNumberOfPosAndOriRecords &&
		m_axPosAndOriRecord[m_uNumberOfPosAndOriRecords - 1].m_fTimeStamp + dbg_fPhysicsSpeedboatExternalMinTimeBetweenRecords > Asura_Timers::GetLocalScaledTime())
	{
		// Tweak the position and orientation of the physics object based on our calculated offsets.
		Asura_Physics_Vehicle_Speedboat::AdjustPositionAndOrientation(xPosition, xOrientation);
		xPosition.y += m_fCurrentYOffset;
		return;
	}
	// Update our position and orientation records.
	if (m_uNumberOfPosAndOriRecords == 3)
	{
		// Shift current records up one.
		m_axPosAndOriRecord[0] = m_axPosAndOriRecord[1];
		m_axPosAndOriRecord[1] = m_axPosAndOriRecord[2];
		m_uNumberOfPosAndOriRecords--;
	}
	// Add new record at the end.
	m_axPosAndOriRecord[m_uNumberOfPosAndOriRecords].m_fTimeStamp = Asura_Timers::GetLocalScaledTime();
	m_axPosAndOriRecord[m_uNumberOfPosAndOriRecords].m_xOrientation.FromMatrix(xOrientation);
	m_axPosAndOriRecord[m_uNumberOfPosAndOriRecords].m_xPosition = xPosition;
	m_uNumberOfPosAndOriRecords++;

	// Tweak the position and orientation of the physics object based on our calculated offsets.
	Asura_Physics_Vehicle_Speedboat::AdjustPositionAndOrientation(xPosition, xOrientation);
	xPosition.y += m_fCurrentYOffset;
}

DEBUGVAR float dbg_fTiltToThrottleEstimate = 50.0f;
float Asura_Physics_Vehicle_Speedboat_External::GetThrottle() const
{
	// We're going to have a pretty wild guess at the throttle.
	return Asura_Maths::ClampToRange(m_fCurrentEulerXOffset * dbg_fTiltToThrottleEstimate, 0.0f, 1.0f);
}

DEBUGVAR float dbg_fPhysicsSpeedboatAccInterpRate = 2.0f;
DEBUGVAR float dbg_fMinSpeedForBouncingAndTilting = 15.0f; 

void Asura_Physics_Vehicle_Speedboat_External::Update()
{
	// If we've got enough previous position/ori records then we'll proceed in calculating our offsets.
	if (m_uNumberOfPosAndOriRecords != 3)
	{
		// Can't do an update unless we have enough info to work out velocity AND acceleration.
		return;
	}


	// Work out velocity and acceleration.
	Asura_Vector_3 xOldVelocity = (m_axPosAndOriRecord[1].m_xPosition - m_axPosAndOriRecord[0].m_xPosition) / (m_axPosAndOriRecord[1].m_fTimeStamp - m_axPosAndOriRecord[0].m_fTimeStamp);
	Asura_Vector_3 xCurrentVelocity = (m_axPosAndOriRecord[2].m_xPosition - m_axPosAndOriRecord[1].m_xPosition) / (m_axPosAndOriRecord[2].m_fTimeStamp - m_axPosAndOriRecord[1].m_fTimeStamp);
	Asura_Vector_3 xAcceleration = (xCurrentVelocity - xOldVelocity) / (m_axPosAndOriRecord[2].m_fTimeStamp - m_axPosAndOriRecord[1].m_fTimeStamp);

	// Acceleration seems to change very suddenly on the splines. Smooth it out.
	float fInterpolation = dbg_fPhysicsSpeedboatAccInterpRate * Asura_Physics_System::GetTimeSlice();
	m_xSmoothedAcceleration = (xAcceleration * fInterpolation) + (m_xSmoothedAcceleration * (1.0f - fInterpolation));


	// Calculate change in y-velocity due to bouyancy force.
	float fYPosition = m_axPosAndOriRecord[2].m_xPosition.y + m_fCurrentYOffset;

	// JG - 08/02/06: When the boat comes to a stop, the bouyancy force does some quite weird things
	// Try stopping it doing that
	if (xCurrentVelocity.Magnitude() > dbg_fMinSpeedForBouncingAndTilting)
	{
		float fBouyancyForce;
		CalculateBouyancyForce(fBouyancyForce, fYPosition, m_axPosAndOriRecord[2].m_xOrientation.x + m_fCurrentEulerXOffset);
		m_fYVelocity += fBouyancyForce * Asura_Physics_System::GetTimeSlice();
	}
	else
	{
		m_fYVelocity = 0.0f;
	}

	// Apply the offset.
	m_fCurrentYOffset += m_fYVelocity * Asura_Physics_System::GetTimeSlice();

	// Apply friction to the vertical velocity.
	m_fYVelocity *= 1.0f - Asura_Maths::ClampToRange(m_pxSpeedboatDescription->m_fFrictionForVerticalMovement * Asura_Physics_System::GetTimeSlice(), 0.0f, 1.0f);

	float fEulerXForce = 0.0f;
	float fEulerZForce = 0.0f;
 	Asura_Euler_3 xOriEuler = m_axPosAndOriRecord[2].m_xOrientation;
	xOriEuler.x += m_fCurrentEulerXOffset;
	xOriEuler.z += m_fCurrentEulerZOffset;

	if (xCurrentVelocity.Magnitude() > dbg_fMinSpeedForBouncingAndTilting)
	{
		CalculateTiltForce(fEulerXForce, fEulerZForce, fYPosition, xOriEuler, xCurrentVelocity, m_xSmoothedAcceleration, true);
	}
	else
	{
		m_fCurrentEulerXOffset = 0.0f; 
		m_fCurrentEulerZOffset = 0.0f; 
	}

	// Apply the forces to the angular velocity.
	m_fEulerXVelocity += fEulerXForce * Asura_Physics_System::GetTimeSlice();
	m_fEulerZVelocity += fEulerZForce * Asura_Physics_System::GetTimeSlice();

	// Apply the angular velocity to our offsets.
	m_fCurrentEulerXOffset += m_fEulerXVelocity * Asura_Physics_System::GetTimeSlice();
	m_fCurrentEulerZOffset += m_fEulerZVelocity * Asura_Physics_System::GetTimeSlice();

	// Apply friction to the angular velocity.
	m_fEulerXVelocity *= 1.0f - Asura_Maths::ClampToRange(m_pxSpeedboatDescription->m_fFrictionAngularXZ * Asura_Physics_System::GetTimeSlice(), 0.0f, 1.0f);
	m_fEulerZVelocity *= 1.0f - Asura_Maths::ClampToRange(m_pxSpeedboatDescription->m_fFrictionAngularXZ * Asura_Physics_System::GetTimeSlice(), 0.0f, 1.0f);
}

/////////////////////////////////////////////////
// Asura_Physics_Vehicle_Speedboat_Controller  //
/////////////////////////////////////////////////
Asura_Physics_Vehicle_Speedboat_Controller* Asura_Physics_Vehicle_Speedboat_Controller::Create( const Asura_Physics_Vehicle_Speedboat_Description& xSpeedboatDesc, Asura_Physics_Object* pxPhysicsObject)
{
	return new Asura_Physics_Vehicle_Speedboat_Controller(xSpeedboatDesc, pxPhysicsObject);
}

void Asura_Physics_Vehicle_Speedboat_Controller::HandleControls(float fThrottle, float fRudder)
{
	// This function may be called more or less often than the physics update.
	// Perhaps some smoothing would be improve responsiveness.
	m_fThrottle = fThrottle;
	m_fRudder = fRudder;	
}

DEBUGVAR float dbg_fKeepOldMomentum = 0.3f;
DEBUGVAR bool dbg_bUseCalculatedRotation = false;
DEBUGVAR float dbg_fMinTimeBetweenCollisions = 1.0f;
DEBUGVAR float dbg_fLargeCollisionThresholdSquared = 100.0f;
DEBUGVAR float dbg_fSmallCollisionThresholdSquared = 1.0f;

void Asura_Physics_Vehicle_Speedboat_Controller::Update()
{
	// First, we grab the position and orientation from the rigid body.
	Asura_Vector_3 xPosition = m_pxPhysicsObject->GetPosition();
	Asura_Matrix_3x3 xOrientation = m_pxPhysicsObject->GetOrientation();
	Asura_Euler_3 xOriEuler;
	xOriEuler.FromMatrix(xOrientation);
	xOriEuler.x += m_fCurrentEulerXOffset;
	xOriEuler.z += m_fCurrentEulerZOffset;
	xOriEuler.ToMatrix(xOrientation);

	Asura_Physics_RigidBodyInstance* pxInstance = NULL;
	if (m_pxPhysicsObject->GetHandle() == ASURA_INVALID_HANDLE) return;
	pxInstance = Asura_Physics_RigidBodyInstance::GetInstancePointer( m_pxPhysicsObject->GetHandle() );
	if (!pxInstance) return;
	pxInstance->SetLockYAxisToBeVertical( true );
	
	pxInstance->SetFrictionless( true );

 	if (!dbg_fKeepOldMomentum)
	{
		// Neutralise all velocity in the rigid body. We are going to construct new velocities
		// from our own state. The disadvantage of this is that we lose any forces that
		// may have resulted from collisions, the advantage is that we have some chance of
		// exhibiting vaguely controlled behaviour.
		pxInstance->KillMomentum();
	}
	else
	{
		// Figure out our velocity.
		Asura_AVector3 xVelocity;
		Asura_Euler_3 xAngularVelocity;
 		pxInstance->CalculateVelocity(xVelocity, xAngularVelocity);
		if (dbg_bUseCalculatedRotation)
		{
			m_xAngularVelocity.y = (dbg_fKeepOldMomentum * xAngularVelocity.y) + ((1.0f - dbg_fKeepOldMomentum) * m_xAngularVelocity.y);
		}
		// Check for collisions by comparing our expected velocity with what has come back from the physics system.
		// Don't allow collisions to be too close to one another.
		float fDiffSquared = (m_xVelocity.x - xVelocity.GetX()) * (m_xVelocity.x - xVelocity.GetX()) + 
							(m_xVelocity.y - xVelocity.GetY()) * (m_xVelocity.y - xVelocity.GetY()) + 
							(m_xVelocity.z - xVelocity.GetZ()) * (m_xVelocity.z - xVelocity.GetZ());
		if (fDiffSquared > dbg_fLargeCollisionThresholdSquared)
		{
			if (m_fSpeedboatCollisionTimer <= 0.0f ||
				m_eSpeedboatCollision == SPEEDBOAT_COLLISION_SMALL)
			{
				m_eSpeedboatCollision = SPEEDBOAT_COLLISION_LARGE;
			}
			m_fSpeedboatCollisionTimer = dbg_fMinTimeBetweenCollisions;
		}
		else if (fDiffSquared > dbg_fSmallCollisionThresholdSquared)
		{
			if (m_fSpeedboatCollisionTimer <= 0.0f)
			{
				m_eSpeedboatCollision = SPEEDBOAT_COLLISION_SMALL;
			}
			m_fSpeedboatCollisionTimer = dbg_fMinTimeBetweenCollisions;
		}
		if (m_fSpeedboatCollisionTimer > 0.0f)
		{
			m_fSpeedboatCollisionTimer = Asura_Maths::Max(0.0f, m_fSpeedboatCollisionTimer - Asura_Physics_System::GetTimeSlice());
		}
		//Asura_DebugTextPrint((5, 5, 0xffffffff, 0, "Velocity Diff X : %f", Asura_Maths::FtoD(m_xVelocity.x - xVelocity.GetX())));
		//Asura_DebugTextPrint((5, 6, 0xffffffff, 0, "Velocity Diff Y : %f", Asura_Maths::FtoD(m_xVelocity.y - xVelocity.GetY())));
		//Asura_DebugTextPrint((5, 7, 0xffffffff, 0, "Velocity Diff Z : %f", Asura_Maths::FtoD(m_xVelocity.z - xVelocity.GetZ())));
 		m_xVelocity =  (m_xVelocity * (1.0f - dbg_fKeepOldMomentum)) + Asura_Vector_3(xVelocity * dbg_fKeepOldMomentum);
		pxInstance->KillMomentum();
	}
	float fDepth = GetDepth(xPosition.y, xOriEuler.x);
	ApplyFriction(fDepth, Asura_Vector_3(xOrientation.e13, xOrientation.e23, xOrientation.e33));


	// Now we get on to the actual physics model.

	// Calculate thrust.
	Asura_Vector_3 xThrust(0.0f, 0.0f, m_fThrottle * m_pxSpeedboatDescription->m_fEnginePower);
	if (m_fThrottle < 0.0f)
	{
		// Adjust for reverse gear.
		xThrust *= m_pxSpeedboatDescription->m_fReversePowerProportion;

		// Invert the rudder 
		m_fRudder *= -1; 
	}

	// Adjust for engine potentially being out of the water.
	if (fDepth < m_pxSpeedboatDescription->m_fDepthZeroEnginePower)
	{
		xThrust.Zero();
	}
	else if (fDepth < m_pxSpeedboatDescription->m_fDepthFullEnginePower)
	{
		xThrust *= ((fDepth - m_pxSpeedboatDescription->m_fDepthZeroEnginePower) / (m_pxSpeedboatDescription->m_fDepthFullEnginePower - m_pxSpeedboatDescription->m_fDepthZeroEnginePower));
	}
	Asura_Vector_3 xAcceleration = (xThrust * xOrientation);

	// Calculate the bouyancy force (accounts for gravity too)
	float fBouyancy;
 	CalculateBouyancyForce(fBouyancy, xPosition.y, xOriEuler.x);
   	xAcceleration.y += fBouyancy;

	// Calculate the acceleration caused by the boat not facing the direction of movement.
	// The shape of the hull will transfer some of our momentum into the new direction.
	Asura_Vector_3 xForwardFacingDirection(xOrientation.e13, xOrientation.e23, xOrientation.e33);
	Asura_Vector_3 xVelocityDirection = m_xVelocity;
	float fTotalVelocity = xVelocityDirection.Normalise();
	float fDotProduct = xForwardFacingDirection * xVelocityDirection;
	if (fDotProduct > 0.0f) // Don't bother doing this if we're going backwards.
	{
		Asura_Vector_3 xChangeOfDirectionAcceleration = (xForwardFacingDirection - xVelocityDirection) * fDotProduct * fTotalVelocity;
		xAcceleration += xChangeOfDirectionAcceleration * m_pxSpeedboatDescription->m_fChangeDirectionAbility;
	}

	// Apply the acceleration to the velocity.
	Asura_Vector_3 xOldVelocity(m_xVelocity);
	m_xVelocity += xAcceleration * Asura_Physics_System::GetTimeSlice();
	// Apply the velocity.
  	pxInstance->ApplyImpulse(Asura_AVector3(m_xVelocity * Asura_Physics_System::GetTimeSlice()));
 
	//Asura_DebugTextPrint((5, 5, 0xffffffff, 0, "Velocity X : %f", Asura_Maths::FtoD(m_xVelocity.x)));
	//Asura_DebugTextPrint((5, 6, 0xffffffff, 0, "Velocity Y : %f", Asura_Maths::FtoD(m_xVelocity.y)));
	//Asura_DebugTextPrint((5, 7, 0xffffffff, 0, "Velocity Z : %f", Asura_Maths::FtoD(m_xVelocity.z)));

	// Calculate the rudder force (acts on Euler.y)
	float fRudderProportion = 0.0f;
	if (fDepth > m_pxSpeedboatDescription->m_fMaxRudderDepth)
	{
		fRudderProportion = 1.0f;
	}
	else if (fDepth > m_pxSpeedboatDescription->m_fMinRudderDepth)
	{
		fRudderProportion = (fDepth - m_pxSpeedboatDescription->m_fMinRudderDepth) / (m_pxSpeedboatDescription->m_fMaxRudderDepth - m_pxSpeedboatDescription->m_fMinRudderDepth);
	}	
	// Calculate the forces that will act on angular velocity (euler.y) due to velocity and rudder orientation.
	Asura_Euler_3 xAngularVelocityForce;
	float fForwardsVelocity = m_xVelocity * Asura_Vector_3(xOrientation.e13, xOrientation.e23, xOrientation.e33);

	if (fForwardsVelocity < m_pxSpeedboatDescription->m_fMinTurningSpeed)
		fForwardsVelocity = m_pxSpeedboatDescription->m_fMinTurningSpeed; 

 	xAngularVelocityForce.y = fForwardsVelocity * fRudderProportion * m_fRudder * m_pxSpeedboatDescription->m_fVelocityRudderEffectiveness;
	xAngularVelocityForce.y = Asura_Maths::Sign(xAngularVelocityForce.y) * Asura_Maths::Min(m_pxSpeedboatDescription->m_fRudderVelocityForceMax, Asura_Maths::Abs(xAngularVelocityForce.y)); 
	// Calculate the forces that will act on angular velocity (euler.y) due to thrust and propeller orientation.
	xAngularVelocityForce.y += xThrust.Magnitude() * m_fRudder * m_pxSpeedboatDescription->m_fThrustRudderEffectiveness;
 
	// Now the forces that act on Euler.x and Euler.Z.
	Asura_Vector_3 xActualAcceleration = (m_xVelocity - xOldVelocity) / Asura_Physics_System::GetTimeSlice();
	CalculateTiltForce(xAngularVelocityForce.x, xAngularVelocityForce.z, xPosition.y, xOriEuler, m_xVelocity, xActualAcceleration, false);

	// Apply the forces to the angular velocity.
	for (int i=0;i<3;i++)
	{
		m_xAngularVelocity.Element[i] += xAngularVelocityForce.Element[i] * Asura_Physics_System::GetTimeSlice();
	}

	// The Euler.y component of the velocity is applied at a different position depending on
	// how tilted we are.
	Asura_Vector_3 xCentreOfRotation(xPosition);
	float fCentreOfRotationAdjustment = Asura_Maths::ClampToRange(-m_pxSpeedboatDescription->m_fYRotationCentreAdjustment * xOriEuler.x, -m_pxSpeedboatDescription->m_fMaxRotationCentreOffset, 0.0f);
	xCentreOfRotation += (Asura_Vector_3(0.0f, 0.0f, fCentreOfRotationAdjustment) * xOrientation);
	pxInstance->ApplyRotationalImpulse(Asura_AVector3(xCentreOfRotation), Asura_Euler_3(0.0f, m_xAngularVelocity.y * Asura_Physics_System::GetTimeSlice(), 0.0f));

	// Apply the angular velocity - Euler.x and Euler.z components.
 	m_fCurrentEulerXOffset += m_xAngularVelocity.x * Asura_Physics_System::GetTimeSlice();
	m_fCurrentEulerZOffset += m_xAngularVelocity.z * Asura_Physics_System::GetTimeSlice();


}

float Asura_Physics_Vehicle_Speedboat_Controller::GetThrottle() const
{
	return m_fThrottle;
}

void Asura_Physics_Vehicle_Speedboat_Controller::ApplyFriction(float fDepth, const Asura_Vector_3& xFacing)
{
	// Apply friction to the velocity.
	float fFrictionProportion = 1.0f;
	if (fDepth < m_pxSpeedboatDescription->m_fMinFrictionDepth)
	{
		fFrictionProportion = m_pxSpeedboatDescription->m_fMinFrictionProportion;
	}
	else if (fDepth < m_pxSpeedboatDescription->m_fMaxFrictionDepth)
	{
		fFrictionProportion = (fDepth - m_pxSpeedboatDescription->m_fMinFrictionDepth) / (m_pxSpeedboatDescription->m_fMaxFrictionDepth - m_pxSpeedboatDescription->m_fMinFrictionDepth);
	}
	if (m_xVelocity * xFacing >= 0.0f)
	{
		// Going forwards, use lower friction.
		const float fFriction = 1.0f - Asura_Maths::ClampToRange(fFrictionProportion * m_pxSpeedboatDescription->m_fFrictionGoingForwards * Asura_Physics_System::GetTimeSlice(), 0.0f, 1.0f); 
		m_xVelocity.x *= fFriction;
		m_xVelocity.z *= fFriction;
	}
	else
	{
		// Going backwards, use higher friction.
		const float fFriction = 1.0f - Asura_Maths::ClampToRange(fFrictionProportion * m_pxSpeedboatDescription->m_fFrictionGoingBackwards * Asura_Physics_System::GetTimeSlice(), 0.0f, 1.0f);
		m_xVelocity.x *= fFriction;
		m_xVelocity.z *= fFriction;
	}
	// High friction for vertical movement seems to work well for damping the harmonic motion caused by bouyancy.
	m_xVelocity.y *= 1.0f - Asura_Maths::ClampToRange(fFrictionProportion * m_pxSpeedboatDescription->m_fFrictionForVerticalMovement * Asura_Physics_System::GetTimeSlice(), 0.0f, 1.0f);

	// Apply friction to the angular velocity.
	m_xAngularVelocity.x *= 1.0f - Asura_Maths::ClampToRange(m_pxSpeedboatDescription->m_fFrictionAngularXZ * Asura_Physics_System::GetTimeSlice(), 0.0f, 1.0f);
	m_xAngularVelocity.y *= 1.0f - Asura_Maths::ClampToRange(m_pxSpeedboatDescription->m_fFrictionAngularY * Asura_Physics_System::GetTimeSlice(), 0.0f, 1.0f);
	m_xAngularVelocity.z *= 1.0f - Asura_Maths::ClampToRange(m_pxSpeedboatDescription->m_fFrictionAngularXZ * Asura_Physics_System::GetTimeSlice(), 0.0f, 1.0f);
}

#endif
