private void DrawPath(IList <Vector2Int> path) { if (path == null) { return; } // Raw path drawing foreach (var cell in path) { DrawPathPoint(cell, Color.Blue, 1); } Debug.WriteLine($"Raw path length: {path.Count}"); PathSmoother smoother = new PathSmoother(); smoother.OnObstacleDetectedEvent += OnSmootherObstacleDetected; IList <Vector2Int> smoothedPath = smoother.GetSmoothedPath(_cellMap, path); foreach (var cell in smoothedPath) { DrawPathPoint(cell, Color.DarkCyan, 3); } Debug.WriteLine($"Smoothed path length: {path.Count}"); }
private void OnPathRequestSucceeded(IPathRequestQuery request) { Vector3[] roughPath = request.GetSolutionPath(m_pathAgent.PathManager.PathTerrain); Vector3[] steeringPath = roughPath; if (m_usePathSmoothing) { // Smooth the path Vector3[] aLeftPortalEndPts; Vector3[] aRightPortalEndPts; m_pathAgent.PathManager.PathTerrain.ComputePortalsForPathSmoothing(roughPath, out aLeftPortalEndPts, out aRightPortalEndPts); steeringPath = PathSmoother.Smooth(roughPath, request.GetStartPos(), request.GetGoalPos(), aLeftPortalEndPts, aRightPortalEndPts); } if (sFinded != null) { sFinded(steeringPath); } if (m_pathGroup != null) { // Debug.Log("steeringPath=" + steeringPath.Length); m_pathGroup.SetPath(steeringPath, false); StartCoroutine(DelayDestory()); } else { // Begin steering along this path m_steeringAgent.SteerAlongPath(steeringPath, m_pathAgent.PathManager.PathTerrain); } }
public PathPlanner() { this.Options = PathSmoother.GetDefaultOptions(); this.Options.alpha_s = 100; this.Options.alpha_d = 100; this.Options.alpha_w = 0; }
/// <summary> /// Default constructor /// </summary> /// <param name="owner">The owner entity of the PathManager</param> /// <param name="graph">The graph where the pathfinding searches are going to happen</param> /// <param name="search">Search algorithm used by the planner</param> /// <param name="neighboursSearchRange"> /// The maximum distance the PathPlanner will use to search a neighbour-walkable /// node to a given position /// </param> /// <param name="smoothAlgorithm">Smooth algorithm for the path</param> public PathPlanner(MovingEntity owner, Graph <NavigationNode, T> graph, GraphSearchAlgorithm <NavigationNode, T> search, float neighboursSearchRange, PathSmoother smoothAlgorithm) { this.owner = owner; this.graph = graph; this.search = search; this.neighboursSearchRange = neighboursSearchRange; this.smoothAlgorithm = smoothAlgorithm; }
public PlanningSettings() { Options = PathSmoother.GetDefaultOptions(); Options.alpha_s = 100; Options.alpha_d = 100; Options.alpha_w = 0; Options.num_passes = 2; Options.reverse = false; }
public IList <Vector2Int> GetSmoothedPath(ICellMap map, Vector2Int start, Vector2Int stop, NeighbourMode neighbourMode) { var rawPath = GetPath(map, start, stop, neighbourMode); if (rawPath == null) { return(null); } PathSmoother smoother = new PathSmoother(); var path = smoother.GetSmoothedPath(map, rawPath); return(path); }
private void OnPathRequestSucceeded(IPathRequestQuery request) { Vector3[] roughPath = request.GetSolutionPath(m_pathAgent.PathManager.PathTerrain); Vector3[] steeringPath = roughPath; if (m_usePathSmoothing) { // Smooth the path Vector3[] aLeftPortalEndPts; Vector3[] aRightPortalEndPts; m_pathAgent.PathManager.PathTerrain.ComputePortalsForPathSmoothing(roughPath, out aLeftPortalEndPts, out aRightPortalEndPts); steeringPath = PathSmoother.Smooth(roughPath, request.GetStartPos(), request.GetGoalPos(), aLeftPortalEndPts, aRightPortalEndPts); } // Begin steering along this path m_steeringAgent.SteerAlongPath(steeringPath, m_pathAgent.PathManager.PathTerrain); }
private void ConstructPath() { _path.Clear(); _smoothPath.Clear(); _currentNodeIndex = 0; if (!_target.isSet) { return; } NavGrid navGrid = Map.instance.navGrid; // We first try to see if we can reach the target straightly without // needed a pathfinding // to see that, we look for a clear line between the character and the target if (CanReachTargetDirectly()) { return; } Profiler.BeginSample("A*"); _coordPath = Pathfinder.A_Star(_character.position, _target.position); Profiler.EndSample(); for (int i = 0; i < _coordPath.Count; i++) { _path.Add(navGrid.GetCasePosition(_coordPath[i])); } Profiler.BeginSample("smooth"); _smoothCoordPath = PathSmoother.SmoothPath(_coordPath); Profiler.EndSample(); for (int i = 0; i < _smoothCoordPath.Count; i++) { _smoothPath.Add(navGrid.GetCasePosition(_smoothCoordPath[i])); } }
private void SmoothIt() { Stopwatch s = Stopwatch.StartNew(); SmootherOptions opt = PathSmoother.GetDefaultOptions(); if (optIPOPT.Checked) { opt.alg = SmoothingAlgorithm.Ipopt; } else { opt.alg = SmoothingAlgorithm.Loqo; } opt.alpha_w = getW(); opt.alpha_d = getD1(); opt.set_init_velocity = true; opt.init_velocity = getV(); opt.set_init_heading = true; opt.init_heading = Math.Atan2(pathPoints[1].Y - pathPoints[0].Y, pathPoints[1].X - pathPoints[0].X); opt.set_final_heading = true; opt.final_heading = Math.Atan2(pathPoints[pathPoints.Count - 1].Y - pathPoints[pathPoints.Count - 2].Y, pathPoints[pathPoints.Count - 1].X - pathPoints[pathPoints.Count - 2].X); opt.set_final_offset = true; List <PathPoint> path = new List <PathPoint>(); Boundary ub = new Boundary(); ub.Coords = ubPoints; ub.DesiredSpacing = 1; ub.MinSpacing = 0.25; ub.Polygon = false; List <Boundary> ub_bounds = new List <Boundary>(); ub_bounds.Add(ub); Boundary lb = new Boundary(); lb.Coords = lbPoints; lb.DesiredSpacing = 0.5; lb.MinSpacing = 0; lb.Polygon = false; List <Boundary> lb_bounds = new List <Boundary>(); lb_bounds.Add(lb); LineList basePath = new LineList(); basePath.AddRange(pathPoints); SmoothResult sr = SmoothResult.Error; try { sr = PathSmoother.SmoothPath(basePath, ub_bounds, lb_bounds, opt, path); } catch (Exception ex) { MessageBox.Show("exception: " + ex.Message); return; } smoothedPoints = path.ConvertAll <Coordinates>(delegate(PathPoint p) { return(new Coordinates(p.x, p.y)); }); s.Stop(); long ms = s.ElapsedMilliseconds; if (sr != SmoothResult.Sucess) { MessageBox.Show("Path smooth result: " + sr.ToString()); } MessageBox.Show("Elapsed MS: " + ms); picEntry.Invalidate(); }
private bool TrimPath(Path path) { //Make sure that the path was resolved before moving too far from where it was requested. var nn = _pathSettings.nextNodeDistance; if (_unit.position.DirToXZ(path.Peek().position).sqrMagnitude < nn * nn) { return(true); } //First thing to do is to remove any waypoints that have already been reached. //Since the actual waypoints may differ from the requested ones (if they were corrected to allow the unit to access them) //we cannot remove them by comparison. //Since the last point is now always a waypoint (but not a via point), we skip that as obviously the last point cannot be pruned. int currentWaypointCount = _wayPoints.viaPointsCount; int pathCount = path.count; int encounteredWaypoints = 0; for (int i = pathCount - 2; i >= 0; i--) { var node = path[i]; if (node is Waypoint) { if (encounteredWaypoints == currentWaypointCount) { path.Truncate(i + 1); break; } encounteredWaypoints++; } } //Next find the last node on the path to which we can move directly, i.e. skipping nodes already reached. var gridBounds = _currentGrid.bounds; var matrix = _currentGrid.cellMatrix; var costStrategy = GameServices.cellCostStrategy; int firstNodeIdx = -1; pathCount = path.count; for (int i = 0; i < pathCount; i++) { //If we haven't yet found an accessible node move on if the inspected node is outside the current grid or is a portal. //Otherwise stop, we do not move past portals. var node = path[i]; if (node is IPortalNode || !gridBounds.Contains(node.position)) { if (firstNodeIdx < 0) { continue; } break; } if (PathSmoother.CanReducePath(node, _unit, _unit, matrix, costStrategy)) { firstNodeIdx = i; } else if (firstNodeIdx >= 0) { break; } //We must visit waypoint so they cannot be smoothed away if (node is Waypoint) { break; } } if (firstNodeIdx < 0) { return(false); } path.Truncate(firstNodeIdx); return(true); }
public SmoothingResult PlanPath(PlanningSettings settings) { SmootherOptions opts = settings.Options; // for now, just run the smoothing opts.init_heading = settings.initHeading; opts.set_init_heading = true; opts.min_init_velocity = settings.startSpeed*0.5; opts.set_min_init_velocity = true; opts.max_init_velocity = Math.Max(settings.maxSpeed, settings.startSpeed); opts.set_max_init_velocity = true; opts.min_velocity = 0.1; opts.max_velocity = Math.Max(opts.min_velocity+0.1, settings.maxSpeed); opts.k_max = Math.Min(TahoeParams.CalculateCurvature(-TahoeParams.SW_max, settings.startSpeed), TahoeParams.CalculateCurvature(TahoeParams.SW_max, settings.startSpeed)) * 0.97; opts.generate_details = true;// GenerateDetails; if (settings.endingHeading != null) { opts.set_final_heading = true; opts.final_heading = settings.endingHeading.Value; } else { opts.set_final_heading = false; } opts.set_final_offset = settings.endingPositionFixed; opts.final_offset_min = settings.endingPositionMin; opts.final_offset_max = settings.endingPositionMax; if (settings.maxEndingSpeed != null) { opts.set_final_velocity_max = true; opts.final_velocity_max = Math.Max(opts.min_velocity+0.1, settings.maxEndingSpeed.Value); } else { opts.set_final_velocity_max = false; } opts.a_lat_max = 6; // create the boundary list List<UrbanChallenge.PathSmoothing.PathPoint> ret = new List<UrbanChallenge.PathSmoothing.PathPoint>(); smoother = new PathSmoother(); OperationalTrace.WriteVerbose("calling smooth path"); SmoothResult result = smoother.SmoothPath(settings.basePath, settings.targetPaths, settings.leftBounds, settings.rightBounds, opts, ret); if (result != SmoothResult.Sucess) { OperationalTrace.WriteWarning("smooth path result: {0}", result); } else { OperationalTrace.WriteVerbose("smooth path result: {0}", result); } AvoidanceDetails details = null; if (opts.generate_details) { details = new AvoidanceDetails(); details.leftBounds = settings.leftBounds; details.rightBounds = settings.rightBounds; details.smoothingDetails = smoother.GetSmoothingDetails(); LastAvoidanceDetails = details; // push out the points Coordinates[] leftPoints = new Coordinates[details.smoothingDetails.leftBounds.Length]; for (int i = 0; i < leftPoints.Length; i++) { leftPoints[i] = details.smoothingDetails.leftBounds[i].point; } Coordinates[] rightPoints = new Coordinates[details.smoothingDetails.rightBounds.Length]; for (int i = 0; i < rightPoints.Length; i++) { rightPoints[i] = details.smoothingDetails.rightBounds[i].point; } Services.UIService.PushPoints(leftPoints, settings.timestamp, "left bound points", true); Services.UIService.PushPoints(rightPoints, settings.timestamp, "right bound points", true); } //if (result == SmoothResult.Sucess) { Coordinates[] points = new Coordinates[ret.Count]; double[] speeds = new double[ret.Count]; for (int i = 0; i < ret.Count; i++) { points[i] = new Coordinates(ret[i].x, ret[i].y); speeds[i] = ret[i].v; } SmoothedPath path = new SmoothedPath(settings.timestamp, points, speeds); return new SmoothingResult(result, path, details); /*} else { SmoothedPath path = new SmoothedPath(settings.timestamp, settings.basePath, null); return new SmoothingResult(result, path); }*/ }
public SmoothingResult PlanPath(PlanningSettings settings) { SmootherOptions opts = settings.Options; // for now, just run the smoothing opts.init_heading = settings.initHeading; opts.set_init_heading = true; opts.min_init_velocity = settings.startSpeed * 0.5; opts.set_min_init_velocity = true; opts.max_init_velocity = Math.Max(settings.maxSpeed, settings.startSpeed); opts.set_max_init_velocity = true; opts.min_velocity = 0.1; opts.max_velocity = Math.Max(opts.min_velocity + 0.1, settings.maxSpeed); opts.k_max = Math.Min(TahoeParams.CalculateCurvature(-TahoeParams.SW_max, settings.startSpeed), TahoeParams.CalculateCurvature(TahoeParams.SW_max, settings.startSpeed)) * 0.97; opts.generate_details = true; // GenerateDetails; if (settings.endingHeading != null) { opts.set_final_heading = true; opts.final_heading = settings.endingHeading.Value; } else { opts.set_final_heading = false; } opts.set_final_offset = settings.endingPositionFixed; opts.final_offset_min = settings.endingPositionMin; opts.final_offset_max = settings.endingPositionMax; if (settings.maxEndingSpeed != null) { opts.set_final_velocity_max = true; opts.final_velocity_max = Math.Max(opts.min_velocity + 0.1, settings.maxEndingSpeed.Value); } else { opts.set_final_velocity_max = false; } opts.a_lat_max = 6; // create the boundary list List <UrbanChallenge.PathSmoothing.PathPoint> ret = new List <UrbanChallenge.PathSmoothing.PathPoint>(); smoother = new PathSmoother(); OperationalTrace.WriteVerbose("calling smooth path"); SmoothResult result = smoother.SmoothPath(settings.basePath, settings.targetPaths, settings.leftBounds, settings.rightBounds, opts, ret); if (result != SmoothResult.Sucess) { OperationalTrace.WriteWarning("smooth path result: {0}", result); } else { OperationalTrace.WriteVerbose("smooth path result: {0}", result); } AvoidanceDetails details = null; if (opts.generate_details) { details = new AvoidanceDetails(); details.leftBounds = settings.leftBounds; details.rightBounds = settings.rightBounds; details.smoothingDetails = smoother.GetSmoothingDetails(); LastAvoidanceDetails = details; // push out the points Coordinates[] leftPoints = new Coordinates[details.smoothingDetails.leftBounds.Length]; for (int i = 0; i < leftPoints.Length; i++) { leftPoints[i] = details.smoothingDetails.leftBounds[i].point; } Coordinates[] rightPoints = new Coordinates[details.smoothingDetails.rightBounds.Length]; for (int i = 0; i < rightPoints.Length; i++) { rightPoints[i] = details.smoothingDetails.rightBounds[i].point; } Services.UIService.PushPoints(leftPoints, settings.timestamp, "left bound points", true); Services.UIService.PushPoints(rightPoints, settings.timestamp, "right bound points", true); } //if (result == SmoothResult.Sucess) { Coordinates[] points = new Coordinates[ret.Count]; double[] speeds = new double[ret.Count]; for (int i = 0; i < ret.Count; i++) { points[i] = new Coordinates(ret[i].x, ret[i].y); speeds[i] = ret[i].v; } SmoothedPath path = new SmoothedPath(settings.timestamp, points, speeds); return(new SmoothingResult(result, path, details)); /*} * else { * SmoothedPath path = new SmoothedPath(settings.timestamp, settings.basePath, null); * * return new SmoothingResult(result, path); * }*/ }
public void ComputePath(IReadOnlyList <Cell> cells, bool smoothPath) { // Remove previous path visualization foreach (var cell in cells.Where(c => Cell.ReplayCellStates.Contains(c.CellState))) { cell.CellState = CellState.Normal; } var lookup = new Dictionary <Position, Cell>(); var grid = new Grid(10, 10); var start = new Position(0, 0); var end = new Position(9, 9); // Take the properties of the cells and translate them // to properties on the right positions in the grid foreach (var cell in cells) { var position = new Position(cell.X, cell.Y); lookup.Add(position, cell); switch (cell.CellState) { case CellState.Normal: grid.SetCellCost(position, cell.Cost); break; case CellState.Start: start = position; break; case CellState.End: end = position; break; case CellState.Blocked: grid.BlockCell(position); break; case CellState.Current: case CellState.Open: case CellState.Closed: case CellState.OnPath: case CellState.Replaced: break; default: throw new ArgumentOutOfRangeException(); } } var path = grid.GetPath(start, end); if (smoothPath) { PathSmoother.SmoothPath(grid, path, MovementPatterns.Full, 10); } // Visualize the path in the cells, skip the start and end node, we already // have those in the visualization foreach (var p in path.Skip(1).Take(path.Length - 2)) { var cell = lookup[p]; cell.CellState = CellState.OnPath; } }