private List <BreadCrumbsTracker> GetUnExploredNeighbors(
BreadCrumbsTracker waypoint, HashSet <BreadCrumbsTracker> alreadyExplored, Dictionary <Vector2Int, BreadCrumbsTracker> blocks
)
{
return(GetNeighbors(waypoint, blocks)
.Where(_ => !alreadyExplored.Contains(_) && !_.Point.IsBlocked)
.ToList());
}
private List <BreadCrumbsTracker> GetNeighbors(BreadCrumbsTracker waypoint, Dictionary <Vector2Int, BreadCrumbsTracker> blocks)
{
var startPosition = waypoint.Point.GetPositionInGrid();
return(_directions
// Filter in case the neighbor doesn't exist
// This will happen if the waypoint is on the edge
.Where(_ => blocks.ContainsKey(_ + startPosition))
.Select(_ => blocks[_ + startPosition])
.ToList());
}
private List <Waypoint> ConstructPathFromLeadingPoints(BreadCrumbsTracker start, BreadCrumbsTracker end)
{
var tracker = end;
var path = new List <Waypoint> {
tracker.Point
};
while (tracker.LeadingPoint != null)
{
path.Insert(0, tracker.Point);
tracker = tracker.LeadingPoint;
}
path.Insert(0, start.Point);
return(path);
}
public List <Waypoint> GetPath(Vector2Int?startingPosition = null)
{
var queue = new Queue <BreadCrumbsTracker>();
var explored = new HashSet <BreadCrumbsTracker>();
var blocks = GetWrappedBlocks();
var startWrapper = new BreadCrumbsTracker
{
Point = startingPosition != null
? GetWaypointByPosition((Vector2Int)startingPosition, blocks)
: _start,
LeadingPoint = null
};
var endWrapper = new BreadCrumbsTracker {
Point = _end, LeadingPoint = null
};
BreadCrumbsTracker current = null;
queue.Enqueue(startWrapper);
while (queue.Any())
{
current = queue.Dequeue();
explored.Add(current);
// If reached destination
if (current.Point == endWrapper.Point)
{
break;
}
GetUnExploredNeighbors(current, explored, blocks)
.ForEach(_ =>
{
if (!queue.Contains(_))
{
queue.Enqueue(_);
}
_.LeadingPoint = current;
});
}
var path = ConstructPathFromLeadingPoints(startWrapper, current);
return(path);
}
