11package de.ronny_h.extensions
22
3- import java.util.*
3+ import java.util.PriorityQueue
44import kotlin.Int.Companion.MAX_VALUE
55
6- // NOTE: This modified A* implementation is based on the pseudocode on the Wikipedia page
6+ // NOTE: This A* implementation is a 1:1 equivalent in Kotlin to the pseudo code on the Wikipedia page
77// https://en.wikipedia.org/wiki/A*_search_algorithm
88
9-
10- private fun <N > reconstructPaths (cameFrom : Map <N , Collection <N >>, last : N ): List <List <N >> {
11- if (! cameFrom.contains(last)) {
12- return listOf (listOf (last))
9+ private fun <N > reconstructPath (cameFrom : Map <N , N >, last : N ): List <N > {
10+ var current = last
11+ val totalPath = mutableListOf (current)
12+ while (current in cameFrom.keys) {
13+ current = cameFrom.getValue(current)
14+ totalPath.add(0 , current)
1315 }
14- return cameFrom.getValue(last)
15- .flatMap { pred -> reconstructPaths(cameFrom, pred) }
16- .map { path -> path + last }
17- .toList()
16+ return totalPath
1817}
1918
2019data class ShortestPath <N >(val path : List <N >, val distance : Int )
2120
2221private const val LARGE_VALUE = MAX_VALUE / 2
2322
2423/* *
25- * A modified A* algorithm that finds all shortest paths from `start` to `goal`.
24+ * A* finds a path from `start` to `goal`.
2625 * @param start the start node
27- * @param isGoal predicate deciding if a node is a goal
26+ * @param goal the goal node
2827 * @param neighbors is a function that returns the list of neighbours for a given node.
2928 * @param d is the distance/cost function. d(m,n) provides the distance (or cost) to reach node n from node m.
3029 * @param h is the heuristic function. h(n) estimates the cost to reach goal from node n.
3130 */
32- fun <N > aStar (
33- start : N , isGoal : N .() -> Boolean , neighbors : (N ) -> List <N >, d : (N , N ) -> Int , h : (N ) -> Int ,
34- printIt : (visited: Set <N >, current: N , additionalInfo: () -> String ) -> Unit = { _, _, _ -> }
35- ): List <ShortestPath <N >> {
31+ fun <N > aStar (start : N , goal : N , neighbors : (N ) -> List <N >, d : (N , N ) -> Int , h : (N ) -> Int ,
32+ printIt : (visited: Set <N >, current: N , additionalInfo: () -> String ) -> Unit = {_, _, _ -> }): ShortestPath <N > {
3633 // For node n, fScore[n] := gScore[n] + h(n). fScore[n] represents our current best guess as to
3734 // how cheap a path could be from start to finish if it goes through n.
38- val fScore = mutableMapOf<N , Int >().withDefault( { _ -> LARGE_VALUE }) // map with default value of Infinity
35+ val fScore = mutableMapOf<N , Int >().withDefault { _ -> LARGE_VALUE } // map with default value of Infinity
3936
4037 // The set of discovered nodes that may need to be (re-)expanded.
4138 // Initially, only the start node is known.
4239 // This is usually implemented as a min-heap or priority queue rather than a hash-set.
43- val openSet =
44- PriorityQueue <N > { a, b -> fScore.getValue(a).compareTo(fScore.getValue(b)) }
40+ val openSet = PriorityQueue <N > { a, b -> fScore.getValue(a).compareTo(fScore.getValue(b)) }
4541 openSet.add(start)
4642
47- // For node n, cameFrom[n] is the list of nodes immediately preceding it on the cheapest paths from the start
43+ // For node n, cameFrom[n] is the node immediately preceding it on the cheapest path from the start
4844 // to n currently known.
49- val cameFrom = mutableMapOf<N , MutableSet < N > >()
45+ val cameFrom = mutableMapOf<N , N >()
5046
5147 // For node n, gScore[n] is the currently known cost of the cheapest path from start to n.
52- val gScore = mutableMapOf<N , Int >().withDefault( { _ -> LARGE_VALUE }) // map with default value of Infinity
48+ val gScore = mutableMapOf<N , Int >().withDefault { _ -> LARGE_VALUE } // map with default value of Infinity
5349 gScore[start] = 0
5450
5551 fScore[start] = h(start)
5652
5753 while (openSet.isNotEmpty()) {
5854 // This operation can occur in O(Log(N)) time if openSet is a min-heap or a priority queue
5955 val current = openSet.peek()
60- if (current.isGoal() ) {
61- return reconstructPaths( cameFrom, current).map { x -> ShortestPath (x , gScore.getValue(current)) }
56+ if (current == goal ) {
57+ return ShortestPath (reconstructPath( cameFrom, current), gScore.getValue(current))
6258 }
6359
6460 openSet.remove(current)
6561 for (neighbor in neighbors(current)) {
6662 // d(current,neighbor) is the weight of the edge from current to neighbor
6763 // tentative_gScore is the distance from start to the neighbor through current
6864 val tentativeGScore = gScore.getValue(current) + d(current, neighbor)
69- if (tentativeGScore <= gScore.getValue(neighbor)) {
70- if (tentativeGScore < gScore.getValue(neighbor)) {
71- // This path to neighbor is better than any previous one. Record it!
72- cameFrom[neighbor] = mutableSetOf (current)
73- } else {
74- // This path to neighbor is equal to the best one. Record it!
75- cameFrom.getOrPut(neighbor) { mutableSetOf () } + = current
76- }
65+ if (tentativeGScore < gScore.getValue(neighbor)) {
66+ // This path to neighbor is better than any previous one. Record it!
67+ cameFrom[neighbor] = current
7768 gScore[neighbor] = tentativeGScore
7869 fScore[neighbor] = tentativeGScore + h(neighbor)
7970 if (neighbor !in openSet) {
@@ -87,5 +78,5 @@ fun <N> aStar(
8778 }
8879
8980 // Open set is empty but goal was never reached
90- error(" No path found from $start to goal" )
81+ error(" No path found from $start to $ goal" )
9182}
0 commit comments