// ----------------------------------------------------------------------------
// avoidance of "close neighbors" -- used only by steerToAvoidNeighbors
//
// XXX Does a hard steer away from any other agent who comes withing a
// XXX critical distance. Ideally this should be replaced with a call
// XXX to steerForSeparation.
public Vector3 steerToAvoidCloseNeighbors(float minSeparationDistance, ArrayList others)
{
// for each of the other vehicles...
//for (AVIterator i = others.begin(); i != others.end(); i++)
for (int i = 0; i < others.Count; i++)
{
AbstractVehicle other = (AbstractVehicle)others[i];
if (other != this)
{
float sumOfRadii = radius() + other.radius();
float minCenterToCenter = minSeparationDistance + sumOfRadii;
Vector3 offset = other.Position - Position;
float currentDistance = offset.Length;
if (currentDistance < minCenterToCenter)
{
annotateAvoidCloseNeighbor(other, minSeparationDistance);
return(OpenSteerUtility.perpendicularComponent(-offset, forward()));
}
}
}
// otherwise return zero
return(Vector3.Zero);
}
// Given two vehicles, based on their current positions and velocities,
// determine the time until nearest approach
//
// XXX should this return zero if they are already in contact?
float predictNearestApproachTime(AbstractVehicle other)
{
// imagine we are at the origin with no velocity,
// compute the relative velocity of the other vehicle
Vector3 myVelocity = velocity();
Vector3 otherVelocity = other.velocity();
Vector3 relVelocity = otherVelocity - myVelocity;
float relSpeed = relVelocity.Length;
// for parallel paths, the vehicles will always be at the same distance,
// so return 0 (aka "now") since "there is no time like the present"
if (relSpeed == 0)
{
return(0);
}
// Now consider the path of the other vehicle in this relative
// space, a line defined by the relative position and velocity.
// The distance from the origin (our vehicle) to that line is
// the nearest approach.
// Take the unit tangent along the other vehicle's path
Vector3 relTangent = relVelocity / relSpeed;
// find distance from its path to origin (compute offset from
// other to us, find length of projection onto path)
Vector3 relPosition = Position - other.Position;
float projection = relTangent.Dot(relPosition);
return(projection / relSpeed);
}
// ----------------------------------------------------------------------------
// used by boid behaviors: is a given vehicle within this boid's neighborhood?
bool inBoidNeighborhood(AbstractVehicle other, float minDistance, float maxDistance, float cosMaxAngle)
{
if (other == this)
{
return(false);
}
else
{
Vector3 offset = other.Position - Position;
float distanceSquared = offset.LengthSquared;
// 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
Vector3 unitOffset = offset / (float)System.Math.Sqrt(distanceSquared);
float forwardness = forward().Dot(unitOffset);
return(forwardness > cosMaxAngle);
}
}
}
}
// ----------------------------------------------------------------------------
// Separation behavior: steer away from neighbors
public Vector3 steerForSeparation(float maxDistance, float cosMaxAngle, ArrayList flock)
{
// steering accumulator and count of neighbors, both initially zero
Vector3 steering = Vector3.Zero;
int neighbors = 0;
// for each of the other vehicles...
//for (AVIterator other = flock.begin(); other != flock.end(); other++)
for (int i = 0; i < flock.Count; i++)
{
AbstractVehicle other = (AbstractVehicle)flock[i];
if (inBoidNeighborhood(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)
Vector3 offset = (other).Position - Position;
float distanceSquared = offset.Dot(offset);
steering += (offset / -distanceSquared);
// count neighbors
neighbors++;
}
}
// divide by neighbors, then normalize to pure direction
if (neighbors > 0)
{
steering = (steering / (float)neighbors);
steering.Normalize();
}
return(steering);
}
// ----------------------------------------------------------------------------
// Cohesion behavior: to to move toward center of neighbors
public Vector3 steerForCohesion(float maxDistance, float cosMaxAngle, ArrayList flock)
{
// steering accumulator and count of neighbors, both initially zero
Vector3 steering = Vector3.Zero;
int neighbors = 0;
// for each of the other vehicles...
// for (AVIterator other = flock.begin(); other != flock.end(); other++)
for (int i = 0; i < flock.Count; i++)
{
AbstractVehicle other = (AbstractVehicle)flock[i];
if (inBoidNeighborhood(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)
{
steering = ((steering / (float)neighbors) - Position);
steering.Normalize();
}
return(steering);
}
// XXX 4-23-03: Temporary work around (see comment above)
//
// Checks for intersection of the given spherical obstacle with a
// volume of "likely future vehicle positions": a cylinder along the
// current path, extending minTimeToCollision seconds along the
// forward axis from current position.
//
// If they intersect, a collision is imminent and this function returns
// a steering force pointing laterally away from the obstacle's center.
//
// Returns a zero vector if the obstacle is outside the cylinder
//
// xxx couldn't this be made more compact using localizePosition?
Vector3 steerToAvoid(AbstractVehicle v, float minTimeToCollision)
{
// minimum distance to obstacle before avoidance is required
float minDistanceToCollision = minTimeToCollision * v.speed();
float minDistanceToCenter = minDistanceToCollision + radius;
// contact distance: sum of radii of obstacle and vehicle
float totalRadius = radius + v.radius();
// obstacle center relative to vehicle position
Vector3 localOffset = center - v.Position;
// distance along vehicle's forward axis to obstacle's center
float forwardComponent = localOffset.DotProduct(v.forward());
Vector3 forwardOffset = forwardComponent * v.forward();
// offset from forward axis to obstacle's center
Vector3 offForwardOffset = localOffset - forwardOffset;
// test to see if sphere overlaps with obstacle-free corridor
bool inCylinder = offForwardOffset.Length < totalRadius;
bool nearby = forwardComponent < minDistanceToCenter;
bool inFront = forwardComponent > 0;
// if all three conditions are met, steer away from sphere center
if (inCylinder && nearby && inFront)
{
return(offForwardOffset * -1);
}
else
{
return(Vector3.ZERO);
}
}
// constructor
public tokenType(AbstractVehicle parentObject, BruteForceProximityDatabase pd)
{
// store pointer to our associated database and the object this
// token represents, and store this token on the database's vector
bfpd = pd;
tParentObject = parentObject;
bfpd.group.Add(this);
}
public override AbstractVehicle getNearestVehicle(Vector3 position, float radius)
{
lqClientProxy tProxy = lq.lqFindNearestNeighborWithinRadius(position.x, position.y, position.z, radius, null);
AbstractVehicle tVehicle = null;
if (tProxy != null)
{
tVehicle = (AbstractVehicle)tProxy.clientObject;
}
return(tVehicle);
}
// Given the time until nearest approach (predictNearestApproachTime)
// determine position of each vehicle at that time, and the distance
// between them
float computeNearestApproachPositions(AbstractVehicle other, float time)
{
Vector3 myTravel = forward() * speed() * time;
Vector3 otherTravel = other.forward() * other.speed() * time;
Vector3 myFinal = Position + myTravel;
Vector3 otherFinal = other.Position + otherTravel;
// xxx for annotation
ourPositionAtNearestApproach = myFinal;
hisPositionAtNearestApproach = otherFinal;
return((myFinal - otherFinal).Length);//Vector3::distance (myFinal, otherFinal);
}
// ----------------------------------------------------------------------------
// evasion of another vehicle
public Vector3 steerForEvasion(AbstractVehicle menace, float maxPredictionTime)
{
// offset from this to menace, that distance, unit vector toward menace
Vector3 offset = menace.Position - Position;
float distance = offset.Length;
float roughTime = distance / menace.speed();
float predictionTime = ((roughTime > maxPredictionTime) ?
maxPredictionTime :
roughTime);
Vector3 target = menace.predictFuturePosition(predictionTime);
return(steerForFlee(target));
}
public Vector3 steerForPursuit(AbstractVehicle quarry, float maxPredictionTime)
{
// offset from this to quarry, that distance, unit vector toward quarry
Vector3 offset = quarry.Position - Position;
float distance = offset.Length;
Vector3 unitOffset = offset / distance;
// how parallel are the paths of "this" and the quarry
// (1 means parallel, 0 is pependicular, -1 is anti-parallel)
float parallelness = forward().Dot(quarry.forward());
// how "forward" is the direction to the quarry
// (1 means dead ahead, 0 is directly to the side, -1 is straight back)
float forwardness = forward().Dot(unitOffset);
float directTravelTime = distance / speed();
int f = intervalComparison(forwardness, -0.707f, 0.707f);
int p = intervalComparison(parallelness, -0.707f, 0.707f);
float timeFactor = 0; // to be filled in below
Vector3 color = OpenSteerColours.gBlack; // to be filled in below (xxx just for debugging)
// 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;
color = OpenSteerColours.gBlack;
break;
case 0: // ahead, perpendicular
timeFactor = 1.8f;
color = OpenSteerColours.gGray50;
break;
case -1: // ahead, anti-parallel
timeFactor = 0.85f;
color = OpenSteerColours.gWhite;
break;
}
break;
case 0:
switch (p)
{
case +1: // aside, parallel
timeFactor = 1;
color = OpenSteerColours.gRed;
break;
case 0: // aside, perpendicular
timeFactor = 0.8f;
color = OpenSteerColours.gYellow;
break;
case -1: // aside, anti-parallel
timeFactor = 4;
color = OpenSteerColours.gGreen;
break;
}
break;
case -1:
switch (p)
{
case +1: // behind, parallel
timeFactor = 0.5f;
color = OpenSteerColours.gCyan;
break;
case 0: // behind, perpendicular
timeFactor = 2;
color = OpenSteerColours.gBlue;
break;
case -1: // behind, anti-parallel
timeFactor = 2;
color = OpenSteerColours.gMagenta;
break;
}
break;
}
// estimated time until intercept of quarry
float et = directTravelTime * timeFactor;
// xxx experiment, if kept, this limit should be an argument
float etl = (et > maxPredictionTime) ? maxPredictionTime : et;
// estimated position of quarry at intercept
Vector3 target = quarry.predictFuturePosition(etl);
// annotation
annotationLine(Position,
target,
gaudyPursuitAnnotation ? color : OpenSteerColours.gGray40);
return(steerForSeek(target));
}
// ----------------------------------------------------------------------------
// pursuit of another vehicle (& version with ceiling on prediction time)
public Vector3 steerForPursuit(AbstractVehicle quarry)
{
return(steerForPursuit(quarry, float.MaxValue));
}
public Vector3 steerForPursuit( AbstractVehicle quarry, float maxPredictionTime)
{
// offset from this to quarry, that distance, unit vector toward quarry
Vector3 offset = quarry.Position - Position;
float distance = offset.Length;
Vector3 unitOffset = offset / distance;
// how parallel are the paths of "this" and the quarry
// (1 means parallel, 0 is pependicular, -1 is anti-parallel)
float parallelness = forward().DotProduct (quarry.forward());
// how "forward" is the direction to the quarry
// (1 means dead ahead, 0 is directly to the side, -1 is straight back)
float forwardness = forward().DotProduct (unitOffset);
float directTravelTime = distance / speed ();
int f = intervalComparison (forwardness, -0.707f, 0.707f);
int p = intervalComparison (parallelness, -0.707f, 0.707f);
float timeFactor = 0; // to be filled in below
Vector3 color=OpenSteerColours.gBlack; // to be filled in below (xxx just for debugging)
// 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;
color = OpenSteerColours.gBlack;
break;
case 0: // ahead, perpendicular
timeFactor = 1.8f;
color = OpenSteerColours.gGray50;
break;
case -1: // ahead, anti-parallel
timeFactor = 0.85f;
color = OpenSteerColours.gWhite;
break;
}
break;
case 0:
switch (p)
{
case +1: // aside, parallel
timeFactor = 1;
color = OpenSteerColours.gRed;
break;
case 0: // aside, perpendicular
timeFactor = 0.8f;
color = OpenSteerColours.gYellow;
break;
case -1: // aside, anti-parallel
timeFactor = 4;
color = OpenSteerColours.gGreen;
break;
}
break;
case -1:
switch (p)
{
case +1: // behind, parallel
timeFactor = 0.5f;
color = OpenSteerColours.gCyan;
break;
case 0: // behind, perpendicular
timeFactor = 2;
color = OpenSteerColours.gBlue;
break;
case -1: // behind, anti-parallel
timeFactor = 2;
color = OpenSteerColours.gMagenta;
break;
}
break;
}
// estimated time until intercept of quarry
float et = directTravelTime * timeFactor;
// xxx experiment, if kept, this limit should be an argument
float etl = (et > maxPredictionTime) ? maxPredictionTime : et;
// estimated position of quarry at intercept
Vector3 target = quarry.predictFuturePosition (etl);
// annotation
annotationLine (Position,
target,
gaudyPursuitAnnotation ? color : OpenSteerColours.gGray40);
return steerForSeek (target);
}
// called when steerToAvoidCloseNeighbors decides steering is required
// (default action is to do nothing, layered classes can overload it)
public virtual void annotateAvoidCloseNeighbor(AbstractVehicle otherVehicle, float seperationDistance)
{
}
// called when steerToAvoidNeighbors decides steering is required
// (default action is to do nothing, layered classes can overload it)
public virtual void annotateAvoidNeighbor( AbstractVehicle vehicle, float steer,Vector3 position, Vector3 threatPosition)
{
}
// Given the time until nearest approach (predictNearestApproachTime)
// determine position of each vehicle at that time, and the distance
// between them
float computeNearestApproachPositions(AbstractVehicle other, float time)
{
Vector3 myTravel = forward () * speed () * time;
Vector3 otherTravel = other.forward () * other.speed () * time;
Vector3 myFinal = Position + myTravel;
Vector3 otherFinal = other.Position + otherTravel;
// xxx for annotation
ourPositionAtNearestApproach = myFinal;
hisPositionAtNearestApproach = otherFinal;
return (myFinal - otherFinal).Length;//Vector3::distance (myFinal, otherFinal);
}
// ----------------------------------------------------------------------------
// used by boid behaviors: is a given vehicle within this boid's neighborhood?
bool inBoidNeighborhood( AbstractVehicle other, float minDistance, float maxDistance, float cosMaxAngle)
{
if (other == this)
{
return false;
}
else
{
Vector3 offset = other.Position - Position;
float distanceSquared = offset.SquaredLength;
// 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
Vector3 unitOffset = offset / (float) System.Math.Sqrt (distanceSquared);
float forwardness = forward().DotProduct (unitOffset);
return forwardness > cosMaxAngle;
}
}
}
}
// ----------------------------------------------------------------------------
// pursuit of another vehicle (& version with ceiling on prediction time)
public Vector3 steerForPursuit( AbstractVehicle quarry)
{
return steerForPursuit (quarry, float.MaxValue);
}
// allocate a token to represent a given client object in this database
//public override tokenType allocateToken (Object parentObject)
public override AbstractTokenForProximityDatabase allocateToken(AbstractVehicle parentObject)
{
tokenType tToken = new tokenType(parentObject, this);
return((AbstractTokenForProximityDatabase)tToken);
}
// Given two vehicles, based on their current positions and velocities,
// determine the time until nearest approach
//
// XXX should this return zero if they are already in contact?
float predictNearestApproachTime(AbstractVehicle other)
{
// imagine we are at the origin with no velocity,
// compute the relative velocity of the other vehicle
Vector3 myVelocity = velocity();
Vector3 otherVelocity = other.velocity();
Vector3 relVelocity = otherVelocity - myVelocity;
float relSpeed = relVelocity.Length;
// for parallel paths, the vehicles will always be at the same distance,
// so return 0 (aka "now") since "there is no time like the present"
if (relSpeed == 0) return 0;
// Now consider the path of the other vehicle in this relative
// space, a line defined by the relative position and velocity.
// The distance from the origin (our vehicle) to that line is
// the nearest approach.
// Take the unit tangent along the other vehicle's path
Vector3 relTangent = relVelocity / relSpeed;
// find distance from its path to origin (compute offset from
// other to us, find length of projection onto path)
Vector3 relPosition = Position - other.Position;
float projection = relTangent.DotProduct(relPosition);
return projection / relSpeed;
}
// allocate a token to represent a given client object in this database
public override AbstractTokenForProximityDatabase allocateToken(AbstractVehicle parentObject)
{
return new tokenType(parentObject, this);
}
// type for the "tokens" manipulated by this spatial database
//typedef AbstractTokenForProximityDatabase<ContentType> tokenType;
// allocate a token to represent a given client object in this database
public virtual AbstractTokenForProximityDatabase allocateToken(AbstractVehicle parentObject)
{
return new AbstractTokenForProximityDatabase();
}
// constructor
public tokenType(AbstractVehicle parentObject, BruteForceProximityDatabase pd)
{
// store pointer to our associated database and the object this
// token represents, and store this token on the database's vector
bfpd = pd;
tParentObject = parentObject;
bfpd.group.Add(this);
}
// XXX 4-23-03: Temporary work around (see comment above)
//
// Checks for intersection of the given spherical obstacle with a
// volume of "likely future vehicle positions": a cylinder along the
// current path, extending minTimeToCollision seconds along the
// forward axis from current position.
//
// If they intersect, a collision is imminent and this function returns
// a steering force pointing laterally away from the obstacle's center.
//
// Returns a zero vector if the obstacle is outside the cylinder
//
// xxx couldn't this be made more compact using localizePosition?
Vector3 steerToAvoid(AbstractVehicle v, float minTimeToCollision)
{
// minimum distance to obstacle before avoidance is required
float minDistanceToCollision = minTimeToCollision * v.speed();
float minDistanceToCenter = minDistanceToCollision + radius;
// contact distance: sum of radii of obstacle and vehicle
float totalRadius = radius + v.radius();
// obstacle center relative to vehicle position
Vector3 localOffset = center - v.Position;
// distance along vehicle's forward axis to obstacle's center
float forwardComponent = localOffset.DotProduct(v.forward());
Vector3 forwardOffset = forwardComponent * v.forward();
// offset from forward axis to obstacle's center
Vector3 offForwardOffset = localOffset - forwardOffset;
// test to see if sphere overlaps with obstacle-free corridor
bool inCylinder = offForwardOffset.Length < totalRadius;
bool nearby = forwardComponent < minDistanceToCenter;
bool inFront = forwardComponent > 0;
// if all three conditions are met, steer away from sphere center
if (inCylinder && nearby && inFront)
{
return offForwardOffset * -1;
}
else
{
return Vector3.ZERO;
}
}
// called when steerToAvoidCloseNeighbors decides steering is required
// (default action is to do nothing, layered classes can overload it)
public virtual void annotateAvoidCloseNeighbor(AbstractVehicle otherVehicle, float seperationDistance)
{
}
// type for the "tokens" manipulated by this spatial database
//typedef AbstractTokenForProximityDatabase<ContentType> tokenType;
// allocate a token to represent a given client object in this database
public virtual AbstractTokenForProximityDatabase allocateToken(AbstractVehicle parentObject)
{
return(new AbstractTokenForProximityDatabase());
}
// called when steerToAvoidNeighbors decides steering is required
// (default action is to do nothing, layered classes can overload it)
public virtual void annotateAvoidNeighbor(AbstractVehicle vehicle, float steer, Vector3 position, Vector3 threatPosition)
{
}
// ----------------------------------------------------------------------------
// Unaligned collision avoidance behavior: avoid colliding with other nearby
// vehicles moving in unconstrained directions. Determine which (if any)
// other other vehicle we would collide with first, then steers to avoid the
// site of that potential collision. Returns a steering force vector, which
// is zero length if there is no impending collision.
public Vector3 steerToAvoidNeighbors(float minTimeToCollision, ArrayList others)
{
// first priority is to prevent immediate interpenetration
Vector3 separation = steerToAvoidCloseNeighbors(0, others);
if (separation != Vector3.Zero)
{
return(separation);
}
// otherwise, go on to consider potential future collisions
float steer = 0;
AbstractVehicle threat = null;
// Time (in seconds) until the most immediate collision threat found
// so far. Initial value is a threshold: don't look more than this
// many frames into the future.
float minTime = minTimeToCollision;
// xxx solely for annotation
Vector3 xxxThreatPositionAtNearestApproach = new Vector3();
Vector3 xxxOurPositionAtNearestApproach = new Vector3();
// for each of the other vehicles, determine which (if any)
// pose the most immediate threat of collision.
//for (AVIterator i = others.begin(); i != others.end(); i++)
for (int i = 0; i < others.Count; i++)
{
AbstractVehicle other = (AbstractVehicle)others[i];
if (other != this)
{
// avoid when future positions are this close (or less)
float collisionDangerThreshold = radius() * 2;
// predicted time until nearest approach of "this" and "other"
float time = predictNearestApproachTime(other);
// If the time is in the future, sooner than any other
// threatened collision...
if ((time >= 0) && (time < minTime))
{
// if the two will be close enough to collide,
// make a note of it
if (computeNearestApproachPositions(other, time)
< collisionDangerThreshold)
{
minTime = time;
threat = other;
xxxThreatPositionAtNearestApproach
= hisPositionAtNearestApproach;
xxxOurPositionAtNearestApproach
= ourPositionAtNearestApproach;
}
}
}
}
// if a potential collision was found, compute steering to avoid
if (threat != null)
{
// parallel: +1, perpendicular: 0, anti-parallel: -1
float parallelness = forward().Dot(threat.forward());
float angle = 0.707f;
if (parallelness < -angle)
{
// anti-parallel "head on" paths:
// steer away from future threat position
Vector3 offset = xxxThreatPositionAtNearestApproach - Position;
float sideDot = offset.Dot(side());
steer = (sideDot > 0) ? -1.0f : 1.0f;
}
else
{
if (parallelness > angle)
{
// parallel paths: steer away from threat
Vector3 offset = threat.Position - Position;
float sideDot = offset.Dot(side());
steer = (sideDot > 0) ? -1.0f : 1.0f;
}
else
{
// perpendicular paths: steer behind threat
// (only the slower of the two does this)
if (threat.speed() <= speed())
{
float sideDot = side().Dot(threat.velocity());
steer = (sideDot > 0) ? -1.0f : 1.0f;
}
}
}
annotateAvoidNeighbor(threat,
steer,
xxxOurPositionAtNearestApproach,
xxxThreatPositionAtNearestApproach);
}
return(side() * steer);
}
// ----------------------------------------------------------------------------
// evasion of another vehicle
public Vector3 steerForEvasion( AbstractVehicle menace, float maxPredictionTime)
{
// offset from this to menace, that distance, unit vector toward menace
Vector3 offset = menace.Position - Position;
float distance = offset.Length;
float roughTime = distance / menace.speed();
float predictionTime = ((roughTime > maxPredictionTime) ?
maxPredictionTime :
roughTime);
Vector3 target = menace.predictFuturePosition (predictionTime);
return steerForFlee (target);
}