diff --git a/Algorithms.Tests/Graph/AStarTests.cs b/Algorithms.Tests/Graph/AStarTests.cs new file mode 100644 index 00000000..c622c125 --- /dev/null +++ b/Algorithms.Tests/Graph/AStarTests.cs @@ -0,0 +1,443 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using Algorithms.Graph; +using FluentAssertions; +using NUnit.Framework; + +namespace Algorithms.Tests.Graph; + +/// +/// Tests for A* pathfinding algorithm. +/// +public class AStarTests +{ + #region Generic Graph Tests + + [Test] + public void FindPath_SimpleGraph_ReturnsShortestPath() + { + // Arrange: Simple graph A -> B -> C + IEnumerable<(string, double)> GetNeighbors(string node) => node switch + { + "A" => new[] { ("B", 1.0) }, + "B" => new[] { ("C", 1.0) }, + _ => Array.Empty<(string, double)>(), + }; + + double Heuristic(string a, string b) => 0; // Dijkstra mode + + // Act + var path = AStar.FindPath("A", "C", GetNeighbors, Heuristic); + + // Assert + path.Should().NotBeNull(); + path.Should().Equal("A", "B", "C"); + } + + [Test] + public void FindPath_MultiplePathsGraph_ReturnsShortestPath() + { + // Arrange: Graph with multiple paths + // A --1--> B --1--> D + // | ^ + // +------5----------+ + IEnumerable<(string, double)> GetNeighbors(string node) => node switch + { + "A" => new[] { ("B", 1.0), ("D", 5.0) }, + "B" => new[] { ("D", 1.0) }, + _ => Array.Empty<(string, double)>(), + }; + + double Heuristic(string a, string b) => 0; + + // Act + var path = AStar.FindPath("A", "D", GetNeighbors, Heuristic); + + // Assert + path.Should().NotBeNull(); + path.Should().Equal("A", "B", "D"); + } + + [Test] + public void FindPath_NoPath_ReturnsNull() + { + // Arrange: Disconnected graph + IEnumerable<(string, double)> GetNeighbors(string node) => node switch + { + "A" => new[] { ("B", 1.0) }, + "C" => new[] { ("D", 1.0) }, + _ => Array.Empty<(string, double)>(), + }; + + double Heuristic(string a, string b) => 0; + + // Act + var path = AStar.FindPath("A", "D", GetNeighbors, Heuristic); + + // Assert + path.Should().BeNull(); + } + + [Test] + public void FindPath_StartEqualsGoal_ReturnsSingleNode() + { + // Arrange + IEnumerable<(string, double)> GetNeighbors(string node) => Array.Empty<(string, double)>(); + double Heuristic(string a, string b) => 0; + + // Act + var path = AStar.FindPath("A", "A", GetNeighbors, Heuristic); + + // Assert + path.Should().NotBeNull(); + path.Should().Equal("A"); + } + + [Test] + public void FindPath_WithHeuristic_FindsOptimalPath() + { + // Arrange: Graph where heuristic guides search + IEnumerable<(string, double)> GetNeighbors(string node) => node switch + { + "A" => new[] { ("B", 1.0), ("C", 4.0) }, + "B" => new[] { ("D", 2.0) }, + "C" => new[] { ("D", 1.0) }, + _ => Array.Empty<(string, double)>(), + }; + + var positions = new Dictionary + { + ["A"] = (0, 0), + ["B"] = (1, 0), + ["C"] = (0, 1), + ["D"] = (2, 0), + }; + + double Heuristic(string a, string b) + { + var (x1, y1) = positions[a]; + var (x2, y2) = positions[b]; + return Math.Abs(x1 - x2) + Math.Abs(y1 - y2); + } + + // Act + var path = AStar.FindPath("A", "D", GetNeighbors, Heuristic); + + // Assert + path.Should().NotBeNull(); + path.Should().Equal("A", "B", "D"); + } + + [Test] + public void FindPath_NullGetNeighbors_ThrowsArgumentNullException() + { + // Arrange + double Heuristic(string a, string b) => 0; + + // Act + Action act = () => AStar.FindPath("A", "B", null!, Heuristic); + + // Assert + act.Should().Throw().WithParameterName("getNeighbors"); + } + + [Test] + public void FindPath_NullHeuristic_ThrowsArgumentNullException() + { + // Arrange + IEnumerable<(string, double)> GetNeighbors(string node) => Array.Empty<(string, double)>(); + + // Act + Action act = () => AStar.FindPath("A", "B", GetNeighbors, null!); + + // Assert + act.Should().Throw().WithParameterName("heuristic"); + } + + #endregion + + #region Grid Tests + + [Test] + public void FindPathInGrid_SimpleGrid_ReturnsShortestPath() + { + // Arrange: 3x3 grid, all walkable + var grid = new bool[,] + { + { true, true, true }, + { true, true, true }, + { true, true, true }, + }; + + // Act + var path = AStar.FindPathInGrid(grid, (0, 0), (2, 2), allowDiagonal: false); + + // Assert + path.Should().NotBeNull(); + path!.Count.Should().Be(5); // Manhattan distance: right, right, down, down + path.First().Should().Be((0, 0)); + path.Last().Should().Be((2, 2)); + } + + [Test] + public void FindPathInGrid_WithObstacle_FindsAlternatePath() + { + // Arrange: Grid with obstacle in the middle + var grid = new bool[,] + { + { true, true, true }, + { true, false, true }, + { true, true, true }, + }; + + // Act + var path = AStar.FindPathInGrid(grid, (0, 0), (2, 2), allowDiagonal: false); + + // Assert + path.Should().NotBeNull(); + path.Should().NotContain((1, 1)); // Should avoid obstacle + path!.First().Should().Be((0, 0)); + path!.Last().Should().Be((2, 2)); + } + + [Test] + public void FindPathInGrid_NoPath_ReturnsNull() + { + // Arrange: Grid with wall blocking path + var grid = new bool[,] + { + { true, false, true }, + { true, false, true }, + { true, false, true }, + }; + + // Act + var path = AStar.FindPathInGrid(grid, (0, 0), (0, 2), allowDiagonal: false); + + // Assert + path.Should().BeNull(); + } + + [Test] + public void FindPathInGrid_DiagonalAllowed_UsesDiagonalPath() + { + // Arrange: 3x3 grid, all walkable + var grid = new bool[,] + { + { true, true, true }, + { true, true, true }, + { true, true, true }, + }; + + // Act + var path = AStar.FindPathInGrid(grid, (0, 0), (2, 2), allowDiagonal: true); + + // Assert + path.Should().NotBeNull(); + path!.Count.Should().Be(3); // Diagonal path is shorter + path.First().Should().Be((0, 0)); + path.Last().Should().Be((2, 2)); + } + + [Test] + public void FindPathInGrid_LargeGrid_FindsPath() + { + // Arrange: 10x10 grid + var grid = new bool[10, 10]; + for (int i = 0; i < 10; i++) + { + for (int j = 0; j < 10; j++) + { + grid[i, j] = true; + } + } + + // Act + var path = AStar.FindPathInGrid(grid, (0, 0), (9, 9), allowDiagonal: false); + + // Assert + path.Should().NotBeNull(); + path!.Count.Should().Be(19); // Manhattan distance + path.First().Should().Be((0, 0)); + path.Last().Should().Be((9, 9)); + } + + [Test] + public void FindPathInGrid_ComplexMaze_FindsPath() + { + // Arrange: Complex maze + var grid = new bool[,] + { + { true, true, false, true, true }, + { false, true, false, true, false }, + { true, true, true, true, true }, + { true, false, false, false, true }, + { true, true, true, true, true }, + }; + + // Act + var path = AStar.FindPathInGrid(grid, (0, 0), (4, 4), allowDiagonal: false); + + // Assert + path.Should().NotBeNull(); + path!.First().Should().Be((0, 0)); + path!.Last().Should().Be((4, 4)); + } + + [Test] + public void FindPathInGrid_NullGrid_ThrowsArgumentNullException() + { + // Act + Action act = () => AStar.FindPathInGrid(null!, (0, 0), (1, 1)); + + // Assert + act.Should().Throw().WithParameterName("grid"); + } + + [Test] + public void FindPathInGrid_StartOutOfBounds_ThrowsArgumentException() + { + // Arrange + var grid = new bool[3, 3]; + + // Act + Action act = () => AStar.FindPathInGrid(grid, (-1, 0), (1, 1)); + + // Assert + act.Should().Throw().WithParameterName("start"); + } + + + + [Test] + public void FindPathInGrid_StartNotWalkable_ThrowsArgumentException() + { + // Arrange + var grid = new bool[,] + { + { false, true, true }, + { true, true, true }, + { true, true, true }, + }; + + // Act + Action act = () => AStar.FindPathInGrid(grid, (0, 0), (2, 2)); + + // Assert + act.Should().Throw().WithParameterName("start"); + } + + [Test] + public void FindPathInGrid_GoalNotWalkable_ThrowsArgumentException() + { + // Arrange + var grid = new bool[,] + { + { true, true, true }, + { true, true, true }, + { true, true, false }, + }; + + // Act + Action act = () => AStar.FindPathInGrid(grid, (0, 0), (2, 2)); + + // Assert + act.Should().Throw().WithParameterName("goal"); + } + + #endregion + + #region Path Cost Tests + + [Test] + public void CalculatePathCost_SimplePath_ReturnsCorrectCost() + { + // Arrange + var path = new List { "A", "B", "C" }; + double GetCost(string a, string b) => 1.0; + + // Act + var cost = AStar.CalculatePathCost(path, GetCost); + + // Assert + cost.Should().Be(2.0); + } + + [Test] + public void CalculatePathCost_VariableCosts_ReturnsCorrectCost() + { + // Arrange + var path = new List { "A", "B", "C", "D" }; + var costs = new Dictionary<(string, string), double> + { + [("A", "B")] = 1.5, + [("B", "C")] = 2.0, + [("C", "D")] = 3.5, + }; + + double GetCost(string a, string b) => costs[(a, b)]; + + // Act + var cost = AStar.CalculatePathCost(path, GetCost); + + // Assert + cost.Should().Be(7.0); + } + + [Test] + public void CalculatePathCost_SingleNode_ReturnsZero() + { + // Arrange + var path = new List { "A" }; + double GetCost(string a, string b) => 1.0; + + // Act + var cost = AStar.CalculatePathCost(path, GetCost); + + // Assert + cost.Should().Be(0); + } + + [Test] + public void CalculatePathCost_EmptyPath_ReturnsZero() + { + // Arrange + var path = new List(); + double GetCost(string a, string b) => 1.0; + + // Act + var cost = AStar.CalculatePathCost(path, GetCost); + + // Assert + cost.Should().Be(0); + } + + #endregion + + #region Integer Node Tests + + [Test] + public void FindPath_IntegerNodes_FindsPath() + { + // Arrange: Graph with integer nodes + IEnumerable<(int, double)> GetNeighbors(int node) => node switch + { + 1 => new[] { (2, 1.0), (3, 2.0) }, + 2 => new[] { (4, 1.0) }, + 3 => new[] { (4, 1.0) }, + _ => Array.Empty<(int, double)>(), + }; + + double Heuristic(int a, int b) => Math.Abs(a - b); + + // Act + var path = AStar.FindPath(1, 4, GetNeighbors, Heuristic); + + // Assert + path.Should().NotBeNull(); + path.Should().Equal(1, 2, 4); + } + + #endregion +} diff --git a/Algorithms/Graph/AStar.cs b/Algorithms/Graph/AStar.cs new file mode 100644 index 00000000..9e9b6622 --- /dev/null +++ b/Algorithms/Graph/AStar.cs @@ -0,0 +1,220 @@ +using System; +using System.Collections.Generic; +using System.Linq; + +namespace Algorithms.Graph; + +/// +/// A* (A-Star) pathfinding algorithm implementation. +/// Finds the shortest path between two nodes using a heuristic function. +/// +public static class AStar +{ + /// + /// Finds the shortest path from start to goal using A* algorithm. + /// + /// Type of node identifier. + /// Starting node. + /// Goal node. + /// Function to get neighbors of a node with their costs. + /// Heuristic function estimating cost from node to goal. + /// List of nodes representing the path from start to goal, or null if no path exists. + public static List? FindPath( + T start, + T goal, + Func> getNeighbors, + Func heuristic) where T : notnull + { + if (getNeighbors == null) + { + throw new ArgumentNullException(nameof(getNeighbors)); + } + + if (heuristic == null) + { + throw new ArgumentNullException(nameof(heuristic)); + } + + var openSet = new PriorityQueue(); + var cameFrom = new Dictionary(); + var gScore = new Dictionary { [start] = 0 }; + var fScore = new Dictionary { [start] = heuristic(start, goal) }; + + openSet.Enqueue(start, fScore[start]); + + while (openSet.Count > 0) + { + var current = openSet.Dequeue(); + + if (EqualityComparer.Default.Equals(current, goal)) + { + return ReconstructPath(cameFrom, current); + } + + foreach (var (neighbor, cost) in getNeighbors(current)) + { + var tentativeGScore = gScore[current] + cost; + + if (!gScore.ContainsKey(neighbor) || tentativeGScore < gScore[neighbor]) + { + cameFrom[neighbor] = current; + gScore[neighbor] = tentativeGScore; + fScore[neighbor] = tentativeGScore + heuristic(neighbor, goal); + + openSet.Enqueue(neighbor, fScore[neighbor]); + } + } + } + + return null; // No path found + } + + /// + /// Finds the shortest path in a 2D grid from start to goal. + /// + /// 2D grid where true represents walkable cells and false represents obstacles. + /// Starting position (row, col). + /// Goal position (row, col). + /// Whether diagonal movement is allowed. + /// List of positions representing the path, or null if no path exists. + public static List<(int Row, int Col)>? FindPathInGrid( + bool[,] grid, + (int Row, int Col) start, + (int Row, int Col) goal, + bool allowDiagonal = false) + { + if (grid == null) + { + throw new ArgumentNullException(nameof(grid)); + } + + int rows = grid.GetLength(0); + int cols = grid.GetLength(1); + + ValidateGridPosition(grid, start, rows, cols, nameof(start)); + ValidateGridPosition(grid, goal, rows, cols, nameof(goal)); + + IEnumerable<((int Row, int Col) Node, double Cost)> GetNeighbors((int Row, int Col) pos) + { + return GetGridNeighbors(pos, rows, cols, grid, allowDiagonal); + } + + double ManhattanDistance((int Row, int Col) a, (int Row, int Col) b) + { + return Math.Abs(a.Row - b.Row) + Math.Abs(a.Col - b.Col); + } + + double EuclideanDistance((int Row, int Col) a, (int Row, int Col) b) + { + int dr = a.Row - b.Row; + int dc = a.Col - b.Col; + return Math.Sqrt((dr * dr) + (dc * dc)); + } + + Func<(int Row, int Col), (int Row, int Col), double> heuristic = allowDiagonal + ? EuclideanDistance + : ManhattanDistance; + + return FindPath(start, goal, GetNeighbors, heuristic); + } + + /// + /// Calculates the total cost of a path. + /// + /// Type of node identifier. + /// The path to calculate cost for. + /// Function to get the cost between two adjacent nodes. + /// Total cost of the path. + public static double CalculatePathCost(List path, Func getCost) + where T : notnull + { + if (path == null || path.Count < 2) + { + return 0; + } + + double totalCost = 0; + for (int i = 0; i < path.Count - 1; i++) + { + totalCost += getCost(path[i], path[i + 1]); + } + + return totalCost; + } + + private static void ValidateGridPosition( + bool[,] grid, + (int Row, int Col) position, + int rows, + int cols, + string paramName) + { + bool isOutOfBounds = position.Row < 0 || position.Row >= rows; + isOutOfBounds = isOutOfBounds || position.Col < 0 || position.Col >= cols; + + if (isOutOfBounds) + { + throw new ArgumentException("Position is out of bounds.", paramName); + } + + if (!grid[position.Row, position.Col]) + { + throw new ArgumentException("Position is not walkable.", paramName); + } + } + + private static IEnumerable<((int Row, int Col) Position, double Cost)> GetGridNeighbors( + (int Row, int Col) pos, + int rows, + int cols, + bool[,] grid, + bool allowDiagonal) + { + var neighbors = new List<((int Row, int Col), double)>(); + + var directions = new[] + { + (-1, 0), (1, 0), (0, -1), (0, 1), + }; + + if (allowDiagonal) + { + directions = directions.Concat(new[] + { + (-1, -1), (-1, 1), (1, -1), (1, 1), + }).ToArray(); + } + + foreach (var (dr, dc) in directions) + { + int newRow = pos.Row + dr; + int newCol = pos.Col + dc; + + bool isInBounds = newRow >= 0 && newRow < rows; + isInBounds = isInBounds && newCol >= 0 && newCol < cols; + + if (isInBounds && grid[newRow, newCol]) + { + // Cost is sqrt(2) for diagonal, 1 for cardinal + bool isDiagonal = dr != 0 && dc != 0; + double cost = isDiagonal ? Math.Sqrt(2) : 1.0; + neighbors.Add(((newRow, newCol), cost)); + } + } + + return neighbors; + } + + private static List ReconstructPath(Dictionary cameFrom, T current) where T : notnull + { + var path = new List { current }; + + while (cameFrom.ContainsKey(current)) + { + current = cameFrom[current]; + path.Insert(0, current); + } + + return path; + } +}