public void AddNode(GraphNode node) { nodeSet.Add(node); }
public void AddDirectedEdge(GraphNode from, GraphNode to, int cost = 100000) { from.neighbors.Add(to); from.Costs.Add(cost); }