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