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