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