/*==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);
}