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