protected override Vector3 CalculateForce()
{
if (_pathway == null)
{
return(Vector3.zero);
}
Vector3 pos = _pathway.MapPathDistanceToPoint(rate * _pathway.TotalPathLength);
rate += Time.deltaTime * speed;
if (rate > 1)
{
rate = 0;
}
this.TargetPoint = pos;
this.ConsiderVelocity = false;
return(base.CalculateForce());
}
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));
}
