public int Distance(Node x, Node y)
{
int xRow = x.Id / rows + 1;
int yRow = y.Id / rows + 1;
int xCol = x.Id % rows + 1;
int yCol = y.Id % rows + 1;
return Math.Abs(yRow - xRow) + Math.Abs(yCol - xCol);
}
// Critical Section for implementing 3-Node Corner detection
public void AddAdjacent(Node node, Node adjacent)
{
if (explored.Contains(adjacent) || reachable.Contains(adjacent))
return;
adjacent.previous = node;
reachable.Add(adjacent);
}
public void Setup()
{
graph = new Graph(new int[,] {
{ 0, 0, 0 },
{ 0, 0, 1 }, // Obstacle still part of adjacencyList
{ 0, 0, 0 }
});
node = graph.nodes[graph.nodes.Length / 2];
}
public void Start(Node start, Node goal)
{
// Clear and reset all values
reachable = new List<Node>();
reachable.Add(start);
goalNode = goal;
explored = new List<Node>();
path = new List<Node>();
iterations = 0;
for (int i = 0; i < graph.nodes.Length; ++i)
{
graph.nodes[i].Clear();
}
}
private void SetupNeighbors()
{
rows = grid.GetLength(0);
columns = grid.GetLength(1);
// Since each node is going to store a reference to its own neighbors,
// the node data can be kept as a flattened array
nodes = new Node[grid.Length];
// Setup initial empty nodes
for (int i = 0; i < nodes.Length; ++i)
{
var node = new Node();
node.Id = i;
nodes[i] = node;
}
// Build adjacency list for each node of grid
for (int r = 0; r < rows; ++r)
{
for (int c = 0; c < columns; ++c)
{
Node node = nodes[columns * r + c];
// 1 represents unusable node for path, 0 is an available tile
// If the current node in the grid is an obstacle, check if corner
// If not, a crossing formed by parallel neighbors can be created
if (grid[r, c] == 1)
{
if (r > 0 && r < rows - 1 && c > 0 && c < columns - 1)
{
if (grid[r - 1, c] == 1 && grid[r + 1, c] == 1 &&
grid[r, c - 1] == 0 && grid[r, c + 1] == 0)
{
node.adjacencyList.Add(nodes[columns * r + c + 1]);
node.adjacencyList.Add(nodes[columns * r + c - 1]);
}
else if (grid[r - 1, c] == 0 && grid[r + 1, c] == 0 &&
grid[r, c - 1] == 1 && grid[r, c + 1] == 1)
{
node.adjacencyList.Add(nodes[columns * (r - 1) + c]);
node.adjacencyList.Add(nodes[columns * (r + 1) + c]);
}
}
else // Boundary locations
{
// Left or right columns
if ((c == 0 || c == columns - 1) && (r > 0 && r < rows - 1) &&
(grid[r - 1, c] == 0 && grid[r + 1, c] == 0))
{
node.adjacencyList.Add(nodes[columns * (r - 1) + c]);
node.adjacencyList.Add(nodes[columns * (r + 1) + c]);
}
// Top or bottom rows
else if ((r == 0 || r == rows - 1) && (c > 0 && c < columns - 1) &&
(grid[r, c - 1] == 0 && grid[r, c + 1] == 0))
{
node.adjacencyList.Add(nodes[columns * r + c + 1]);
node.adjacencyList.Add(nodes[columns * r + c - 1]);
}
}
continue;
}
// Up
if (r > 0)
{
node.adjacencyList.Add(nodes[columns * (r - 1) + c]);
}
// Right
if (c < columns - 1)
{
node.adjacencyList.Add(nodes[columns * r + c + 1]);
}
// Down
if (r < rows - 1)
{
node.adjacencyList.Add(nodes[columns * (r + 1) + c]);
}
// Left
if (c > 0)
{
node.adjacencyList.Add(nodes[columns * r + c - 1]);
}
}
}
}
public void Clear()
{
previous = null;
}
private int CornerHeuristic(Node node)
{
if (node.previous != null)
{
if (node.previous.previous != null)
{
if ((node.previous.Id == node.Id + graph.columns && node.previous.previous.Id == node.Id + graph.columns - 1) || // 1
(node.previous.Id == node.Id + graph.columns && node.previous.previous.Id == node.Id + graph.columns + 1) || // 2
(node.previous.Id == node.Id - graph.columns && node.previous.previous.Id == node.Id - graph.columns - 1) || // 3
(node.previous.Id == node.Id - graph.columns && node.previous.previous.Id == node.Id - graph.columns + 1) || // 4
(node.previous.Id == node.Id + 1 && node.previous.previous.Id == node.Id + 1 - graph.columns) || // 5
(node.previous.Id == node.Id - 1 && node.previous.previous.Id == node.Id - 1 - graph.columns) || // 6
(node.previous.Id == node.Id + 1 && node.previous.previous.Id == node.Id + 1 + graph.columns) || // 7
(node.previous.Id == node.Id - 1 && node.previous.previous.Id == node.Id - 1 + graph.columns)) // 8
{
return 50;
}
}
}
return 0;
}