CWE-ou-minkata/Sources/Plasma/FeatureLib/pfAnimation/pfObjectFlocker.cpp

/*==LICENSE==*

CyanWorlds.com Engine - MMOG client, server and tools
Copyright (C) 2011 Cyan Worlds, Inc.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.

Additional permissions under GNU GPL version 3 section 7

If you modify this Program, or any covered work, by linking or
combining it with any of RAD Game Tools Bink SDK, Autodesk 3ds Max SDK,
NVIDIA PhysX SDK, Microsoft DirectX SDK, OpenSSL library, Independent
JPEG Group JPEG library, Microsoft Windows Media SDK, or Apple QuickTime SDK
(or a modified version of those libraries),
containing parts covered by the terms of the Bink SDK EULA, 3ds Max EULA,
PhysX SDK EULA, DirectX SDK EULA, OpenSSL and SSLeay licenses, IJG
JPEG Library README, Windows Media SDK EULA, or QuickTime SDK EULA, the
licensors of this Program grant you additional
permission to convey the resulting work. Corresponding Source for a
non-source form of such a combination shall include the source code for
the parts of OpenSSL and IJG JPEG Library used as well as that of the covered
work.

You can contact Cyan Worlds, Inc. by email legal@cyan.com
 or by snail mail at:
      Cyan Worlds, Inc.
      14617 N Newport Hwy
      Mead, WA   99021

*==LICENSE==*/
#include "hsTypes.h"
#include "hsStlUtils.h"
#include "hsMatrix44.h"
#include "hsGeometry3.h"
#include "plgDispatch.h"
#include "hsResMgr.h"
#include "../pnMessage/plTimeMsg.h"
#include "../pnMessage/plRefMsg.h"
#include "../plMessage/plAgeLoadedMsg.h"
#include "../pnSceneObject/plSceneObject.h"
#include "hsTimer.h"
#include "../plMath/plRandom.h"
#include "../pnMessage/plEnableMsg.h"
#include "../plMessage/plAnimCmdMsg.h"
#include "../plMessage/plLoadCloneMsg.h"

//#include "../plPipeline/plDebugGeometry.h"

#include <math.h>
#include <algorithm>

#include "pfObjectFlocker.h"

#define PI        3.14159f
#define HALF_PI   (PI/2)
#define GRAVITY   9.806650f  // meters/second
#ifdef INFINITY
#undef INFINITY
#endif
#define INFINITY  999999.0f

#define RAND() (float) (rand()/(RAND_MAX * 1.0))
#define SIGN(x) (((x) < 0) ? -1 : 1)

const int pfObjectFlocker::fFileVersion = 1;

#define FLOCKER_SHOW_DEBUG_LINES 0

#if FLOCKER_SHOW_DEBUG_LINES
// make a few easy-to-use colors for the debug lines
#define DEBUG_COLOR_RED			255,   0,   0
#define DEBUG_COLOR_GREEN		  0, 255,   0
#define DEBUG_COLOR_BLUE		  0,   0, 255
#define DEBUG_COLOR_YELLOW		255, 255,   0
#define DEBUG_COLOR_CYAN		  0, 255, 255
#define DEBUG_COLOR_PINK		255,   0, 255
#endif

///////////////////////////////////////////////////////////////////////////////
// Some quick utility functions
///////////////////////////////////////////////////////////////////////////////

// Basic linear interpolation
template<class T>
inline T Interpolate(float alpha, const T& x0, const T& x1)
{
	return x0 + ((x1 - x0) * alpha);
}

// Clip a value to the min and max, if necessary
inline float Clip(const float x, const float min, const float max)
{
	if (x < min) return min;
	if (x > max) return max;
	return x;
}

inline float ScalarRandomWalk(const float initial, const float walkspeed, const float min, const float max)
{
	const float next = initial + (((RAND() * 2) - 1) * walkspeed);
	if (next < min) return min;
	if (next > max) return max;
	return next;
}

// Classify a value relative to the interval between two bounds:
//     returns -1 when below the lower bound
//     returns  0 when between the bounds (inside the interval)
//     returns +1 when above the upper bound
inline int IntervalComparison (float x, float lowerBound, float upperBound)
{
	if (x < lowerBound) return -1;
	if (x > upperBound) return +1;
	return 0;
}

// Blend the new value into the accumulator using the smooth rate
// If smoothRate is 0 the accumulator will not change.
// If smoothRate is 1 the accumulator will be set to the new value with no smoothing.
// Useful values are "near zero".
template<class T>
inline void BlendIntoAccumulator(const float smoothRate, const T &newValue, T &smoothedAccumulator)
{
	smoothedAccumulator = Interpolate(Clip(smoothRate, 0, 1), smoothedAccumulator, newValue);
}

// return component of vector parallel to a unit basis vector
// (IMPORTANT NOTE: assumes "basis" has unit magnitude (length==1))
inline hsVector3 ParallelComponent(const hsVector3 &vec, const hsVector3 &unitBasis)
{
	const float projection = vec * unitBasis;
	return unitBasis * projection;
}

// return component of vector perpendicular to a unit basis vector
// (IMPORTANT NOTE: assumes "basis" has unit magnitude (length==1))
inline hsVector3 PerpendicularComponent(const hsVector3 &vec, const hsVector3& unitBasis)
{
	return vec - ParallelComponent(vec, unitBasis);
}

// clamps the length of a given vector to maxLength.  If the vector is
// shorter its value is returned unaltered, if the vector is longer
// the value returned has length of maxLength and is parallel to the
// original input.
hsVector3 TruncateLength (const hsVector3 &vec, const float maxLength)
{
	const float maxLengthSquared = maxLength * maxLength;
	const float vecLengthSquared = vec.MagnitudeSquared();
	if (vecLengthSquared <= maxLengthSquared)
		return vec;
	else
		return vec * (maxLength / sqrt(vecLengthSquared));
}

// Enforce an upper bound on the angle by which a given arbitrary vector
// diviates from a given reference direction (specified by a unit basis
// vector).  The effect is to clip the "source" vector to be inside a cone
// defined by the basis and an angle.
hsVector3 LimitMaxDeviationAngle(const hsVector3 &vec, const float cosineOfConeAngle, const hsVector3 &basis)
{
	// immediately return zero length input vectors
	float sourceLength = vec.Magnitude();
	if (sourceLength == 0) return vec;

	// measure the angular diviation of "source" from "basis"
	const hsVector3 direction = vec / sourceLength;
	float cosineOfSourceAngle = direction * basis;

	// Simply return "source" if it already meets the angle criteria.
	if (cosineOfSourceAngle >= cosineOfConeAngle) return vec;

	// find the portion of "source" that is perpendicular to "basis"
	const hsVector3 perp = PerpendicularComponent(vec, basis);

	// normalize that perpendicular
	hsVector3 unitPerp = perp;
	unitPerp.Normalize();

	// construct a new vector whose length equals the source vector,
	// and lies on the intersection of a plane (formed the source and
	// basis vectors) and a cone (whose axis is "basis" and whose
	// angle corresponds to cosineOfConeAngle)
	float perpDist = sqrt(1 - (cosineOfConeAngle * cosineOfConeAngle));
	const hsVector3 c0 = basis * cosineOfConeAngle;
	const hsVector3 c1 = unitPerp * perpDist;
	return (c0 + c1) * sourceLength;
}

///////////////////////////////////////////////////////////////////////////////
// pfVehicle functions
///////////////////////////////////////////////////////////////////////////////

void pfVehicle::IMeasurePathCurvature(const float elapsedTime)
{
	if (elapsedTime > 0)
	{
		const hsVector3 deltaPosition(&fLastPos, &Position());
		const hsVector3 deltaForward = (fLastForward - Forward()) / deltaPosition.Magnitude();
		const hsVector3 lateral = PerpendicularComponent(deltaForward, Forward());
		const float sign = ((lateral * Side()) < 0) ? 1.0f : -1.0f;
		fCurvature = lateral.Magnitude() * sign;
		BlendIntoAccumulator(elapsedTime * 4.0f, fCurvature, fSmoothedCurvature);
		fLastForward = Forward();
		fLastPos = Position();
	}
}

// Reset functions
void pfVehicle::Reset()
{
	ResetLocalSpace();

	SetMass(1); // defaults to 1 so acceleration = force
	SetSpeed(0); // speed along the forward direction

	SetMaxForce(10.0f); // steering force is clipped to this magnitude
	SetMaxSpeed(5.0f); // velocity is clipped to this magnitude

	SetRadius(0.5f); // size of bounding sphere

	// Reset bookkeeping for our averages
	ResetSmoothedPosition();
	ResetSmoothedCurvature();
	ResetSmoothedAcceleration();
}

float pfVehicle::ResetSmoothedCurvature(float value /* = 0 */)
{
	fLastForward.Set(0, 0, 0);
	fLastPos.Set(0, 0, 0);
	return fSmoothedCurvature = fCurvature = value;
}

hsVector3 pfVehicle::ResetSmoothedAcceleration(const hsVector3 &value /* = hsVector3(0,0,0) */)
{
	return fSmoothedAcceleration = value;
}

hsPoint3 pfVehicle::ResetSmoothedPosition(const hsPoint3 &value /* = hsPoint3(0,0,0) */)
{
	return fSmoothedPosition = value;
}

void pfVehicle::ResetLocalSpace()
{
	fSide.Set(1, 0, 0);
	fForward.Set(0, 1, 0);
	fUp.Set(0, 0, 1);
	fPos.Set(0, 0, 0);
}

// Geometry functions
void pfVehicle::SetUnitSideFromForwardAndUp()
{
	// derive new unit side vector from forward and up
	fSide = fForward % fUp;
	fSide.Normalize();
}

void pfVehicle::RegenerateOrthonormalBasisUF(const hsVector3 &newUnitForward)
{
	fForward = newUnitForward;

	// derive new side vector from NEW forward and OLD up
	SetUnitSideFromForwardAndUp();

	// derive new up vector from new side and new forward (should have unit length since side and forward are
	// perpendicular and unit length)
	fUp = fSide % fForward;
}

void pfVehicle::RegenerateLocalSpace(const hsVector3 &newVelocity, const float /*elapsedTime*/)
{
	// adjust orthonormal basis vectors to be aligned with new velocity
	if (Speed() > 0)
		RegenerateOrthonormalBasisUF(newVelocity / Speed());
}

void pfVehicle::RegenerateLocalSpaceForBanking(const hsVector3 &newVelocity, const float elapsedTime)
{
	// the length of this global-upward-pointing vector controls the vehicle's
	// tendency to right itself as it is rolled over from turning acceleration
	const hsVector3 globalUp(0, 0, 0.2f);

	// acceleration points toward the center of local path curvature, the
	// length determines how much the vehicle will roll while turning
	const hsVector3 accelUp = fSmoothedAcceleration * 0.05f;

	// combined banking, sum of up due to turning and global up
	const hsVector3 bankUp = accelUp + globalUp;

	// blend bankUp into vehicle's up vector
	const float smoothRate = elapsedTime * 3;
	hsVector3 tempUp = Up();
	BlendIntoAccumulator(smoothRate, bankUp, tempUp);
	tempUp.Normalize();
	SetUp(tempUp);

	// adjust orthonormal basis vectors to be aligned with new velocity
	if (Speed() > 0)
		RegenerateOrthonormalBasisUF(newVelocity / Speed());
}

// Physics functions
void pfVehicle::ApplySteeringForce(const hsVector3 &force, const float deltaTime)
{
	const hsVector3 adjustedForce = AdjustRawSteeringForce(force, deltaTime);

	// enforce limit on magnitude of steering force
	const hsVector3 clippedForce = TruncateLength(adjustedForce, MaxForce());

#if FLOCKER_SHOW_DEBUG_LINES
	// Draw the adjusted force vector
	plDebugGeometry::Instance()->DrawLine(Position(), Position() + clippedForce, DEBUG_COLOR_GREEN);
#endif

	// compute acceleration and velocity
	hsVector3 newAcceleration = (clippedForce / Mass());
	hsVector3 newVelocity = Velocity();

	// damp out abrupt changes and oscillations in steering acceleration
	// (rate is proportional to time step, then clipped into useful range)
	if (deltaTime > 0)
	{
		const float smoothRate = Clip(9 * deltaTime, 0.15f, 0.4f);
		BlendIntoAccumulator(smoothRate, newAcceleration, fSmoothedAcceleration);
	}

	// Euler integrate (per frame) acceleration into velocity
	newVelocity += fSmoothedAcceleration * deltaTime;

	// enforce speed limit
	newVelocity = TruncateLength(newVelocity, MaxSpeed());

	// update Speed
	SetSpeed(newVelocity.Magnitude());

	// Euler integrate (per frame) velocity into position
	SetPosition(Position() + (newVelocity * deltaTime));

	// regenerate local space (by default: align vehicle's forward axis with
	// new velocity, but this behavior may be overridden by derived classes.)
	RegenerateLocalSpace(newVelocity, deltaTime);

	// maintain path curvature information
	IMeasurePathCurvature(deltaTime);

	// running average of recent positions
	BlendIntoAccumulator(deltaTime * 0.06f, Position(), fSmoothedPosition);
}

hsVector3 pfVehicle::AdjustRawSteeringForce(const hsVector3 &force, const float deltaTime)
{
	const float maxAdjustedSpeed = 0.2f * MaxSpeed();

	if ((Speed() > maxAdjustedSpeed) || (force == hsVector3(0,0,0)))
		return force; // no adjustment needed if they are going above 20% of max speed
	else
	{
		const float range = Speed() / maxAdjustedSpeed; // make sure they don't turn too much if below 20% of max speed
		const float cosine = Interpolate(pow(range, 20), 1.0f, -1.0f);
		return LimitMaxDeviationAngle(force, cosine, Forward());
	}
}

void pfVehicle::ApplyBrakingForce(const float rate, const float deltaTime)
{
	const float rawBraking = Speed() * rate;
	const float clipBraking = ((rawBraking < MaxForce()) ? rawBraking : MaxForce());

	SetSpeed(Speed() - (clipBraking * deltaTime));
}

hsPoint3 pfVehicle::PredictFuturePosition(const float predictionTime)
{
	return Position() + (Velocity() * predictionTime);
}

///////////////////////////////////////////////////////////////////////////////
// pfBoidGoal functions
///////////////////////////////////////////////////////////////////////////////

pfBoidGoal::pfBoidGoal()
{
	fLastPos.Set(0, 0, 0);
	fCurPos.Set(0, 0, 0);
	fSpeed = 0;
	fHasLastPos = false; // our last pos doesn't make sense yet
}

void pfBoidGoal::Update(plSceneObject *goal, float deltaTime)
{
	if (!fHasLastPos) // the last pos is invalid, so we need to init now
	{
		fLastPos = fCurPos = goal->GetLocalToWorld().GetTranslate();
		fSpeed = 0;
		fForward.Set(1,0,0); // make a unit vector, it shouldn't matter that it's incorrect as this is only for one frame
		fHasLastPos = true;
		return;
	}

	fLastPos = fCurPos;
	fCurPos = goal->GetLocalToWorld().GetTranslate();
	hsVector3 change(&fCurPos, &fLastPos);
	float unadjustedSpeed = change.Magnitude();
	fSpeed = unadjustedSpeed / deltaTime; // update speed (mag is in meters, time is in seconds)
	if (unadjustedSpeed == 0)
		return; // if our speed is zero, don't recalc our forward vector (leave it as it was last time)
	fForward = change / unadjustedSpeed;

#if FLOCKER_SHOW_DEBUG_LINES
	// Show where we are predicting the location to be in 1 second
	plDebugGeometry::Instance()->DrawLine(Position(), PredictFuturePosition(1), DEBUG_COLOR_BLUE);
#endif
}

hsPoint3 pfBoidGoal::PredictFuturePosition(const float predictionTime)
{
	return fCurPos + (fForward * fSpeed * predictionTime);
}

///////////////////////////////////////////////////////////////////////////////
// pfBoid functions
///////////////////////////////////////////////////////////////////////////////

pfBoid::pfBoid(pfProximityDatabase& pd, pfObjectFlocker *flocker, plKey &key)
{
	// allocate a token for this boid in the proximity database
	fProximityToken = NULL;
	ISetupToken(pd);

	Reset();

	fFlockerPtr = flocker;
	fObjKey = key;

	IFlockDefaults();

	fProximityToken->UpdateWithNewPosition(Position());
}

pfBoid::pfBoid(pfProximityDatabase& pd, pfObjectFlocker *flocker, plKey &key, hsPoint3 &pos)
{
	// allocate a token for this boid in the proximity database
	fProximityToken = NULL;
	ISetupToken(pd);

	Reset();

	fFlockerPtr = flocker;
	fObjKey = key;

	SetPosition(pos);

	IFlockDefaults();

	fProximityToken->UpdateWithNewPosition(Position());
}

pfBoid::pfBoid(pfProximityDatabase& pd, pfObjectFlocker *flocker, plKey &key, hsPoint3 &pos, float speed, hsVector3 &forward, hsVector3 &side, hsVector3 &up)
{
	// allocate a token for this boid in the proximity database
	fProximityToken = NULL;
	ISetupToken(pd);

	Reset();

	fFlockerPtr = flocker;
	fObjKey = key;

	SetPosition(pos);
	SetSpeed(speed);
	SetForward(forward);
	SetSide(side);
	SetUp(up);

	IFlockDefaults();

	fProximityToken->UpdateWithNewPosition(Position());
}

pfBoid::~pfBoid()
{
	// delete this boid's token in the proximity database
	delete fProximityToken;

	plLoadCloneMsg* msg = TRACKED_NEW plLoadCloneMsg(fObjKey, fFlockerPtr->GetKey(), 0, false);
	msg->Send();
}

void pfBoid::IFlockDefaults()
{
	fSeparationRadius = 5.0f;
	fSeparationAngle = -0.707f;
	fSeparationWeight = 12.0f;

	fCohesionRadius = 9.0f;
	fCohesionAngle = -0.15f;
	fCohesionWeight = 8.0f;

	fGoalWeight = 8.0f;
	fRandomWeight = 12.0f;
}

void pfBoid::ISetupToken(pfProximityDatabase &pd)
{
	// delete this boid's token in the old proximity database
	delete fProximityToken;

	// allocate a token for this boid in the proximity database
	fProximityToken = pd.MakeToken(this);
}

hsBool pfBoid::IInBoidNeighborhood(const pfVehicle &other, const float minDistance, const float maxDistance, const float cosMaxAngle)
{
	if (&other == this) // abort if we're looking at ourselves
		return false;
	else
	{
		const hsVector3 offset(&(other.Position()), &Position());
		const float distanceSquared = offset.MagnitudeSquared();

		// definitely in neighborhood if inside minDistance sphere
		if (distanceSquared < (minDistance * minDistance))
			return true;
		else
		{
			// definitely not in neighborhood if outside maxDistance sphere
			if (distanceSquared > (maxDistance * maxDistance))
				return false;
			else
			{
				// otherwise, test angular offset from forward axis (can we "see" it?)
				const hsVector3 unitOffset = offset / sqrt(distanceSquared);
				const float forwardness = Forward() * unitOffset;
				return forwardness > cosMaxAngle;
			}
		}
	}
}

// Steering functions
hsVector3 pfBoid::ISteerForWander(float timeDelta)
{
	// random walk the fWanderSide and fWanderUp variables between -1 and +1
	const float speed = 10 * timeDelta; // the 10 value found through experimentation
	fWanderSide = ScalarRandomWalk(fWanderSide, speed, -1, +1);
	fWanderUp = ScalarRandomWalk(fWanderUp, speed, -1, +1);

	hsVector3 force = (Side() * fWanderSide) + (Up() * fWanderUp);

#if FLOCKER_SHOW_DEBUG_LINES
	// Draw the random walk component
	plDebugGeometry::Instance()->DrawLine(Position(), Position()+force, DEBUG_COLOR_PINK);
#endif

	return force;
}

hsVector3 pfBoid::ISteerForSeek(const hsPoint3 &target)
{
#if FLOCKER_SHOW_DEBUG_LINES
	// Draw to where we are steering towards
	plDebugGeometry::Instance()->DrawLine(Position(), target, DEBUG_COLOR_RED);
#endif

	const hsVector3 desiredVelocity(&target, &Position());
	return desiredVelocity - Velocity();
}

hsVector3 pfBoid::ISteerToGoal(pfBoidGoal &goal, float maxPredictionTime)
{
	// offset from this to quarry, that distance, unit vector toward quarry
	const hsVector3 offset(&goal.Position(), &Position());
	const float distance = offset.Magnitude();
	if (distance == 0) // nowhere to go
		return hsVector3(0, 0, 0);
	const hsVector3 unitOffset = offset / distance;

	// how parallel are the paths of "this" and the goal
	// (1 means parallel, 0 is pependicular, -1 is anti-parallel after later calculations)
	const float parallelness = Forward() * goal.Forward();

	// how "forward" is the direction to the quarry
	// (1 means dead ahead, 0 is directly to the side, -1 is straight back after later calculations)
	const float forwardness = Forward() * unitOffset;

	float speed = Speed();
	if (speed == 0)
		speed = 0.00001; // make it really small in case we start out not moving
	const float directTravelTime = distance / speed;
	const int f = IntervalComparison(forwardness,  -0.707f, 0.707f); // -1 if below -0.707f, 0 if between, and +1 if above 0.707f)
	const int p = IntervalComparison(parallelness, -0.707f, 0.707f); // 0.707 is basically cos(45deg) (45deg = PI/4)

	float timeFactor = 0; // to be filled in below - how far ahead to predict position so it looks good

	// Break the pursuit into nine cases, the cross product of the
	// quarry being [ahead, aside, or behind] us and heading
	// [parallel, perpendicular, or anti-parallel] to us.
	switch (f)
	{
	case +1:
		switch (p)
		{
		case +1: // ahead, parallel
			timeFactor = 4;
			break;
		case 0: // ahead, perpendicular
			timeFactor = 1.8f;
			break;
		case -1: // ahead, anti-parallel
			timeFactor = 0.85f;
			break;
		}
		break;
	case 0:
		switch (p)
		{
		case +1: // aside, parallel
			timeFactor = 1;
			break;
		case 0: // aside, perpendicular
			timeFactor = 0.8f;
			break;
		case -1: // aside, anti-parallel
			timeFactor = 4;
			break;
		}
		break;
	case -1:
		switch (p)
		{
		case +1: // behind, parallel
			timeFactor = 0.5f;
			break;
		case 0: // behind, perpendicular
			timeFactor = 2;
			break;
		case -1: // behind, anti-parallel
			timeFactor = 2;
			break;
		}
		break;
	}

	// estimated time until intercept of quarry
	const float et = directTravelTime * timeFactor;
	const float etl = (et > maxPredictionTime) ? maxPredictionTime : et;

	// estimated position of quarry at intercept
	const hsPoint3 target = goal.PredictFuturePosition(etl);

	// steer directly for our target (which is a point ahead of the object we are pursuing)
	return ISteerForSeek(target);
}

hsVector3 pfBoid::ISteerForSeparation(const float maxDistance, const float cosMaxAngle, const std::vector<pfVehicle*> &flock)
{
	// steering accumulator and count of neighbors, both initially zero
	hsVector3 steering(0, 0, 0);
	int neighbors = 0;

	// for each of the other vehicles...
	for (std::vector<pfVehicle*>::const_iterator other = flock.begin(); other != flock.end(); other++)
	{
		if (IInBoidNeighborhood(**other, Radius() * 3, maxDistance, cosMaxAngle))
		{
			// add in steering contribution
			// (opposite of the offset direction, divided once by distance
			// to normalize, divided another time to get 1/d falloff)
			const hsVector3 offset(&((**other).Position()), &Position());
			const float distanceSquared = offset * offset;
			steering += (offset / -distanceSquared);

			// count neighbors
			neighbors++;
		}
	}

	// divide by neighbors, then normalize to pure direction
	if (neighbors > 0)
	{
		steering = (steering / (float)neighbors);
		steering.Normalize();
	}

#if FLOCKER_SHOW_DEBUG_LINES
	// Draw the random walk component
	plDebugGeometry::Instance()->DrawLine(Position(), Position()+steering, DEBUG_COLOR_CYAN);
#endif

	return steering;
}

hsVector3 pfBoid::ISteerForCohesion(const float maxDistance, const float cosMaxAngle, const std::vector<pfVehicle*> &flock)
{
	// steering accumulator and count of neighbors, both initially zero
	hsVector3 steering(0, 0, 0);
	int neighbors = 0;

	// for each of the other vehicles...
	for (std::vector<pfVehicle*>::const_iterator other = flock.begin(); other != flock.end(); other++)
	{
		if (IInBoidNeighborhood(**other, Radius() * 3, maxDistance, cosMaxAngle))
		{
			// accumulate sum of neighbor's positions
			steering += (**other).Position();

			// count neighbors
			neighbors++;
		}
	}

	// divide by neighbors, subtract off current position to get error-
	// correcting direction, then normalize to pure direction
	if (neighbors > 0)
	{
		hsVector3 posVector(&(Position()), &(hsPoint3(0,0,0))); // quick hack to turn a point into a vector
		steering = ((steering / (float)neighbors) - posVector);
		steering.Normalize();
	}

#if FLOCKER_SHOW_DEBUG_LINES
	// Draw the random walk component
	plDebugGeometry::Instance()->DrawLine(Position(), Position()+steering, DEBUG_COLOR_YELLOW);
#endif

	return steering;
}

// Used for frame-by-frame updates; no time deltas on positions.
void pfBoid::Update(pfBoidGoal &goal, float deltaTime)
{
	const float maxTime = 20; // found by testing

	// find all flockmates within maxRadius using proximity database
	fNeighbors.clear();
	fProximityToken->FindNeighbors(Position(), fSeparationRadius, fNeighbors);

	hsVector3 goalVector = ISteerToGoal(goal, maxTime);
	hsVector3 randomVector = ISteerForWander(deltaTime);
	hsVector3 separationVector = ISteerForSeparation(fSeparationRadius, fSeparationAngle, fNeighbors);
	hsVector3 cohesionVector = ISteerForCohesion(fCohesionRadius, fCohesionAngle, fNeighbors);

	hsVector3 steeringVector = (fGoalWeight * goalVector) + (fRandomWeight * randomVector) +
		(fSeparationWeight * separationVector) + (fCohesionWeight * cohesionVector);

	ApplySteeringForce(steeringVector, deltaTime);

	fProximityToken->UpdateWithNewPosition(Position());
}

void pfBoid::RegenerateLocalSpace(const hsVector3 &newVelocity, const float elapsedTime)
{
	RegenerateLocalSpaceForBanking(newVelocity, elapsedTime);
}

///////////////////////////////////////////////////////////////////////////////
// pfFlock functions
///////////////////////////////////////////////////////////////////////////////
pfFlock::pfFlock() :
fGoalWeight(8.0f),
fRandomWeight(12.0f),
fSeparationRadius(5.0f),
fSeparationWeight(12.0f),
fCohesionRadius(9.0f),
fCohesionWeight(8.0f),
fMaxForce(10.0f),
fMaxSpeed(5.0f),
fMinSpeed(4.0f)
{
	 fDatabase = TRACKED_NEW pfBasicProximityDatabase<pfVehicle*>();
}

pfFlock::~pfFlock()
{
	int flock_size = fBoids.size();
	for (int i = 0; i < flock_size; i++)
	{
		delete fBoids[i];
		fBoids[i] = nil;
	}
	fBoids.clear();

	delete fDatabase;
	fDatabase = NULL;
}

void pfFlock::SetGoalWeight(float goalWeight)
{
	for (int i = 0; i < fBoids.size(); i++)
		fBoids[i]->SetGoalWeight(goalWeight);
	fGoalWeight = goalWeight;
}

void pfFlock::SetWanderWeight(float wanderWeight)
{
	for (int i = 0; i < fBoids.size(); i++)
		fBoids[i]->SetWanderWeight(wanderWeight);
	fRandomWeight = wanderWeight;
}

void pfFlock::SetSeparationWeight(float weight)
{
	for (int i = 0; i < fBoids.size(); i++)
		fBoids[i]->SetSeparationWeight(weight);
	fSeparationWeight = weight;
}

void pfFlock::SetSeparationRadius(float radius)
{
	for (int i = 0; i < fBoids.size(); i++)
		fBoids[i]->SetSeparationRadius(radius);
	fSeparationRadius = radius;
}

void pfFlock::SetCohesionWeight(float weight)
{
	for (int i = 0; i < fBoids.size(); i++)
		fBoids[i]->SetCohesionWeight(weight);
	fCohesionWeight = weight;
}

void pfFlock::SetCohesionRadius(float radius)
{
	for (int i = 0; i < fBoids.size(); i++)
		fBoids[i]->SetCohesionRadius(radius);
	fCohesionRadius = radius;
}

void pfFlock::SetMaxForce(float force)
{
	for (int i = 0; i < fBoids.size(); i++)
		fBoids[i]->SetMaxForce(force);
	fMaxForce = force;
}

void pfFlock::SetMaxSpeed(float speed)
{
	for (int i = 0; i < fBoids.size(); i++)
	{
		float speedAdjust = (fMaxSpeed - fMinSpeed) * RAND();
		fBoids[i]->SetMaxSpeed(speed - speedAdjust);
	}
	fMaxSpeed = speed;
}

void pfFlock::SetMinSpeed(float minSpeed)
{
	fMinSpeed = minSpeed;
}

void pfFlock::Update(plSceneObject *goal, float deltaTime)
{
	// update the goal data
	fBoidGoal.Update(goal, deltaTime);

	// update the flock
	float delta = (deltaTime > 0.3f) ? 0.3f : deltaTime;
	std::vector<pfBoid*>::iterator i;

	for (i = fBoids.begin(); i != fBoids.end(); i++)
		(*i)->Update(fBoidGoal, delta);
}

void pfFlock::AddBoid(pfObjectFlocker *flocker, plKey &key, hsPoint3 &pos)
{
	pfBoid *newBoid = TRACKED_NEW pfBoid(*fDatabase, flocker, key, pos);

	newBoid->SetGoalWeight(fGoalWeight);
	newBoid->SetWanderWeight(fRandomWeight);

	newBoid->SetSeparationWeight(fSeparationWeight);
	newBoid->SetSeparationRadius(fSeparationRadius);

	newBoid->SetCohesionWeight(fCohesionWeight);
	newBoid->SetCohesionRadius(fCohesionRadius);

	newBoid->SetMaxForce(fMaxForce);
	float speedAdjust = (fMaxSpeed - fMinSpeed) * RAND();
	newBoid->SetMaxSpeed(fMaxSpeed - speedAdjust);

	fBoids.push_back(newBoid);
}

pfBoid *pfFlock::GetBoid(int i)
{
	if (i >= 0 && i < fBoids.size())
		return fBoids[i];
	else
		return nil;
}

pfObjectFlocker::pfObjectFlocker() :
fUseTargetRotation(false),
fRandomizeAnimationStart(true),
fNumBoids(0)
{
}

pfObjectFlocker::~pfObjectFlocker()
{
	plgDispatch::Dispatch()->UnRegisterForExactType(plEvalMsg::Index(), GetKey());
}

void pfObjectFlocker::SetNumBoids(UInt8 val)
{
	fNumBoids = val;
}

hsBool pfObjectFlocker::MsgReceive(plMessage* msg)
{
	plInitialAgeStateLoadedMsg* loadMsg = plInitialAgeStateLoadedMsg::ConvertNoRef(msg);
	if (loadMsg)
	{
		plEnableMsg* pMsg = TRACKED_NEW plEnableMsg;
		pMsg->AddReceiver(fBoidKey);
		pMsg->SetCmd(plEnableMsg::kDrawable);
		pMsg->AddType(plEnableMsg::kDrawable);
		pMsg->SetBCastFlag(plMessage::kPropagateToModifiers | plMessage::kPropagateToChildren);
		pMsg->SetCmd(plEnableMsg::kDisable);
		pMsg->Send();

		hsPoint3 pos(fTarget->GetLocalToWorld().GetTranslate());
		for (int i = 0; i < fNumBoids; i++)
		{
			plLoadCloneMsg* cloneMsg = TRACKED_NEW plLoadCloneMsg(fBoidKey->GetUoid(), GetKey(), 0);
			plKey newKey = cloneMsg->GetCloneKey();
			cloneMsg->Send();

			hsScalar xAdjust = (2 * RAND()) - 1; // produces a random number between -1 and 1
			hsScalar yAdjust = (2 * RAND()) - 1;
			hsScalar zAdjust = (2 * RAND()) - 1;
			hsPoint3 boidPos(pos.fX + xAdjust, pos.fY + yAdjust, pos.fZ + zAdjust);
			fFlock.AddBoid(this, newKey, boidPos);
		}
		plgDispatch::Dispatch()->UnRegisterForExactType(plInitialAgeStateLoadedMsg::Index(), GetKey());
		plgDispatch::Dispatch()->RegisterForExactType(plEvalMsg::Index(), GetKey());

		return true;
	}

	plLoadCloneMsg* lcMsg = plLoadCloneMsg::ConvertNoRef(msg);
	if (lcMsg && lcMsg->GetIsLoading())
	{
		if (fRandomizeAnimationStart)
		{
			plAnimCmdMsg* pMsg = TRACKED_NEW plAnimCmdMsg;
			pMsg->SetSender(GetKey());
			pMsg->SetBCastFlag(plMessage::kPropagateToModifiers | plMessage::kPropagateToChildren);
			pMsg->AddReceiver( lcMsg->GetCloneKey() );
			pMsg->SetCmd(plAnimCmdMsg::kGoToPercent);
			pMsg->fTime = RAND();
			pMsg->Send();
		}
	}

	return plSingleModifier::MsgReceive(msg);
}

hsBool pfObjectFlocker::IEval(double secs, hsScalar del, UInt32 dirty)
{
	fFlock.Update(fTarget, del);

	plSceneObject* boidSO = nil;
	for (int i = 0; i < fNumBoids; i++)
	{
		pfBoid* boid = fFlock.GetBoid(i);
		boidSO = plSceneObject::ConvertNoRef(boid->GetKey()->VerifyLoaded());

		hsMatrix44 l2w;
		hsMatrix44 w2l;

		if (fUseTargetRotation)
			l2w = fTarget->GetLocalToWorld();
		else
		{
			l2w = boidSO->GetLocalToWorld();
			hsVector3 forward = boid->Forward();
			hsVector3 up = boid->Up();
			hsVector3 side = boid->Side();

			// copy the vectors over
			for(int i = 0; i < 3; i++)
			{
				l2w.fMap[i][0] = side[i];
				l2w.fMap[i][1] = forward[i];
				l2w.fMap[i][2] = up[i];
			}
		}

		hsScalarTriple pos = boid->Position();
		l2w.SetTranslate(&pos);
		l2w.GetInverse(&w2l);

		boidSO->SetTransform(l2w, w2l);
	}

	return true;
}


void pfObjectFlocker::SetTarget(plSceneObject* so)
{
	plSingleModifier::SetTarget(so);

	if( fTarget )
	{
		plgDispatch::Dispatch()->RegisterForExactType(plInitialAgeStateLoadedMsg::Index(), GetKey());
	}
}

void pfObjectFlocker::Read(hsStream* s, hsResMgr* mgr)
{
	plSingleModifier::Read(s, mgr);

	int version = s->ReadByte();
	hsAssert(version <= fFileVersion, "Flocker data is newer then client, please update your client");

	SetNumBoids(s->ReadByte());
	fBoidKey = mgr->ReadKey(s);

	fFlock.SetGoalWeight(s->ReadSwapScalar());
	fFlock.SetWanderWeight(s->ReadSwapScalar());

	fFlock.SetSeparationWeight(s->ReadSwapScalar());
	fFlock.SetSeparationRadius(s->ReadSwapScalar());

	fFlock.SetCohesionWeight(s->ReadSwapScalar());
	fFlock.SetCohesionRadius(s->ReadSwapScalar());

	fFlock.SetMaxForce(s->ReadSwapScalar());
	fFlock.SetMaxSpeed(s->ReadSwapScalar());
	fFlock.SetMinSpeed(s->ReadSwapScalar());

	fUseTargetRotation = s->ReadBool();
	fRandomizeAnimationStart = s->ReadBool();
}

void pfObjectFlocker::Write(hsStream* s, hsResMgr* mgr)
{
	plSingleModifier::Write(s, mgr);

	s->WriteByte(fFileVersion);

	s->WriteByte(fNumBoids);
	mgr->WriteKey(s, fBoidKey);

	s->WriteSwapScalar(fFlock.GoalWeight());
	s->WriteSwapScalar(fFlock.WanderWeight());

	s->WriteSwapScalar(fFlock.SeparationWeight());
	s->WriteSwapScalar(fFlock.SeparationRadius());

	s->WriteSwapScalar(fFlock.CohesionWeight());
	s->WriteSwapScalar(fFlock.CohesionRadius());

	s->WriteSwapScalar(fFlock.MaxForce());
	s->WriteSwapScalar(fFlock.MaxSpeed());
	s->WriteSwapScalar(fFlock.MinSpeed());

	s->WriteBool(fUseTargetRotation);
	s->WriteBool(fRandomizeAnimationStart);
}