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/>.

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);
}