Add A* algorithm

A-Star/AStar.swift

@@ -0,0 +1,153 @@
+// Written by Alejandro Isaza.
+
+import Foundation
+
+public protocol Graph {
+    associatedtype Vertex: Hashable
+    associatedtype Edge: WeightedEdge where Edge.Vertex == Vertex
+
+    /// Lists all edges going out from a vertex.
+    func edgesOutgoing(from vertex: Vertex) -> [Edge]
+}
+
+public protocol WeightedEdge {
+    associatedtype Vertex
+
+    /// The edge's cost.
+    var cost: Double { get }
+
+    /// The target vertex.
+    var target: Vertex { get }
+}
+
+public final class AStar<G: Graph> {
+    /// The graph to search on.
+    public let graph: G
+
+    /// The heuristic cost function that estimates the cost between two vertices.
+    ///
+    /// - Note: The heuristic function needs to always return a value that is lower-than or equal to the actual
+    ///         cost for the resulting path of the A* search to be optimal.
+    public let heuristic: (G.Vertex, G.Vertex) -> Double
+
+    /// Open list of nodes to expand.
+    private var open: HashedHeap<Node<G.Vertex>>
+
+    /// Closed list of vertices already expanded.
+    private var closed = Set<G.Vertex>()
+
+    /// Actual vertex cost for vertices we already encountered (refered to as `g` on the literature).
+    private var costs = Dictionary<G.Vertex, Double>()
+
+    /// Store the previous node for each expanded node to recreate the path.
+    private var parents = Dictionary<G.Vertex, G.Vertex>()
+
+    /// Initializes `AStar` with a graph and a heuristic cost function.
+    public init(graph: G, heuristic: @escaping (G.Vertex, G.Vertex) -> Double) {
+        self.graph = graph
+        self.heuristic = heuristic
+        open = HashedHeap(sort: <)
+    }
+
+    /// Finds an optimal path between `source` and `target`.
+    ///
+    /// - Precondition: both `source` and `target` belong to `graph`.
+    public func path(start: G.Vertex, target: G.Vertex) -> [G.Vertex] {
+        open.insert(Node<G.Vertex>(vertex: start, cost: 0, estimate: heuristic(start, target)))
+        while !open.isEmpty {
+            guard let node = open.remove() else {
+                break
+            }
+            costs[node.vertex] = node.cost
+
+            if (node.vertex == target) {
+                let path = buildPath(start: start, target: target)
+                cleanup()
+                return path
+            }
+
+            if !closed.contains(node.vertex) {
+                expand(node: node, target: target)
+                closed.insert(node.vertex)
+            }
+        }
+
+        // No path found
+        return []
+    }
+
+    private func expand(node: Node<G.Vertex>, target: G.Vertex) {
+        let edges = graph.edgesOutgoing(from: node.vertex)
+        for edge in edges {
+            let g = cost(node.vertex) + edge.cost
+            if g < cost(edge.target) {
+                open.insert(Node<G.Vertex>(vertex: edge.target, cost: g, estimate: heuristic(edge.target, target)))
+                parents[edge.target] = node.vertex
+            }
+        }
+    }
+
+    private func cost(_ vertex: G.Edge.Vertex) -> Double {
+        if let c = costs[vertex] {
+            return c
+        }
+
+        let node = Node(vertex: vertex, cost: Double.greatestFiniteMagnitude, estimate: 0)
+        if let index = open.index(of: node) {
+            return open[index].cost
+        }
+
+        return Double.greatestFiniteMagnitude
+    }
+
+    private func buildPath(start: G.Vertex, target: G.Vertex) -> [G.Vertex] {
+        var path = Array<G.Vertex>()
+        path.append(target)
+
+        var current = target
+        while current != start {
+            guard let parent = parents[current] else {
+                return [] // no path found
+            }
+            current = parent
+            path.append(current)
+        }
+
+        return path.reversed()
+    }
+
+    private func cleanup() {
+        open.removeAll()
+        closed.removeAll()
+        parents.removeAll()
+    }
+}
+
+private struct Node<V: Hashable>: Hashable, Comparable {
+    /// The graph vertex.
+    var vertex: V
+
+    /// The actual cost between the start vertex and this vertex.
+    var cost: Double
+
+    /// Estimated (heuristic) cost betweent this vertex and the target vertex.
+    var estimate: Double
+
+    public init(vertex: V, cost: Double, estimate: Double) {
+        self.vertex = vertex
+        self.cost = cost
+        self.estimate = estimate
+    }
+
+    static func < (lhs: Node<V>, rhs: Node<V>) -> Bool {
+        return lhs.cost + lhs.estimate < rhs.cost + rhs.estimate
+    }
+
+    static func == (lhs: Node<V>, rhs: Node<V>) -> Bool {
+        return lhs.vertex == rhs.vertex
+    }
+
+    var hashValue: Int {
+        return vertex.hashValue
+    }
+}

A-Star/README.md

@@ -0,0 +1,7 @@
+# A*
+
+A* (pronounced "ay star") is a heuristic best-first search algorithm. A* minimizes node expansions, therefore minimizing the search time, by using a heuristic function. The heuristic function gives an estimate of the distance between two vertices. For instance if you are searching for a path between two points in a city, you can estimate the actual street distance with the straight-line distance.
+
+A* works by expanding the most promising nodes first, according to the heuristic function. In the city example it would choose streets which go in the general direction of the target first and, only if those are dead ends, backtrack and try other streets. This speeds up search in most sitations.
+
+A* is optimal (it always find the shortest path) if the heuristic function is admissible. A heuristic function is admissible if it never overestimates the cost of reaching the goal. In the extreme case of the heuristic function always retuning `0` A* acts exactly the same as [Dijkstra's Algorithm](../Dijkstra). The closer the heuristic function is the the actual distance the faster the search.

A-Star/Tests/AStarTests.swift

@@ -0,0 +1,57 @@
+import Foundation
+import XCTest
+
+struct GridGraph: Graph {
+    struct Vertex: Hashable {
+        var x: Int
+        var y: Int
+
+        static func == (lhs: Vertex, rhs: Vertex) -> Bool {
+            return lhs.x == rhs.x && lhs.y == rhs.y
+        }
+
+        public var hashValue: Int {
+            return x.hashValue ^ y.hashValue
+        }
+    }
+
+    struct Edge: WeightedEdge {
+        var cost: Double
+        var target: Vertex
+    }
+
+    func edgesOutgoing(from vertex: Vertex) -> [Edge] {
+        return [
+            Edge(cost: 1, target: Vertex(x: vertex.x - 1, y: vertex.y)),
+            Edge(cost: 1, target: Vertex(x: vertex.x + 1, y: vertex.y)),
+            Edge(cost: 1, target: Vertex(x: vertex.x, y: vertex.y - 1)),
+            Edge(cost: 1, target: Vertex(x: vertex.x, y: vertex.y + 1)),
+        ]
+    }
+}
+
+class AStarTests: XCTestCase {
+    func testSameStartAndEnd() {
+        let graph = GridGraph()
+        let astar = AStar(graph: graph, heuristic: manhattanDistance)
+        let path = astar.path(start: GridGraph.Vertex(x: 0, y: 0), target: GridGraph.Vertex(x: 0, y: 0))
+        XCTAssertEqual(path.count, 1)
+        XCTAssertEqual(path[0].x, 0)
+        XCTAssertEqual(path[0].y, 0)
+    }
+
+    func testDiagonal() {
+        let graph = GridGraph()
+        let astar = AStar(graph: graph, heuristic: manhattanDistance)
+        let path = astar.path(start: GridGraph.Vertex(x: 0, y: 0), target: GridGraph.Vertex(x: 10, y: 10))
+        XCTAssertEqual(path.count, 21)
+        XCTAssertEqual(path[0].x, 0)
+        XCTAssertEqual(path[0].y, 0)
+        XCTAssertEqual(path[20].x, 10)
+        XCTAssertEqual(path[20].y, 10)
+    }
+
+    func manhattanDistance(_ s: GridGraph.Vertex, _ t: GridGraph.Vertex) -> Double {
+        return Double(abs(s.x - t.x) + abs(s.y - t.y))
+    }
+}