/// <summary>
/// how far outside path tube is the given point? (negative is inside)
/// </summary>
/// <param name="pathway"></param>
/// <param name="point"></param>
/// <returns></returns>
public static float HowFarOutsidePath(this IPathway pathway, Vector3 point)
{
float outside;
Vector3 tangent;
pathway.MapPointToPath(point, out tangent, out outside);
return(outside);
}
/// <summary>
/// is the given point inside the path tube?
/// </summary>
/// <param name="pathway"></param>
/// <param name="point"></param>
/// <returns></returns>
public static bool IsInsidePath(this IPathway pathway, Vector3 point)
{
float outside;
Vector3 tangent;
pathway.MapPointToPath(point, out tangent, out outside);
return(outside < 0);
}
public static Vector3 SteerToStayOnPath(this IVehicle vehicle, float predictionTime, IPathway path, float maxSpeed, IAnnotationService annotation = null)
{
// predict our future position
Vector3 futurePosition = vehicle.PredictFuturePosition(predictionTime);
// find the point on the path nearest the predicted future position
Vector3 tangent;
float outside;
Vector3 onPath = path.MapPointToPath(futurePosition, out tangent, out outside);
if (outside < 0)
{
return(Vector3.Zero); // our predicted future position was in the path, return zero steering.
}
// our predicted future position was outside the path, need to
// steer towards it. Use onPath projection of futurePosition
// as seek target
if (annotation != null)
{
annotation.PathFollowing(futurePosition, onPath, onPath, outside);
}
return(vehicle.SteerForSeek(onPath, maxSpeed));
}
// ----------------------------------------------------------------------------
// Path Following behaviors
public Vector3 steerToStayOnPath(float predictionTime, IPathway path)
{
// predict our future position
Vector3 futurePosition = predictFuturePosition (predictionTime);
// find the point on the path nearest the predicted future position
PathRelativePosition tStruct = new PathRelativePosition();
Vector3 onPath = path.MapPointToPath(futurePosition, ref tStruct);
if (tStruct.outside < 0)
{
// our predicted future position was in the path,
// return zero steering.
return Vector3.zero;
}
else
{
// our predicted future position was outside the path, need to
// steer towards it. Use onPath projection of futurePosition
// as seek target
#if ANNOTATE_PATH
annotatePathFollowing (futurePosition, onPath, onPath,tStruct.outside);
#endif
return steerForSeek (onPath);
}
}
public static Vector3 SteerToFollowPath(this IVehicle vehicle, bool direction, float predictionTime, IPathway path, float maxSpeed, out float currentPathDistance, IAnnotationService annotation = null)
{
// our goal will be offset from our path distance by this amount
float pathDistanceOffset = (direction ? 1 : -1) * predictionTime * vehicle.Speed;
// predict our future position
Vector3 futurePosition = vehicle.PredictFuturePosition(predictionTime);
// measure distance along path of our current and predicted positions
currentPathDistance = path.MapPointToPathDistance(vehicle.Position);
float futurePathDistance = path.MapPointToPathDistance(futurePosition);
// are we facing in the correction direction?
bool rightway = ((pathDistanceOffset > 0) ?
(currentPathDistance < futurePathDistance) :
(currentPathDistance > futurePathDistance));
// find the point on the path nearest the predicted future position
Vector3 tangent;
float outside;
Vector3 onPath = path.MapPointToPath(futurePosition, out tangent, out 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)
{
//We're going at max speed, in the right direction. don't need to do anything
if (vehicle.Speed >= maxSpeed)
{
return(Vector3.Zero);
}
//Predict vehicle position and sample multiple times, incresingly far along the path
var seek = path.MapPointToPath(vehicle.PredictFuturePosition(predictionTime / 3), out tangent, out outside);
for (int i = 0; i < 3; i++)
{
var s = path.MapPointToPath(seek + tangent * vehicle.Speed / (i + 1), out tangent, out outside);
//terminate search if we wander outside the path
if (outside > 0)
{
break;
}
seek = s;
if (annotation != null)
{
annotation.Circle3D(0.3f, seek, Vector3.Up, Color.Green, 6);
}
}
//Steer towards future path point
return(vehicle.SteerForSeek(seek, maxSpeed, annotation));
}
// otherwise we need to steer towards a target point obtained
// by adding pathDistanceOffset to our current path position
float targetPathDistance = currentPathDistance + pathDistanceOffset;
Vector3 target = path.MapPathDistanceToPoint(targetPathDistance);
if (annotation != null)
{
annotation.PathFollowing(futurePosition, onPath, target, outside);
}
// return steering to seek target on path
return(SteerForSeek(vehicle, target, maxSpeed));
}
