float3
OpenSteer::SteerLibrary::
steerForTargetSpeed (const AbstractVehicle& v,
const float targetSpeed)
{
const float mf = v.maxForce ();
const float speedError = targetSpeed - v.speed ();
return float3_scalar_multiply(make_float3(v.forward ()), clip (speedError, -mf, +mf));
}
float
OpenSteer::SteerLibrary::
computeNearestApproachPositions (const AbstractVehicle& v,
AbstractVehicle& other,
float time)
{
const float3 myTravel = float3_scalar_multiply(make_float3(v.forward()), v.speed () * time);
const float3 otherTravel = float3_scalar_multiply(make_float3(other.forward()), other.speed () * time);
const float3 myFinal = float3_add(make_float3(v.position()), myTravel);
const float3 otherFinal = float3_add(make_float3(other.position()), otherTravel);
// xxx for annotation
ourPositionAtNearestApproach = myFinal;
hisPositionAtNearestApproach = otherFinal;
return float3_distance(myFinal, otherFinal);
}
float3
OpenSteer::SteerLibrary::
steerToFollowPath (const AbstractVehicle& v,
const int direction,
const float predictionTime,
Pathway& path)
{
// our goal will be offset from our path distance by this amount
const float pathDistanceOffset = direction * predictionTime * v.speed();
// predict our future position
const float3 futurePosition = v.predictFuturePosition (predictionTime);
// measure distance along path of our current and predicted positions
const float nowPathDistance =
path.mapPointToPathDistance (make_float3(v.position ()));
const float futurePathDistance =
path.mapPointToPathDistance (futurePosition);
// are we facing in the correction direction?
const bool rightway = ((pathDistanceOffset > 0) ?
(nowPathDistance < futurePathDistance) :
(nowPathDistance > futurePathDistance));
// find the point on the path nearest the predicted future position
// XXX need to improve calling sequence, maybe change to return a
// XXX special path-defined object which includes two float3s and a
// XXX bool (onPath,tangent (ignored), withinPath)
float3 tangent;
float outside;
const float3 onPath = path.mapPointToPath (futurePosition,
// output arguments:
tangent,
outside);
// no steering is required if (a) our future position is inside
// the path tube and (b) we are facing in the correct direction
if ((outside < 0) && rightway)
{
// all is well, return zero steering
return float3_zero();
}
else
{
// otherwise we need to steer towards a target point obtained
// by adding pathDistanceOffset to our current path position
float targetPathDistance = nowPathDistance + pathDistanceOffset;
float3 target = path.mapPathDistanceToPoint (targetPathDistance);
annotatePathFollowing (futurePosition, onPath, target, outside);
// return steering to seek target on path
return steerForSeek (v, target);
}
}
float3
OpenSteer::SteerLibrary::
steerToAvoidObstacle (const AbstractVehicle& v,
const float minTimeToCollision,
const Obstacle& obstacle)
{
const float3 avoidance = obstacle.steerToAvoid (v, minTimeToCollision);
// XXX more annotation modularity problems (assumes spherical obstacle)
if (!float3_equals(avoidance, float3_zero()))
annotateAvoidObstacle (minTimeToCollision * v.speed());
return avoidance;
}
float3
OpenSteer::SteerLibrary::
steerForEvasion (const AbstractVehicle& v,
const AbstractVehicle& menace,
const float maxPredictionTime)
{
// offset from this to menace, that distance, unit vector toward menace
const float3 offset = float3_subtract(make_float3(menace.position()), make_float3(v.position()));
const float distance = float3_length(offset);
const float roughTime = distance / menace.speed();
const float predictionTime = ((roughTime > maxPredictionTime) ?
maxPredictionTime :
roughTime);
const float3 target = menace.predictFuturePosition (predictionTime);
return steerForFlee (v, target);
}
OpenSteer::Vec3
OpenSteer::Obstacle::PathIntersection::
steerToAvoidIfNeeded (const AbstractVehicle& vehicle,
const float minTimeToCollision) const
{
// if nearby intersection found, steer away from it, otherwise no steering
const float minDistanceToCollision = minTimeToCollision * vehicle.speed();
if (intersect && (distance < minDistanceToCollision))
{
// compute avoidance steering force: take the component of
// steerHint which is lateral (perpendicular to vehicle's
// forward direction), set its length to vehicle's maxForce
Vec3 lateral = steerHint.perpendicularComponent (vehicle.forward ());
if (lateral == Vec3::zero)
lateral = vehicle.side ();
return lateral.normalize () * vehicle.maxForce ();
}
else
{
return Vec3::zero;
}
}
float3
OpenSteer::SteerLibrary::
steerForPursuit (const AbstractVehicle& v,
const AbstractVehicle& quarry,
const float maxPredictionTime)
{
// offset from this to quarry, that distance, unit vector toward quarry
const float3 offset = float3_subtract(make_float3(quarry.position()), make_float3(v.position()));
const float distance = float3_length(offset);
const float3 unitOffset = float3_scalar_divide(offset, distance);
// how parallel are the paths of "this" and the quarry
// (1 means parallel, 0 is pependicular, -1 is anti-parallel)
const float parallelness = float3_dot(make_float3(v.forward()), make_float3(quarry.forward()));
// how "forward" is the direction to the quarry
// (1 means dead ahead, 0 is directly to the side, -1 is straight back)
const float forwardness = float3_dot(make_float3(v.forward()), unitOffset);
const float directTravelTime = distance / v.speed ();
const int f = intervalComparison (forwardness, -0.707f, 0.707f);
const int p = intervalComparison (parallelness, -0.707f, 0.707f);
float timeFactor = 0; // to be filled in below
float3 color; // 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 = gBlack;
break;
case 0: // ahead, perpendicular
timeFactor = 1.8f;
color = gGray50;
break;
case -1: // ahead, anti-parallel
timeFactor = 0.85f;
color = gWhite;
break;
}
break;
case 0:
switch (p)
{
case +1: // aside, parallel
timeFactor = 1;
color = gRed;
break;
case 0: // aside, perpendicular
timeFactor = 0.8f;
color = gYellow;
break;
case -1: // aside, anti-parallel
timeFactor = 4;
color = gGreen;
break;
}
break;
case -1:
switch (p)
{
case +1: // behind, parallel
timeFactor = 0.5f;
color= gCyan;
break;
case 0: // behind, perpendicular
timeFactor = 2;
color= gBlue;
break;
case -1: // behind, anti-parallel
timeFactor = 2;
color = gMagenta;
break;
}
break;
}
// estimated time until intercept of quarry
const float et = directTravelTime * timeFactor;
// xxx experiment, if kept, this limit should be an argument
const float etl = (et > maxPredictionTime) ? maxPredictionTime : et;
// estimated position of quarry at intercept
const float3 target = quarry.predictFuturePosition (etl);
// annotation
annotationLine (make_float3(v.position()),
target,
gaudyPursuitAnnotation ? color : gGray40);
return steerForSeek (v, target);
}
float3
OpenSteer::SteerLibrary::
steerToAvoidNeighbors (const AbstractVehicle& v,
const float minTimeToCollision,
const AVGroup& others)
{
// first priority is to prevent immediate interpenetration
const float3 separation = steerToAvoidCloseNeighbors (v, 0, others);
if (!float3_equals(separation, float3_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
float3 xxxThreatPositionAtNearestApproach;
float3 xxxOurPositionAtNearestApproach;
// 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++)
{
AbstractVehicle& other = **i;
if (&other != &v)
{
// avoid when future positions are this close (or less)
const float collisionDangerThreshold = v.radius() * 2;
// predicted time until nearest approach of "this" and "other"
const float time = predictNearestApproachTime (v, 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 (v, 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 = float3_dot(make_float3(v.forward()), make_float3(threat->forward()));
float angle = 0.707f;
if (parallelness < -angle)
{
// anti-parallel "head on" paths:
// steer away from future threat position
float3 offset = float3_subtract(xxxThreatPositionAtNearestApproach, make_float3(v.position()));
float sideDot = float3_dot(offset, v.side());
steer = (sideDot > 0) ? -1.0f : 1.0f;
}
else
{
if (parallelness > angle)
{
// parallel paths: steer away from threat
float3 offset = float3_subtract(make_float3(threat->position()), make_float3(v.position()));
float sideDot = float3_dot(offset, v.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() <= v.speed())
{
float sideDot = float3_dot(v.side(), threat->velocity());
steer = (sideDot > 0) ? -1.0f : 1.0f;
}
}
}
annotateAvoidNeighbor (*threat,
steer,
xxxOurPositionAtNearestApproach,
xxxThreatPositionAtNearestApproach);
}
return float3_scalar_multiply(v.side(), steer);
}