float3 OpenSteer::SteerLibrary:: steerToStayOnPath (const AbstractVehicle& v, const float predictionTime, Pathway& path) { // predict our future position const float3 futurePosition = v.predictFuturePosition (predictionTime); // find the point on the path nearest the predicted future position float3 tangent; float outside; const float3 onPath = path.mapPointToPath (futurePosition, tangent, // output argument outside); // output argument if (outside < 0) { // our predicted future position was in the path, // return zero steering. return float3_zero(); } else { // our predicted future position was outside the path, need to // steer towards it. Use onPath projection of futurePosition // as seek target annotatePathFollowing (futurePosition, onPath, onPath, outside); return steerForSeek (v, onPath); } }
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); } }