public WayPointsFollowingRRTPlanner( double goalBias, int maximumNumberOfIterations, IVehicleModel vehicleModel, IMotionModel motionModel, ITrack track, ICollisionDetector collisionDetector, Random random, TimeSpan timeStep, IActionSet actions, IReadOnlyList <IGoal> wayPoints) { if (goalBias > 0.5) { throw new ArgumentOutOfRangeException($"Goal bias must be at most 0.5 (given {goalBias})."); } this.goalBias = goalBias; this.maximumNumberOfIterations = maximumNumberOfIterations; this.vehicleModel = vehicleModel; this.motionModel = motionModel; this.track = track; this.collisionDetector = collisionDetector; this.random = random; this.timeStep = timeStep; this.wayPoints = wayPoints; this.actions = actions; distances = new DistanceMeasurement(track.Width, track.Height); wayPointsReached = 0; ExploredStates = exploredStates; }
public RRTPlanner( double goalBias, int maximumNumberOfIterations, IWorldDefinition world, Random random, TimeSpan timeStep, IGoal goal) { if (goalBias > 0.5) { throw new ArgumentOutOfRangeException($"Goal bias must be at most 0.5 (given {goalBias})."); } this.goalBias = goalBias; this.maximumNumberOfIterations = maximumNumberOfIterations; this.vehicleModel = world.VehicleModel; this.motionModel = world.MotionModel; this.track = world.Track; this.collisionDetector = world.CollisionDetector; this.random = random; this.timeStep = timeStep; this.goal = goal; this.actions = world.Actions; distances = new DistanceMeasurement(track.Width, track.Height); ExploredStates = exploredStates; }
public MeasurementType(DistanceMeasurement measurement, char code, string singular, string plural, string abbreviation, string symbol = null) { Measurement = measurement; Code = code; SingularName = singular; PluralName = plural; Abbreviation = abbreviation; Symbol = symbol ?? abbreviation; }
// Start is called before the first frame update void Start() { gotoPatrol = 2; playerDistance = gameObject.AddComponent <DistanceMeasurement>(); playerDistance.other = player; patrol1Distance = gameObject.AddComponent <DistanceMeasurement>(); patrol1Distance.other = patrol1; patrol2Distance = gameObject.AddComponent <DistanceMeasurement>(); patrol2Distance.other = patrol2; }
public static void Render(RenderManager.CameraInfo cameraInfo, DistanceMeasurement distance) { var renderManager = RenderManager.instance; if (!distance.IsStraight || distance.HideOverlay) { return; } var minHeight = Mathf.Min(distance.StartPosition.y, distance.EndPosition.y); var maxHeight = Mathf.Max(distance.StartPosition.y, distance.EndPosition.y); renderManager.OverlayEffect.DrawSegment(cameraInfo, distance.Flags == MeasurementFlags.Primary ? Settings.PrimaryColor : Settings.SecondaryColor, new Segment3(distance.StartPosition, distance.EndPosition), Size, 3f, minHeight - 20f, maxHeight + 20f, true, true); }
public static Vector3 GetLabelWorldPosition(DistanceMeasurement distance) { return(distance.Position); }
private (VehicleState?, IAction?, bool) steer(TreeNode from, VehicleState to, DistanceMeasurement distances) { VehicleState?state = null; IAction? bestAction = null; bool reachedWayPoint = false; var shortestDistance = double.MaxValue; // todo what if there are no available actions? var availableActions = from.SelectAvailableActionsFrom(actions.AllPossibleActions).ToArray(); var remainingAvailableActions = availableActions.Length; foreach (var action in availableActions) { var predictedStates = motionModel.CalculateNextState(from.State, action, timeStep).ToList(); var elapsedTime = predictedStates.Last().relativeTime; var resultState = predictedStates.Last().state; var passedGoal = false; var collided = false; foreach (var(simulationTime, intermediateState) in predictedStates) { if (collisionDetector.IsCollision(intermediateState)) { elapsedTime = simulationTime; resultState = intermediateState; passedGoal = false; collided = true; break; } if (wayPoints[wayPointsReached].ReachedGoal(intermediateState.Position)) { passedGoal = true; } } if (collided) { from.DisableAction(action); remainingAvailableActions--; continue; } var currentDistance = distances.DistanceBetween(to, resultState); if ((!reachedWayPoint || passedGoal) && currentDistance < shortestDistance) { state = resultState; bestAction = action; reachedWayPoint = passedGoal; } } if (bestAction != null) { from.DisableAction(bestAction); remainingAvailableActions--; } if (bestAction == null || remainingAvailableActions == 0) { from.DisableFutureExpansions(); } return(state, bestAction, reachedWayPoint); }
/// <summary> /// Finds the n known measurements with the smallest euclidian distance to x, y, t in nearly O(n) time. /// </summary> private List<Measurement> calcNearest(Measurement measure) { DistanceMeasurement[] results = new DistanceMeasurement[this.nearest]; foreach (Measurement curr in this.known) { double distance = this.calcD(curr, measure); // Test if this is a new smallest value. bool shift = false; DistanceMeasurement shifted = null; for (int i = 0; i < results.Length; i++) { // A smaller value was found continue shifting the array. if (shift) { DistanceMeasurement temp = results[i]; results[i] = shifted; shifted = temp; } // Test if this is one of the first n elements. else if (results[i] == null) { // Build a DistanceMeasurement for this value. DistanceMeasurement dm = new DistanceMeasurement(); dm.Measurement = curr; dm.Distance = distance; results[i] = dm; } // Test if value smaller than current element. else if (distance < results[i].Distance) { // Build a DistanceMeasurement for this value. DistanceMeasurement dm = new DistanceMeasurement(); dm.Measurement = curr; dm.Distance = distance; // Swap in this value. shifted = results[i]; results[i] = dm; // Enter shift mode. shift = true; } } } // Convert from DistanceMeasurement to Measurement. List<Measurement> nearest = new List<Measurement>(); foreach (DistanceMeasurement curr in results) { nearest.Add(curr.Measurement); } return nearest; }