//////////////////////////////////////////////////////////////////////////
public void Init(FindPathOptions options, iExplorer <T> exlorer, T start, T[] goal)
{
if (Result == FindPathResult.None)
{
// save data
Options = options;
Explorer = exlorer;
Start = start;
Goal = goal;
// set running state
Result = FindPathResult.Running;
// allocate collections
Path = new LinkedList <T>();
OpenSet = new FastPriorityQueue <PathNode <T> >(Pathfinder.c_AdaptiveBufferExperiance.Average);
ClosedSet = new HashSet <T>();
ClosedNodes = new LinkedList <PathNode <T> >();
// create start node, add to open set
var startNode = new PathNode <T>(start)
{
СameFrom = null,
PathCost = 0.0f,
PathCostEstimated = 0.0f,
Cost = 0.0f
};
OpenSet.Enqueue(startNode, startNode.Cost);
}
}
public Cell[] GreedyFind(Vector2Int start, Vector2Int goal)
{
Reset();
Cell startCell = _grid[start]; //first point
Cell goalCell = _grid[goal]; //last point
//Console.SetCursorPosition(1, 4);
//Console.WriteLine($"{goalCell.Location.X}-----{goalCell.Location.Y} ---- Greedy");
_open.Enqueue(startCell, 0); // not checked
var bounds = _grid.Size;
Cell node = null;
while (_open.Count > 0)
{
node = _open.Dequeue();
node.Closed = true;
var g = node.G + 1;
if (goalCell.Location == node.Location)
{
break;
}
Vector2Int proposed = new Vector2Int(0, 0);
for (var i = 0; i < PathingConstants.Directions.Length; i++)
{
var direction = PathingConstants.Directions[i];
proposed.X = node.Location.X + direction.X;
proposed.Y = node.Location.Y + direction.Y;
if (proposed.X < 0 || proposed.X >= bounds.X ||
proposed.Y < 0 || proposed.Y >= bounds.Y)
{
continue;
}
Cell neighbour = _grid[proposed];
if (neighbour.Blocked)
{
continue;
}
if (_grid[neighbour.Location].Closed)
{
continue;
}
if (!_open.Contains(neighbour))
{
neighbour.G = g;
neighbour.H = Heuristic(neighbour, node);
neighbour.Parent = node;
// F will be set by the queue
_open.Enqueue(neighbour, neighbour.G + neighbour.H + neighbour.Value);
}
else if (g + neighbour.H + neighbour.Value < neighbour.F)
{
neighbour.G = g;
neighbour.F = neighbour.G + neighbour.H + neighbour.Value;
neighbour.Parent = node;
}
}
}
var path = new Stack <Cell>();
while (node != null)
{
path.Push(node);
node = node.Parent;
}
return(path.ToArray());
}
public Cell[] Find(Vector2Int start, Vector2Int goal)
{
Reset();
Cell startCell = _grid[start];
Cell goalCell = _grid[goal];
_open.Enqueue(startCell, 0);
var bounds = _grid.Size;
Cell node = null;
while (_open.Count > 0)
{
node = _open.Dequeue();
node.Closed = true;
var cBlock = false;
var g = node.G + 1;
if (goalCell.Location == node.Location)
{
break;
}
Vector2Int proposed = new Vector2Int(0, 0);
for (var i = 0; i < PathingConstants.Directions.Length; i++)
{
var direction = PathingConstants.Directions[i];
proposed.X = node.Location.X + direction.X;
proposed.Y = node.Location.Y + direction.Y;
// Bounds checking
if (proposed.X < 0 || proposed.X >= bounds.X ||
proposed.Y < 0 || proposed.Y >= bounds.Y)
{
continue;
}
Cell neighbour = _grid[proposed];
if (neighbour.Blocked)
{
if (i < 4)
{
cBlock = true;
}
continue;
}
// Prevent slipping between blocked cardinals by an open diagonal
if (i >= 4 && cBlock)
{
continue;
}
if (_grid[neighbour.Location].Closed)
{
continue;
}
if (!_open.Contains(neighbour))
{
neighbour.G = g;
neighbour.H = Heuristic(neighbour, node);
neighbour.Parent = node;
// F will be set by the queue
_open.Enqueue(neighbour, neighbour.G + neighbour.H);
}
else if (g + neighbour.H < neighbour.F)
{
neighbour.G = g;
neighbour.F = neighbour.G + neighbour.H;
neighbour.Parent = node;
}
}
}
var path = new Stack <Cell>();
while (node != null)
{
path.Push(node);
node = node.Parent;
}
return(path.ToArray());
}
