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