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