public static GridPosition[] FindPath(GridPosition from, GridPosition to, GridGeometry geometry, GridSize size)
{
var visitedNodes = new GridNodesData <bool>(size);
var traces = new GridNodesData <GridPosition>(size);
var counter = 0;
visitedNodes[from] = true;
var nextSteps = new List <GridPosition> {
from
};
var currentStep = new List <GridPosition>();
while (nextSteps.Count != 0 && !visitedNodes[to])
{
var t = currentStep;
currentStep = nextSteps;
nextSteps = t;
nextSteps.Clear();
foreach (var p in currentStep)
{
foreach (var n in geometry.NeighborPositions(p, size))
{
if (visitedNodes[n])
{
continue;
}
// TODO can check for the walls between p and n here
visitedNodes[n] = true;
traces[n] = p;
nextSteps.Add(n);
}
}
counter++;
}
if (!visitedNodes[to])
{
Debug.LogError("Failed to find a path");
return(null);
}
var cursor = to;
var result = new GridPosition[counter + 1];
result[counter] = cursor;
while (cursor != from)
{
counter--;
cursor = traces[cursor];
result[counter] = cursor;
}
return(result);
}
private void CreateConnections(GridNodesData <GameObject> nodes)
{
foreach (var(f, t) in _geometry.AllConnections(size))
{
var from = _geometry.PositionCoordinates(f);
var connection = Instantiate(connectionPrefab, from, Quaternion.identity, transform);
connection.transform.LookAt(nodes[t].transform);
}
}
private GridNodesData <GameObject> CreateNodes()
{
var result = new GridNodesData <GameObject>(size);
foreach (var pos in _geometry.AllNodes(size))
{
var coords = _geometry.PositionCoordinates(pos);
var vec = new Vector3(coords.x, coords.y, 0);
var node = Instantiate(nodePrefab, vec, Quaternion.identity, transform);
result[pos] = node;
node.GetComponent <GridNode>().Init(pos, this);
}
return(result);
}