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 "HeadSpin.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 "pnEncryption/plRandom.h"
#include "pnMessage/plEnableMsg.h"
#include "plMessage/plAnimCmdMsg.h"
#include "plMessage/plLoadCloneMsg.h"

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

#include <cmath>
#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, &fPos);
        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 = 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);
}

bool 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
    {
        hsPoint3 selfpos = Position();
        hsPoint3 otherpos = other.Position();
        const hsVector3 offset(&otherpos, &selfpos);
        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

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

hsVector3 pfBoid::ISteerToGoal(pfBoidGoal &goal, float maxPredictionTime)
{
    // offset from this to quarry, that distance, unit vector toward quarry
    hsPoint3 gpos = goal.Position();
    hsPoint3 pos = Position();
    const hsVector3 offset(&gpos, &pos);
    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)
            hsPoint3 pos = Position();
            hsPoint3 otherpos = (**other).Position();
            const hsVector3 offset(&otherpos, &pos);
            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)
    {
        hsPoint3 pos = Position();
        hsPoint3 zero(0, 0, 0);
        hsVector3 posVector(&pos, &zero); // 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 = 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 = 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_t val)
{
    fNumBoids = val;
}

bool pfObjectFlocker::MsgReceive(plMessage* msg)
{
    plInitialAgeStateLoadedMsg* loadMsg = plInitialAgeStateLoadedMsg::ConvertNoRef(msg);
    if (loadMsg)
    {
        plEnableMsg* pMsg = 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 = new plLoadCloneMsg(fBoidKey->GetUoid(), GetKey(), 0);
            plKey newKey = cloneMsg->GetCloneKey();
            cloneMsg->Send();

            float xAdjust = (2 * RAND()) - 1; // produces a random number between -1 and 1
            float yAdjust = (2 * RAND()) - 1;
            float 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 = 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);
}

bool pfObjectFlocker::IEval(double secs, float del, uint32_t 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->ReadLEScalar());
    fFlock.SetWanderWeight(s->ReadLEScalar());

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

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

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

    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->WriteLEScalar(fFlock.GoalWeight());
    s->WriteLEScalar(fFlock.WanderWeight());

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

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

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

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