//////////////////////////////////////////////////////////////////////
//
// Boid.cpp: implementation of the Boid class.
//
//////////////////////////////////////////////////////////////////////

#include "Boid.h"
#include "utils.h"
#include "forest.h"
#include <cmath>

////////////////////////////////////////////////
// stuff for profiling
#include "profiler/profiler_lowlevel.h"
#include "profiler/profiler_highlevel.h"

// Definitions for the program zones we will use in
// this file (there are also some zones in the
// 'profiler' subdirectory)
Define_Zone(boid_update);
Define_Zone(boid_navigator);
Define_Zone(boid_accumulate);
Define_Zone(boid_compute_neigbors);
Define_Zone(boid_avoid_obstacles);
Define_Zone(boid_keep_distance);
Define_Zone(boid_match_heading);
Define_Zone(boid_steer_to_center);
Define_Zone(boid_goto_goal);
// end of profiling stuff
///////////////////////////////////////////////


//////////////////////////////////////////////////////////////
// Here are the parameters that control the animal behavior
// at this point they are constants, but they could be different
// for each individual animal
const float BOID_MAX_SPEED = 10.00f;
const float BOID_CRUISING_SPEED = 0.7f * BOID_MAX_SPEED;
const float BOID_MAX_ACCELERTION = BOID_MAX_SPEED * 2.0f;
const float BOID_RADIUS = .05f;
const float BOID_COMFORT_RADIUS = BOID_RADIUS * 8.0f;
const float BOID_DANGER_RADIUS = BOID_RADIUS * 4.0f;
const float BOID_VISIBLE_RANGE = BOID_RADIUS * 200.0f;
const float BOID_VISIBLE_TREE_RANGE = 10.4f;
const float BOID_VIEWING_ANGLE = PI*3.0f / 4.0f; // Maximum angle (from the vector pointing directly ahead) that a boid can see
const float BOID_INITIAL_TERRAIN_DRAG_FACTOR = 30.0f;
const float BOID_INITIAL_DOWNHILL_DRAG_FACTOR = BOID_INITIAL_TERRAIN_DRAG_FACTOR / 5;
////////////////////////////////////////////////////////////////////////////////////////


// declaration necessary for static members
list<Boid *> Boid::neighborList;


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

// Constructor #1
// creates an individual boid with randaomized position,
// velocity, and orientation.
Boid::Boid(LPDIRECT3DDEVICE9 CreationDevice, Flock* f, Terrain* t)
{
	parentFlock = f;
	terrain = t;
	
	///////////////////////////////////////////////////////
	// set up the 3D model for this boid
	m_mesh = new Mesh();
	LPD3DXMESH NewMesh;

	// for now animals will be represented by simple spheres
	D3DXCreateSphere(CreationDevice, BOID_RADIUS, 10,10, &NewMesh, NULL);
	m_mesh->SetD3DXMesh(NewMesh);

	// set up the material for this boid
	// for now, a simple red color will do
	D3DMATERIAL9 NewMaterial;
	memset(&NewMaterial,0, sizeof(NewMaterial));
	NewMaterial.Ambient.a = 1;
	NewMaterial.Ambient.b = 0;
	NewMaterial.Ambient.r = 1;
	NewMaterial.Ambient.g = 0;
	NewMaterial.Diffuse = NewMaterial.Ambient;
	m_mesh->SetMaterials(1, &NewMaterial);
	///////////////////////////////////////////////////////

	team = 0;

	// set a random position
	m_2position = Vector2D(3 * (float(rand()) / float(RAND_MAX)) +3.5f,
							3.0f * (float(rand()) / float(RAND_MAX)) +3.5f);

	// pick a random angle...
	float r = 2*PI * (float(rand()) / float(RAND_MAX)); 
	// ...and a random speed...
	float speed = /*BOID_MAX_SPEED*/ 0 * (float(rand()) / float(RAND_MAX)); 
	// ...to form a random velocity
	m_2velocity = Vector2D((float)sin(r), (float)-cos(r)) * speed;

	// initial acceleration is zero
	m_2acceleration = Vector2D(0,0);

	// set the PhysicalEntity 3D positon
	Position = D3DXVECTOR3(m_2position.x, m_2position.y, 
					terrain->GetHeight(m_2position.x, m_2position.y) + BOID_RADIUS*2);

	///////////////////////////////////
	// Temporary addition for calculating energy expenditure
	lastPos = Position;
	energy = 0;
	TERRAIN_DRAG_FACTOR = BOID_INITIAL_TERRAIN_DRAG_FACTOR;
	DOWNHILL_TERRAIN_DRAG_FACTOR = BOID_INITIAL_DOWNHILL_DRAG_FACTOR;
	//////////////////////////////////////////////////////
}

// Constructor #2
// creates an individual boid with specific position and velocity.
Boid::Boid(LPDIRECT3DDEVICE9 CreationDevice, Flock* f, Terrain* t, 
		          const Vector2D& startpos, const Vector2D& startvel)
{
	parentFlock = f;
	terrain = t;

	///////////////////////////////////////////////////////
	// set up the 3D model for this boid
	m_mesh = new Mesh();
	LPD3DXMESH NewMesh;
    
	//Radius = BallRadius;

	// for now animals will be represented by simple spheres
	D3DXCreateSphere(CreationDevice, BOID_RADIUS, 10,10, &NewMesh, NULL);
	m_mesh->SetD3DXMesh(NewMesh);

	// set up the material for this boid
	// for now, a simple red color will do
	D3DMATERIAL9 NewMaterial;
	memset(&NewMaterial,0, sizeof(NewMaterial));
	NewMaterial.Ambient.a = 1;
	NewMaterial.Ambient.b = 0;
	NewMaterial.Ambient.r = 1;
	NewMaterial.Ambient.g = 0;
	NewMaterial.Diffuse = NewMaterial.Ambient;
	m_mesh->SetMaterials(1, &NewMaterial);
	///////////////////////////////////////////////////////

	team = 0;

	// set position and velocity
	m_2position = startpos;
	m_2velocity = startvel;

	// initial acceleration is zero
	m_2acceleration = Vector2D(0,0);

	// set the PhysicalEntity 3D positon
	Position = D3DXVECTOR3(m_2position.x, m_2position.y, 
					terrain->GetHeight(m_2position.x, m_2position.y) + BOID_RADIUS*2);

	///////////////////////////////////////////////////////////
	// Temporary addition for calculating energy expenditure
	lastPos = Position;
	energy = 0;
	TERRAIN_DRAG_FACTOR = BOID_INITIAL_TERRAIN_DRAG_FACTOR;
	DOWNHILL_TERRAIN_DRAG_FACTOR = BOID_INITIAL_DOWNHILL_DRAG_FACTOR;
	///////////////////////////////////////////////////////////
}

// Update() must be called every frame, but
// Update_Behaviors() may be called less often, but preferably still at least several times a second
// Update_Behaviors needs to be called frequently.  This is where the animal
// makes decisions and updates its behavior and current acceleration
void Boid::Update_Behaviors()
{
	// at this point just get new acceleration based on basis behaviors
	m_2acceleration = navigator();
}

// Update needs to be called every frame.  This is where the animal's position
// is updated and animated
void Boid::Update(float elapsed_time)
{
	Profile_Scope(boid_update);

	//////////////////////////////////////////////////////////////////
	// Drag Force behavior

	// we want to take into account the gradient of the terrain at the boid's current position
	Vector2D grad(terrain->GetGradient(this->m_2position.x, this->m_2position.y));
	// slope in the direction of the gradient
	float slope = grad.magnitude();
	
	//m_2acceleration -= grad * 4 / (1+ slope*slope);// * slope;

	if(slope > 0) // if the slope is zero, then nothing to do
	{
		//Vector2D unitgrad = grad / slope; // normalized gradient
		
		// component of current velocity in the direction of the gradient
		float dot = DotProduct(this->m_2velocity, grad); 
		Vector2D new_a;
		if(dot > 0) // boid is traveling uphill
		{
			new_a = (-TERRAIN_DRAG_FACTOR * dot / slope) * grad ;
		}
		else if(dot < 0) // boid is traveling downhill
		{
			new_a = (-DOWNHILL_TERRAIN_DRAG_FACTOR * dot / slope) * grad;
		}

		m_2acceleration += new_a;
	}

	/////////////////////////////////////////////////////////////////////////////////////
	// update position
	float grad_velcomp = DotProduct(this->m_2velocity.normalized(), grad); // component of the gradient in the direction of the current velocity
										// in other words, the slope in the direction the boid is travelling.

	// the 1/sqrtf(s*s + 1) adjustments are there to account for the fact that the two dimensional
	//  velocity and acc are actually moving the boids over 3D terrain.  This effectively reduces the speed
	//  by a function of the slope in that direction.
	// a possible optimization would be to approximate that 1/sqrt(s*s + 1) factor

	m_2position += m_2velocity*elapsed_time / sqrtf(grad_velcomp*grad_velcomp + 1);
	///////////////////////////////////////////////////////////////////////////////////////////

	// update velocity
	m_2velocity += m_2acceleration * elapsed_time;

	// Cap off velocity at maximum allowed value
	if (m_2velocity.squared_magnitude() > (BOID_MAX_SPEED*BOID_MAX_SPEED))
	{
		m_2velocity.setMagnitude(BOID_MAX_SPEED);
    }

	// set the Physical entity positon
	Position = D3DXVECTOR3(m_2position.x, m_2position.y, 
					terrain->GetHeight(m_2position.x, m_2position.y) + BOID_RADIUS*2);

	////////////////////////////////////////////////////////////
	// Temporary addition for calculating energy expenditure
	//D3DXVECTOR3 disp = Position - lastPos;
	//lastPos = Position;
	//float distance = sqrtf(D3DXVec3LengthSq(&disp));

	//if(distance > 0)
	//{
	//	float movement_slope = disp.z / sqrtf(D3DXVec3LengthSq(&disp) - SQUARED(disp.z)); // rise over run

	//	// energy expended is linear with distance traveled, and slope determines the constant, so we need to
	//	// come up with some multiplying factor as a function of the slope.
	//	float slope_factor = -(atanf(6.0f * (movement_slope + 0.34f)) - 5.0f * atanf(8.0f * (movement_slope - 0.45f)) - 3.0f*PI)/(PI);
	//	
	//	//// lets try some piecewise linear function
	//	//// less than x1, the energy expenditure is at some maximum for downhil movement
	//	//// at slope of x2, energy expenditure is at a minimum
	//	//// greater than x3, the energy expenditure is at some maximum for uphill movement
	//	//const float x1 = -1.0f, x2 = -0.1f, x3 = 1.0f;
	//	//if(movement_slope < x1) // below this we just use a constant
	//	//{
	//	//	slope_factor = 3;
	//	//}
	//	//else if(movement_slope < x2) // in the range [x1, x2)
	//	//{
	//	//	const float m = (1.0f-3.0f)/(x2 - x1);
	//	//	const float b = 3.0f - m*(x1);
	//	//	slope_factor = m * movement_slope + b; // (y = m*x + b)
	//	//}
	//	//else if(movement_slope < x3) // in the range [x2, x3)
	//	//{
	//	//	const float m = (4.0f-1.0f)/(x3 - x2);
	//	//	const float b = 1.0f - m*x2;
	//	//	slope_factor = m * movement_slope + b; // (y = m*x + b)
	//	//}
	//	//else // in the range [x3, infinity)
	//	//{
	//	//	slope_factor = 4;
	//	//}

	//	energy += distance * slope_factor;
	//}
	////////////////////////////////////////////////////////////

	// see if it's time to update behavior
	last_behavior_update = (last_behavior_update + 1) % behavior_update_delay;
	if(0 == last_behavior_update)
		Update_Behaviors();
}

// this is the function that combines the basis behaviors
// it combines valueToAdd with the running total contained in accumulator
// and returns the magnitude of the resulting value of accumulator
float Boid::accumulate(Vector2D& accumulator, const Vector2D& valueToAdd)
{
	Profile_Scope(boid_accumulate);

	// we want to come up with some combination of these two
	// vectors but we do not want the final result's magnitude
	// to be greater than 1.0

	// so first, we add the two together and check if we're within
	// the limit.
	Vector2D temp = accumulator + valueToAdd;
	if(temp.squared_magnitude() <= 1.0)
	{
		// we are, so go ahead and return this result
		accumulator = temp;
		return temp.magnitude();
	}
	else
	{
		// we're not, so we want to add on just enough of the valueToAdd
		// to bring our magnitude to 1.0
		// to do this, we want to find a value t such that the magnitude of
		// accumulator + t*valueToAdd  is 1.0
		// solving for this value t requires solving a quadratic equation,
		// here are the 3 coefficients:
		float a = valueToAdd.squared_magnitude();
		float b = 2 * DotProduct(valueToAdd, accumulator);
		float c = accumulator.squared_magnitude()-1;
		// quadratic formula
		float t = (float) (-b + sqrt(b*b - 4*a*c))/(2*a);

		accumulator += valueToAdd*t;
		return 1.0f;
	}
}

// finds all animals in parentFlock which are to be considered
// neigbors of this animal and stores this list of animals in neighborList
void Boid::computeNeighbors()
{
	Profile_Scope(boid_compute_neigbors);

	// loop through all the members of the flock
	list<Boid *>::iterator i;
	for(i = (parentFlock->members.begin()); i != parentFlock->members.end(); i++)
	{
		Boid *pBoid = *i;
	
		// don't bother adding this boid itself to it's own list of its neighbors
		if(pBoid == this)
			continue;

		// get vector pointing from this boid to it's flockmate
		Vector2D dir = pBoid->m_2position - this->m_2position;

		// if this other boid is visible then it's a neighbor
		if( dir.squared_magnitude() <= SQUARED(BOID_VISIBLE_RANGE) && // if it's close enough
			this->m_2velocity.angle(dir) < BOID_VIEWING_ANGLE) // and if it's in the field of view
		{
			neighborList.push_back(pBoid); // then add it to the list of neighbors
		}
	}
}

// computes new acceleration for the boid each frame
// it is called by Update_Behaviors()
Vector2D Boid::navigator()
{
	Profile_Scope(boid_navigator);

	Vector2D acc(0,0);
	float mag;

	computeNeighbors();

	
	
		mag = accumulate(acc, avoidObstacles());

	if(mag < 1.0)
		mag = accumulate(acc, keepDistance());

	if(mag < 1.0)
		mag = accumulate(acc, matchHeading());
		
	if(mag < 1.0)
		mag = accumulate(acc, steerToCenter());

	if(mag < 1.0)
		mag = accumulate(acc, gotoGoal());

	// IMPORTANT:
  // Since the FlockCentering, CollisionAvoidance, and VelocityMatching modules
  // return a vector whose magnitude is a percentage of the maximum acceleration,
  // we need to convert this to a real magnitude 
	acc *= BOID_MAX_ACCELERTION;

	neighborList.clear();

	return acc;
}

extern Forest* MainForest;
// steer arounds trees
Vector2D Boid::avoidObstacles()
{
	Profile_Scope(boid_avoid_obstacles);

	Tree** Trees;
	int TreeCount = MainForest->GetTrees(Trees); // Get the list of trees
	int i;
	Vector2D force(0,0); // accumulative force of all the trees
	
	for(i=0; i<TreeCount; i++) // for each tree
	{
		// get the posititon and radius of this tree
		Vector2D treepos = Vector2D(Trees[i]->Position.x, Trees[i]->Position.y);
		float radius = (Trees[i]->Height / 10.0f) + (BOID_RADIUS);
		// we add BOID_RADIUS, because we want the boid to aim not for the exact
		// edge of the tree, but to allow enough room for boid to get past the edge

		// disp is the vector pointing from the boid's position to the tree's center
		Vector2D disp = treepos - this->m_2position;

		// get the squared distance to the tree
		float disp_mag_squared = disp.squared_magnitude();
		
		// check emergency condition of colliding with the tree.
		if(disp_mag_squared <= SQUARED(radius))
		{
			// in this case we want to accelerate directly away

			// d = distance from current position to the nearest edge of the tree
			//float d = sqrtf(disp_mag_squared) - radius;

			// add a force directly away from the tree
			force -= disp * (100 * SQUARED(BOID_RADIUS) / disp_mag_squared);

			continue; // with next tree
		}

		// otherwise we are not colliding yet

		// check if the direction to the tree is behind the boid.  if so, we can ignore it
		// and just go on to the next tree
		if(DotProduct(disp, this->m_2velocity) <= 0)
			continue; // with next tree

		// if we get to this point we know that the direction to the tree make an angle
		// with the velocity vector of less than 90 degrees.


		// find the projection of the tree onto the line perpendicular to disp
		float projected_r;
		if(disp_mag_squared < 4 * SQUARED(radius))  // estimate if we are getting "close" to the tree
		{
			// we're getting too close to the circle for approximations, we need to be more exact
			projected_r = sqrtf(SQUARED(radius) * disp_mag_squared / (disp_mag_squared - SQUARED(radius)));
		}
		else if(disp_mag_squared < SQUARED(BOID_VISIBLE_TREE_RANGE))
		{
			// use a bigger radius than the actual radius just to give some comfort space
			projected_r = radius * 1.3f;
		}
		else // the tree is not within visible range
		{
			continue; // with next tree
		}
		
		// here comes the tricky part.  Basically we want to project the circle of the tree
		// onto a line perpendicular to the velocity.  If the tree projects entirely to the left
		// or to the right of the current position, then the boid is not heading on a collision course

		// this is the distance to the tree
		float disp_mag = sqrtf(disp_mag_squared);

		// use it to calculate the unit vector pointing to the tree
        Vector2D unit_disp = disp / disp_mag; // unit vector pointing towards center of tree

		// d = unit vector pointing to the tree but rotated 90 degrees
		Vector2D d = Vector2D(unit_disp.y, -unit_disp.x); // rotate 90 degrees

		// projection of the velocity on a line perpendicular to the direction to the tree
		//float vel_proj = DotProduct(d, this->m_2velocity);  // the projection of the current velocity

		// we need to calculate the 2 points to the either side of the tree that we will project
		Vector2D	point1 = (d * projected_r) + disp;
		Vector2D	point2 = (-d * projected_r) + disp;
		
		// rotate the velocity 90 degrees so that we have a vector perpendicular to the velocity
		Vector2D	right(m_2velocity.y, -m_2velocity.x);
		// we don't need to normalize it because we only care about relative distances
		
		// p1 and p2 are the projections of point1 and point2
		float p1 = DotProduct(point1, right);
		float p2 = DotProduct(point2, right);

		// if one projection is positive and the other is negative then the velocity is pointing at 
		// the tree.  Otherwise we can ignore this tree
		if((p1<0 && p2>0) || (p1>0 && p2<0))
		{
			// okay, we're looking at the tree.  Now, which edge of the tree is closer to the current
			// direction the boid is facing.
			Vector2D new_vel;

			//// vector to the goal
			//Vector2D	vec_to_goal = parentFlock->team_goals[team] - this->m_2position;
			//float goal_proj = DotProduct(vec_to_goal, right);

			//// use the point that is closest to the goal
			//if(fabs(p1-goal_proj) > fabs(p2-goal_proj))
			//	new_vel = point2;
			//else
			//	new_vel = point1;

			// use the point that will give the shortest path to the goal
			//if(point1.magnitude() + (vec_to_goal-point1).magnitude() < point2.magnitude() + (vec_to_goal-point2).magnitude())
			//	new_vel = point1;
			//else
			//	new_vel = point2;

			// use the point that is closer to the velocity direction (the one whose projection
			// is closest to zero)
			if(fabs(p1) > fabs(p2))
				new_vel = point2;
			else
				new_vel = point1;

			// we've got a vector pointing the direction we want.  Now to set its magnitude.
			// we can just use the boid's current speed.
			new_vel.setMagnitude(this->m_2velocity.magnitude());

			// now apply a force pushing the current velocity towards this new velocity
			// (use the radius/distance ratio so that nearer trees have a greater impact)
			force += 10000*(new_vel - m_2velocity)*(radius/disp_mag);
		}
	}

	// limit at magnitude 1
	if(force.squared_magnitude() > 1)
		force.setMagnitude(1.0f);

	return force;
}


//// TOTO: finish this function.  WARNING: this fuction is not complete!!!
//Vector2D Boid::avoidCircularObstacle(Vector2D pos, float radius)
//{
//	Vector2D disp = pos - this->m_2position;
//	float disp_mag_2 = disp.squared_magnitude();
//
//	float projected_r;
//
//	if(disp_mag_2 <= SQUARED(radius))
//	{
//		// we're inside the circle! head directly out
//		//return
//	}
//	else if(disp_mag_2 < 4 * SQUARED(radius))
//	{
//		// we're getting too close to the circle for approximations, we need to be more exact
//		projected_r = sqrtf(SQUARED(radius) * disp_mag_2 / (disp_mag_2 - SQUARED(radius)));
//	}
//	else
//		projected_r = radius * 1.5f;
//
//	return Vector2D(0,0);
//}

Vector2D Boid::keepDistance()
{
	Profile_Scope(boid_keep_distance);

	if(neighborList.size() == 0)
		return Vector2D(0,0);

	Vector2D change(0,0);

	list<Boid *>::iterator i;
	for(i = neighborList.begin(); i != neighborList.end(); i++)
	{
		Boid* pBoid = *i;
		Vector2D dist = pBoid->m_2position - this->m_2position;
		float mag2 = dist.squared_magnitude();
		if(mag2 < SQUARED(BOID_COMFORT_RADIUS))
		{
			float newmag = (SQUARED(BOID_COMFORT_RADIUS) - mag2) /
							(SQUARED(BOID_COMFORT_RADIUS) - SQUARED(BOID_DANGER_RADIUS));
			if(newmag > 1.0f) newmag = 1.0f;
			dist.setMagnitude(newmag);
			change -= dist;
			//if(1 <= accumulate(change, -dist))
			//	break;
		}
	}
	if(change.squared_magnitude() > 1)
		change.setMagnitude(1.1f);
	return change;
}

Vector2D Boid::matchHeading()
{
	Profile_Scope(boid_match_heading);

	if(neighborList.size() == 0)
		return Vector2D(0,0);

	Vector2D ave(0,0);// = this->m_token.velocity;
	int count = 0;

	list<Boid *>::iterator i;
	for(i = neighborList.begin(); i != neighborList.end(); i++)
	{
		Boid* pBoid = *i;
		if(pBoid->team == this->team)
		{
			ave += pBoid->m_2velocity;
			count++;
		}
	}
	if(count == 0)
		return Vector2D(0,0);

	ave /= (float)count;
	Vector2D change = (ave - this->m_2velocity)/(BOID_MAX_SPEED/2);
	if(change.squared_magnitude() > 1)
		change.setMagnitude(1);
	return change;
}

Vector2D Boid::steerToCenter()
{
	Profile_Scope(boid_steer_to_center);

	if(neighborList.size() == 0)
		return Vector2D(0,0);

	Vector2D ave(0,0);// = this->m_token.position;
	int count = 0;

	list<Boid *>::iterator i;
	for(i = neighborList.begin(); i != neighborList.end(); i++)
	{
		Boid* pBoid = *i;
		if(pBoid->team == this->team)
		{
			ave += pBoid->m_2position;
			count++;
		}
	}
	if(0 == count)
		return Vector2D(0,0);

	ave /= (float)count;
	Vector2D change = (ave - this->m_2position)/50;// /(BOID_VISIBLE_RANGE/5000);
	if(change.squared_magnitude() > 1)
		change.setMagnitude(1);
	return change;
}

Vector2D Boid::gotoGoal()
{
	Profile_Scope(boid_goto_goal);

	Vector2D newV;

	newV = (parentFlock->team_goals[this->team] - m_2position);

	if(newV.squared_magnitude() > SQUARED(BOID_CRUISING_SPEED))
		newV.setMagnitude(BOID_CRUISING_SPEED);

	Vector2D change = (newV - this->m_2velocity)/(BOID_MAX_SPEED/2);
	if(change.squared_magnitude() > 1)
		change.setMagnitude(1);
	return change;
}

Vector2D Boid::getPos() const
{
	return this->m_2position;
}

///////////////////////////////////////////////////////
// functions required for PhysicalEntity
D3DXVECTOR3 Boid::GetGroundContactPoint()
{
	D3DXVECTOR3 Result;
	Result = Position;
	Result.z -= BOID_RADIUS;
	return Result;
}

void Boid::GetMeshes(Mesh** MeshArray)
{
	MeshArray[0] = m_mesh;
}

int Boid::GetMeshCount()
{
	return 1;
}

unsigned int Boid::GetAction(float TimeStep, Msg** SentMsgs)
{
	Update(TimeStep);
	return 0;
}

unsigned int Boid::SendMsg(Msg* RecievedMsg, Msg** SentMsgs)
{
	return 0;
}
///////////////////////////////////////////////////////

//
void Boid::reset()
{
	m_2position = Vector2D(3 * (float(rand()) / float(RAND_MAX)) +3.5f,
							3 * (float(rand()) / float(RAND_MAX)) +3.5f);

	m_2velocity = Vector2D(0,0);
	m_2acceleration = Vector2D(0,0);

	Position = D3DXVECTOR3(m_2position.x, m_2position.y, 
					terrain->GetHeight(m_2position.x, m_2position.y) + BOID_RADIUS*2);

	///////////////////////////////////
	// Temporary addition for calculating energy expenditure
	lastPos = Position;
	energy = 0;
	//////////////////////////////////////////////////////
}