private static IEnumerable <GraphNode> GenerateChildren(GraphNode state, Node[,] nodes, int maxAvail, X_Y endTargetNodePos)
{
var children = new X_Y[]
{
new X_Y()
{
X = state.EmptyNodePos.X - 1, Y = state.EmptyNodePos.Y
},
new X_Y()
{
X = state.EmptyNodePos.X + 1, Y = state.EmptyNodePos.Y
},
new X_Y()
{
X = state.EmptyNodePos.X, Y = state.EmptyNodePos.Y + 1
},
new X_Y()
{
X = state.EmptyNodePos.X, Y = state.EmptyNodePos.Y - 1
},
};
foreach (var childPos in children)
{
if (!(nodes.InBound(childPos) && CanMove(childPos, nodes, maxAvail)))
{
continue;
}
var targetPos = state.TargetNodePos.Equals(childPos) ? new X_Y()
{
X = state.EmptyNodePos.X, Y = state.EmptyNodePos.Y
} : new X_Y()
{
X = state.TargetNodePos.X, Y = state.TargetNodePos.Y
};
var child = new GraphNode
{
EmptyNodePos = childPos,
TargetNodePos = targetPos,
F = CalcF(targetPos, childPos, endTargetNodePos),
H = state.H + 1,
Parent = state,
};
yield return(child);
}
}
private static string PrintState(string[,] output, X_Y emptyNodePos, string title)
{
var outStr = new StringBuilder();
outStr.AppendLine(title);
for (int i = 0; i < output.GetLength(0); ++i)
{
if (emptyNodePos.X == i && emptyNodePos.Y == 0)
{
outStr.Append("[");
}
else if (i == 0)
{
outStr.Append("(");
}
else
{
outStr.Append(" ");
}
for (int j = 0; j < output.GetLength(1); ++j)
{
outStr.Append(output[i, j].ToString());
if (i == emptyNodePos.X && j == emptyNodePos.Y - 1)
{
outStr.Append("[");
}
else if (i == emptyNodePos.X && j == emptyNodePos.Y)
{
outStr.Append("]");
}
else if (i == 0 && j == 0)
{
outStr.Append(")");
}
else
{
outStr.Append(" ");
}
}
outStr.AppendLine();
}
return(outStr.ToString());
}
private MapNode[,] InitMap(Node[,] nodes, X_Y posOfEmptyNode)
{
var maxAvail = nodes[posOfEmptyNode.X, posOfEmptyNode.Y].Available;
MapNode[,] output = new MapNode[nodes.GetLength(0), nodes.GetLength(1)];
for (int i = 0; i < output.GetLength(0); ++i)
{
for (int j = 0; j < output.GetLength(1); ++j)
{
output[i, j] = new MapNode
{
CanMove = nodes[i, j].Used <= maxAvail,
};
}
}
return(output);
}
private static X_Y GetEmptyNode(Node[,] nodes)
{
var posOfEmptyNode = new X_Y();
var maxAvail = 0;
for (int i = 0; i < nodes.GetLength(0); ++i)
{
for (int j = 0; j < nodes.GetLength(1); ++j)
{
if (maxAvail < nodes[i, j].Available)
{
maxAvail = nodes[i, j].Available;
posOfEmptyNode.X = i;
posOfEmptyNode.Y = j;
}
}
}
return(posOfEmptyNode);
}
/// <summary>
/// Calculates heuristic distance to target node.
/// </summary>
/// <param name="dataPos">The data node position.</param>
/// <param name="emptyPos">The empty node.</param>
/// <param name="targetNodePos">The target node.</param>
/// <returns>The heuristic distance to target node.</returns>
public static int CalcF(X_Y dataPos, X_Y emptyPos, X_Y targetNodePos)
{
var adjustedTargets = new X_Y[]
{
new X_Y()
{
X = dataPos.X - 1, Y = dataPos.Y
},
new X_Y()
{
X = dataPos.X + 1, Y = dataPos.Y
},
new X_Y()
{
X = dataPos.X, Y = dataPos.Y + 1
},
new X_Y()
{
X = dataPos.X, Y = dataPos.Y - 1
},
};
var fs = adjustedTargets.Select(t =>
{
var distanceTargetY = Math.Abs(dataPos.Y - targetNodePos.Y);
var distanceEmptyY = Math.Abs(t.Y - targetNodePos.Y);
var distanceTargetX = Math.Abs(dataPos.X - targetNodePos.X);
var distanceEmptyX = Math.Abs(t.X - targetNodePos.X);
var f = CalcFAdjusted(distanceTargetX, distanceTargetY, distanceEmptyX, distanceEmptyY);
return(new { h = emptyPos.CalcManhattanDistance(t), f });
})
.OrderBy(x => x.h)
.ThenBy(x => x.f);
var target = fs.First();
return(target.h + target.f);
}
private IEnumerable <State> FillGraphForLocation(Dictionary <char, Dictionary <char, GraphNode> > graph, MapNode[,] map, Dictionary <char, X_Y> locations, char location)
{
var visited = new bool[map.GetLength(0), map.GetLength(1)];
var paths = new Dictionary <char, GraphNode>();
var open = new HashSet <GraphNode>
{
new GraphNode
{
CurrentPos = locations[location],
LocationsToVisit = string.Empty,
LastLocation = location,
},
};
while (open.Any())
{
var nextOpen = new HashSet <GraphNode>();
foreach (var node in open)
{
var childrenPos = new X_Y[]
{
new X_Y()
{
X = node.CurrentPos.X - 1, Y = node.CurrentPos.Y
},
new X_Y()
{
X = node.CurrentPos.X + 1, Y = node.CurrentPos.Y
},
new X_Y()
{
X = node.CurrentPos.X, Y = node.CurrentPos.Y + 1
},
new X_Y()
{
X = node.CurrentPos.X, Y = node.CurrentPos.Y - 1
},
};
foreach (var childPos in childrenPos)
{
var childNode = new GraphNode
{
CurrentPos = childPos,
LocationsToVisit = node.LocationsToVisit,
H = node.H + 1,
Parent = node,
LastLocation = node.LastLocation,
};
if (!map.InBound(childPos) ||
!map[childPos.X, childPos.Y].CanMove ||
visited[childPos.X, childPos.Y] ||
nextOpen.Contains(childNode))
{
continue;
}
nextOpen.Add(childNode);
if (map[childPos.X, childPos.Y].Location.HasValue)
{
paths.Add(map[childPos.X, childPos.Y].Location.Value, childNode);
yield return(new State
{
Path = childNode,
});
}
}
visited[node.CurrentPos.X, node.CurrentPos.Y] = true;
}
open = nextOpen;
}
graph.Add(location, paths);
}
private static bool CanMove(X_Y childPos, Node[,] nodes, int maxAvail)
{
return(nodes[childPos.X, childPos.Y].Used <= maxAvail);
}
private static IEnumerable <State> FindPath(Node[,] nodes, X_Y emptyNodePos)
{
var targetNodePos = new X_Y {
X = nodes.GetLength(0) - 1, Y = 0
};
var maxAvail = nodes[emptyNodePos.X, emptyNodePos.Y].Available;
var initState = new GraphNode
{
TargetNodePos = targetNodePos,
EmptyNodePos = emptyNodePos,
H = 0,
F = 0,
};
var endTargetNodePos = new X_Y {
X = 0, Y = 0
};
var open = new HashSetOrderedBy <GraphNode, int>((state) => state.G);
var close = new HashSet <GraphNode>();
open.Add(initState);
var count = 0;
GraphNode path = null;
while (open.Any() && path == null)
{
count++;
var currentNode = open.ValueWithMinSelector();
var children = GenerateChildren(currentNode, nodes, maxAvail, endTargetNodePos);
foreach (var child in children)
{
if (open.TryGetValue(child, out GraphNode existingNode) || close.TryGetValue(child, out existingNode))
{
if (existingNode.H <= child.H)
{
continue;
}
open.Remove(existingNode);
}
open.Add(child);
if (child.TargetNodePos.Equals(endTargetNodePos))
{
path = child;
break;
}
}
open.Remove(currentNode);
close.Add(currentNode);
yield return(new State
{
Closed = close,
Open = open,
LastClosed = currentNode,
Path = path,
});
}
}
