void Planning::DefineCurrentGoal()
{
	// Based on character's percieved characters
	unsigned int size = m_pAgent->m_pPerception->GetCharacters().size();
 
	// Iterate through each and determine a new target
	for (unsigned int i = 0; i < size; ++i)
	{
		if(m_pAgent->m_pPerception->GetCharacters()[i]->m_bExists)
		{
			// Perceived Objects in the original implimentation were meant to include both Characters, and power ups
			// Due to the number of game objects in the scene this was a none-issue
			Character * pChar = reinterpret_cast<Character *>(m_pAgent->m_pPerception->GetCharacters()[i]->m_Object);
 
			// If the character grounded, update target orientation and velocity
			// This was the use of simple steering behaviors such as arrive at, and look at
			if (pChar->IsStateActive(GROUND))
			{
				m_Goal.position = pChar->GetPosition();
				Vec3 dir = m_Goal.position - m_pAgent->GetCharacter()->GetPosition();
				m_Goal.orientation_xz = atan2(-dir.x, -dir.z);
				m_Goal.velocity = pChar->GetVelocity();
 
				// We do not want to only propel the character forward but not affect the physics simulation and collision
				m_Goal.velocity.y = 0.0f;
			}
			else
			{
				m_Goal = m_pAgent->m_Steering;
			}
		}
	}
 
	// Two function calls to the NavMesh class which returns the index of the traingle the current target goal is located in
	int triStartID = m_pAgent->m_pPerception->GetNavigation()->GetTriangle(m_pAgent->GetCharacter()->GetPosition());
	int triGoalID = m_pAgent->m_pPerception->GetNavigation()->GetTriangle(m_Goal.position);
	if(triStartID == -1 || triGoalID == -1)
		return;
 
	// Perform an A* search through a NavMesh lookup table
	// The output returns a reverse order waypoints through the nav mesh
	// We performed this output because it made it much easier to debug the process
	vector<Vec3> output = m_pAgent->m_pPerception->GetNavigation()->AStar(triStartID, triGoalID);
	if(output.size() > 2)	// If distance was more than one waypoint away
	{
		Vec3 dir = output[output.size() - 2] - m_pAgent->GetCharacter()->GetPosition();
		m_Goal.position = output[output.size() - 2];
	}
}

void Reaction::ActuateSteering()
{
	const Kinematic *currentGoal = &m_pAgent->m_pPlanning->GetGoal();
	const Kinematic *actor = &m_pAgent->m_Steering;
 
	// vector from current position to current goal
	Vec3 position2goal = currentGoal->position; 
	position2goal -= actor->position;
 
	const float distance2 = (position2goal.x*position2goal.x + 
							position2goal.y*position2goal.y + 
							position2goal.z*position2goal.z);
	const float maxAcceleration2 = m_fMaxAcceleration * m_fMaxAcceleration;
 
	if (distance2 > 0)
	{
		const float d = sqrtf(distance2);
		position2goal *= (1.0f/d); // position2goal is now unit magnitude
 
		// vParallel and vNormal are component vectors of the
		// actor's velocity where vParallel is in the direction
		// from the actor towards the goal, and vNormal is normal
		// to vParallel.
		const float vP = actor->velocity.Dot(position2goal);
		Vec3 vParallel = position2goal;
		vParallel *= vP;
		Vec3 vNormal = actor->velocity; 
		vNormal -= vParallel;
 
		// Apply centripetal linear equal to -vNormal
		m_Steer.linear = vNormal;
		m_Steer.linear *= -1;
 
		// Calculate the magnitude of propelling linear, after centripetal
		// linear is subtracted from maxAcceleration.
		const float vN2 = (vNormal.x*vNormal.x + 
							vNormal.y*vNormal.y + 
							vNormal.z*vNormal.z);
		const float fP2 = maxAcceleration2 - vN2;
 
		// If the centripetal takes all our acceleration budget,
		// then just crop it.
		if (fP2 < 0) 
		{
			m_Steer.linear *= m_fMaxAcceleration / sqrtf(vN2);
		}
		else 
		{
			// Calculate the braking/acceleration distance
			const float gP = currentGoal->velocity.Dot(position2goal);
			const float brakingDistance = (d * m_fMaxAcceleration * 2 + vP*vP - gP*gP) / m_fMaxAcceleration / 4;
 
 
			// apply a smoothing function around the speed summit
			position2goal *= Squash(d - brakingDistance, sqrtf(fP2), m_fSharpness);
			m_Steer.linear += position2goal;
		}
	}
	else
	{   // if we are on target then adjust speed to match goal speed.
		m_Steer.linear = currentGoal->velocity;
		m_Steer.linear -= actor->velocity;
		float linear2 = (m_Steer.linear.x*m_Steer.linear.x +
							m_Steer.linear.y*m_Steer.linear.y +
							m_Steer.linear.z*m_Steer.linear.z);
 
		// If the linear is too big then crop it.
		if (linear2 > maxAcceleration2)
			m_Steer.linear *= m_fMaxAcceleration / sqrtf(linear2);
	}
 
	// Check if we are not breaking the speed limit
	if (m_pAgent->m_fSpeed > m_fMaxSpeed)
	{
		const float dp = m_Steer.linear.Dot(actor->velocity);
		if (dp > 0)
		{
			// remove the forward-pointing component of steer linear
			m_Steer.linear -= actor->velocity * (dp / m_pAgent->m_fSpeed2);
		}
	}
 
	// HACK: WOOHOO
	/*Vec3 newLinear = (*position2goal.Normalize());
	newLinear.x *= 5.0f;
	newLinear.z *= 5.0f;
	newLinear.y = 0.0f;
	m_Steer.linear = newLinear;*/
 
	float distance = position2goal.Length();
 
	Vec3 newLinear = (*position2goal.Normalize());
	newLinear.y = 0.0f;
 
	newLinear = actor->velocity + newLinear;
	if (newLinear.Length() < 20)//m_pAgent->GetCharacter()->GetDefaultSpeed())
	{
		m_Steer.linear = newLinear;
	}
	else
	{
		const float vP = actor->velocity.Dot(position2goal);
		Vec3 vParallel = position2goal;
		vParallel *= vP;
		Vec3 vNormal = actor->velocity; 
		vNormal -= vParallel;
		const float vN2 = (vNormal.x*vNormal.x + 
			vNormal.y*vNormal.y + 
			vNormal.z*vNormal.z);
 
		m_Steer.linear *= m_fMaxAcceleration / sqrtf(vN2);
	}
 
	// Calculate the angle we need to turn to point us in the desired
	// direction.
	const float dAngle = SmallestTurn(actor->orientation_xz,
		currentGoal->orientation_xz);
 
	// Calculate the angle we will turn through when accelerating to our
	// target rotation.
	float brakingAngle = (fabsf(dAngle) * m_fMaxAngular * 2 +
		actor->rotation * actor->rotation -
		currentGoal->rotation * currentGoal->rotation) /
		m_fMaxAngular / 4;
 
	if(dAngle < 0)
		brakingAngle *= -1;
 
	// apply a smoothing function around the speed summit
	m_Steer.angular_y = Squash(dAngle - brakingAngle, m_fMaxAngular, m_fAngularSharpness);
 
	// If we are rotating faster than we currently are allowed and
	// the new angular will make us accelerate even faster, then
	// set the angular to zero.
	if(fabsf(actor->rotation) >= m_fMaxRotation && actor->rotation * m_Steer.angular_y > 0)
	{
		m_Steer.angular_y = 0;
	}
 
	// Calculate the angle we need to turn to point us in the desired
	// direction.
	const float dAngle_2 = SmallestTurn(actor->orientation_yz,
		currentGoal->orientation_yz);
 
	// Calculate the angle we will turn through when accelerating to our
	// target rotation.
	brakingAngle = (fabsf(dAngle_2) * m_fMaxAngular * 2 +
		actor->rotation * actor->rotation -
		currentGoal->rotation * currentGoal->rotation) /
		m_fMaxAngular / 4;
 
	if(dAngle_2 < 0)
		brakingAngle *= -1;
 
	// apply a smoothing function around the speed summit
	m_Steer.angular_x = Squash(dAngle_2 - brakingAngle, m_fMaxAngular, m_fAngularSharpness);
 
	m_Steer.lookAt = currentGoal->lookAt;
 
	// If we are rotating faster than we currently are allowed and
	// the new angular will make us accelerate even faster, then
	// set the angular to zero.
	if(fabsf(actor->rotation) >= m_fMaxRotation && actor->rotation * m_Steer.angular_x > 0)
	{
		m_Steer.angular_x = 0;
	}
}