public static List <DijkstraVertex> GetDijkstraVertices(bool[,] graph, Position from, double speed) { var start = new DijkstraVertex { Position = from, SpeedLeft = speed, Previous = from }; DijkstraVertices.Clear(); FindDijkstraVertices(graph, start); return(DijkstraVertices); }
private static void FindDijkstraVertices(bool[,] graph, DijkstraVertex start) { foreach (var offset in DiagonalOffset ? Offsets8 : Offsets4) { var currentPosition = start.Position; var nextPosition = start.Position + offset; if (nextPosition.X < 0 || nextPosition.X >= graph.GetLength(0) || nextPosition.Y < 0 || nextPosition.Y >= graph.GetLength(1)) { continue; } if (!graph[currentPosition.X, currentPosition.Y] || !graph[nextPosition.X, nextPosition.Y]) { continue; } var speedLeft = start.SpeedLeft - (DiagonalOffset ? DiagonalSpeedCost : 1); if (speedLeft < 0) { continue; } var vertex = DijkstraVertices.FirstOrDefault(i => i.Position == nextPosition); if (vertex == null) { vertex = new DijkstraVertex { Position = nextPosition.Copy(), SpeedLeft = speedLeft, Previous = currentPosition.Copy(), }; DijkstraVertices.Add(vertex); FindDijkstraVertices(graph, vertex); } else if (vertex.SpeedLeft < speedLeft) { vertex.SpeedLeft = speedLeft; vertex.Previous = currentPosition.Copy(); FindDijkstraVertices(graph, vertex); } } }