You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1060 lines
31 KiB
1060 lines
31 KiB
/*==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); |
|
}
|
|
|