I think its done
This commit is contained in:
@@ -7,12 +7,17 @@ namespace ConsoleApp.PathPlanners
|
||||
public class ReactivePathPlanner : IReactivePathPlanner
|
||||
{
|
||||
private Queue<Coordinate2D> _reactiveHexPath;
|
||||
private Queue<ICell> _reactiveSquarePath;
|
||||
public Queue<Coordinate2D> ReactiveHexPath => _reactiveHexPath;
|
||||
public Queue<ICell> ReactiveSquarePath => _reactiveSquarePath;
|
||||
|
||||
|
||||
|
||||
public ReactivePathPlanner()
|
||||
{
|
||||
_reactiveHexPath = new Queue<Coordinate2D>();
|
||||
_reactiveSquarePath = new Queue<ICell>();
|
||||
|
||||
}
|
||||
public void GenerateReactiveHexPath(IHexMap hexMap, Queue<Coordinate2D> optimalPath,
|
||||
Coordinate2D vehicleCurrentHexCell)
|
||||
@@ -39,6 +44,33 @@ namespace ConsoleApp.PathPlanners
|
||||
_reactiveHexPath.Enqueue(node);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
public void GenerateReactiveSquarePath(ISquareMap squareMap, Queue<ICell> optimalPath,
|
||||
ICell vehicleCurrentSquareCell)
|
||||
{
|
||||
//Clean out old path
|
||||
_reactiveSquarePath.Clear();
|
||||
|
||||
//If the optimal path is empty well we are done, otherwise pop 1
|
||||
if (!optimalPath.TryDequeue(out ICell convergentPoint))
|
||||
return;
|
||||
|
||||
//Dequeue until the cell is not blocked
|
||||
while (squareMap.GetCell(convergentPoint.X, convergentPoint.Y).Blocked)
|
||||
{
|
||||
//if optimal path is empty were done and we can not clear around this position, otherwise
|
||||
//lest pop another.
|
||||
if (!optimalPath.TryDequeue(out convergentPoint))
|
||||
return;
|
||||
}
|
||||
|
||||
var map = (SquareMap)squareMap;
|
||||
var reactivePath = map.GetShortestPath(vehicleCurrentSquareCell, convergentPoint);
|
||||
foreach (var node in reactivePath)
|
||||
{
|
||||
_reactiveSquarePath.Enqueue(node);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user