float
OpenSteer::SteerLibrary::
predictNearestApproachTime (const AbstractVehicle& v,
const AbstractVehicle& other)
{
// imagine we are at the origin with no velocity,
// compute the relative velocity of the other vehicle
const float3 myVelocity = v.velocity();
const float3 otherVelocity = other.velocity();
const float3 relVelocity = float3_subtract(otherVelocity, myVelocity);
const float relSpeed = float3_length(relVelocity);
// 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
const float3 relTangent = float3_scalar_divide(relVelocity, relSpeed);
// find distance from its path to origin (compute offset from
// other to us, find length of projection onto path)
const float3 relPosition = float3_subtract(make_float3(v.position()), make_float3(other.position()));
const float projection = float3_dot(relTangent, relPosition);
return projection / relSpeed;
}
float3
OpenSteer::SteerLibrary::
steerForFlee (const AbstractVehicle& v, const float3& target)
{
const float3 desiredVelocity = float3_subtract(make_float3(v.position()), target);
return float3_subtract(desiredVelocity, v.velocity());
}
float3
OpenSteer::SteerLibrary::
xxxsteerForSeek (const AbstractVehicle& v, const float3& target)
{
const float3 offset = float3_subtract(target, make_float3(v.position()));
const float3 desiredVelocity = float3_truncateLength(offset, v.maxSpeed());
return float3_subtract(desiredVelocity, v.velocity());
}
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);
}