public void AddLink(GraphNode link)
{
this.linked.Add(link);
}
public int DistTo(GraphNode neighbor)
{
return(this.linked.Contains(neighbor) ? 1 : int.MaxValue);
}
public List <GraphNode> FindPathAStar(GraphNode start, GraphNode goal)
{
// The set of nodes already evaluated
var closedSet = new List <GraphNode>(this.nodes.Count);
// The set of currently discovered nodes that are not evaluated yet.
// Initially, only the start node is known.
var openSet = new List <GraphNode>(this.nodes.Count)
{
start
};
// For each node, which node it can most efficiently be reached from.
// If a node can be reached from many nodes, cameFrom will eventually contain the
// most efficient previous step.
var cameFrom = new Dictionary <GraphNode, GraphNode>(this.nodes.Count);
// For each node, the cost of getting from the start node to that node.
var gScore = new Dictionary <GraphNode, int>(this.nodes.Count);
this.nodes.Values.Map((node) => gScore[node] = int.MaxValue);
// The cost of going from start to start is zero.
gScore[start] = 0;
// For each node, the total cost of getting from the start node to the goal
// by passing by that node. That value is partly known, partly heuristic.
var fScore = new Dictionary <GraphNode, int>(this.nodes.Count);
this.nodes.Values.Map((node) => fScore[node] = int.MaxValue);
// For the first node, that value is completely heuristic.
fScore[start] = start.HeuriDist(goal);
while (openSet.Count > 0)
{
// node having the lowest fScore value (approx closest one)
var current = openSet.SelectMin((item) => fScore[item]);
if (current == goal) // we have reached our goal
{
var path = new List <GraphNode>()
{
current
};
while (cameFrom.ContainsKey(current))
{
current = cameFrom[current];
path.Insert(0, current);
}
return(path);
}
openSet.Remove(current);
closedSet.Add(current);
foreach (var neighbor in current.Links)
{
if (closedSet.Contains(neighbor))
{
// already evaluated
continue;
}
if (!openSet.Contains(neighbor))
{
// new node
openSet.Add(neighbor);
}
// The distance from start to a neighbor
var tentativeGScore = gScore[current] + current.DistTo(neighbor);
if (tentativeGScore >= gScore[neighbor])
{
continue; // This is not a better path.
}
// This path is the best until now. Record it!
cameFrom[neighbor] = current;
gScore[neighbor] = tentativeGScore;
fScore[neighbor] = gScore[neighbor] + neighbor.HeuriDist(goal);
}
}
return(null);
}
public int HeuriDist(GraphNode other)
{
var dist = 0;
return(dist);
}
