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