public void AddConnectionToVertex(GraphVertex destinationVertex) { var edge = new GraphEdge { PointA = this, PointB = destinationVertex }; this.Connections.Add(edge); }
public static int Solve(GraphVertex startVertex, GraphVertex destinationVertex) { var distanceToDestination = -1; var knownVertexes = new Dictionary <GraphVertex, int>(); var unvisitedVertexes = new BetterQueue <GraphVertex>(); var visitedVertexes = new HashSet <GraphVertex>(); var currentVertex = startVertex; knownVertexes.Add(currentVertex, 0); while (true) { if (currentVertex == destinationVertex) { distanceToDestination = knownVertexes[currentVertex]; break; } var currentShortestPathDistance = -1; GraphVertex currentShortestPathVertex = null; foreach (var connection in currentVertex.Connections) { var newVertex = connection.PointA == currentVertex ? connection.PointB : connection.PointA; if (visitedVertexes.Contains(newVertex)) { continue; } if (!unvisitedVertexes.Contains(newVertex)) { unvisitedVertexes.Enqueue(newVertex); } var pathDistance = knownVertexes[currentVertex] + 1; if (knownVertexes.ContainsKey(newVertex)) { if (knownVertexes[newVertex] > pathDistance) { knownVertexes[newVertex] = pathDistance; } } else { knownVertexes.Add(newVertex, pathDistance); } if (currentShortestPathDistance == -1 || currentShortestPathDistance > pathDistance) { currentShortestPathDistance = pathDistance; currentShortestPathVertex = newVertex; } } visitedVertexes.Add(currentVertex); // TODO I think this is a bug, but I need to figure out a test to confirm it. //unvisitedVertexes.Remove(currentVertex); // if this is a dead-end if (currentShortestPathDistance == -1) { if (unvisitedVertexes.Count > 0) { currentVertex = unvisitedVertexes.Dequeue(); } else { // the destination is not in the graph break; } } else { currentVertex = currentShortestPathVertex; } } return(distanceToDestination); }