@@ -3,68 +3,77 @@ package de.ronny_h.extensions
33import java.util.*
44import kotlin.Int.Companion.MAX_VALUE
55
6- // NOTE: This A* implementation is a 1:1 equivalent in Kotlin to the pseudo code on the Wikipedia page
6+ // NOTE: This modified A* implementation is based on the pseudocode on the Wikipedia page
77// https://en.wikipedia.org/wiki/A*_search_algorithm
88
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)
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))
1513 }
16- return totalPath
14+ return cameFrom.getValue(last)
15+ .flatMap { pred -> reconstructPaths(cameFrom, pred) }
16+ .map { path -> path + last }
17+ .toList()
1718}
1819
1920data class ShortestPath <N >(val path : List <N >, val distance : Int )
2021
2122private const val LARGE_VALUE = MAX_VALUE / 2
2223
2324/* *
24- * A* finds a path from `start` to `goal`.
25+ * A modified A* algorithm that finds all shortest paths from `start` to `goal`.
2526 * @param start the start node
26- * @param goal the goal node
27+ * @param isGoal predicate deciding if a node is a goal
2728 * @param neighbors is a function that returns the list of neighbours for a given node.
2829 * @param d is the distance/cost function. d(m,n) provides the distance (or cost) to reach node n from node m.
2930 * @param h is the heuristic function. h(n) estimates the cost to reach goal from node n.
3031 */
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 > {
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 >> {
3336 // For node n, fScore[n] := gScore[n] + h(n). fScore[n] represents our current best guess as to
3437 // how cheap a path could be from start to finish if it goes through n.
35- val fScore = mutableMapOf<N , Int >() // map with default value of Infinity
38+ val fScore = mutableMapOf<N , Int >().withDefault({ _ -> LARGE_VALUE }) // map with default value of Infinity
3639
3740 // The set of discovered nodes that may need to be (re-)expanded.
3841 // Initially, only the start node is known.
3942 // This is usually implemented as a min-heap or priority queue rather than a hash-set.
40- val openSet = PriorityQueue <N > { a, b -> fScore.getOrDefault(a, LARGE_VALUE ).compareTo(fScore.getOrDefault(b, LARGE_VALUE )) }
43+ val openSet =
44+ PriorityQueue <N > { a, b -> fScore.getValue(a).compareTo(fScore.getValue(b)) }
4145 openSet.add(start)
4246
43- // For node n, cameFrom[n] is the node immediately preceding it on the cheapest path from the start
47+ // For node n, cameFrom[n] is the list of nodes immediately preceding it on the cheapest paths from the start
4448 // to n currently known.
45- val cameFrom = mutableMapOf<N , N >()
49+ val cameFrom = mutableMapOf<N , MutableSet < N > >()
4650
4751 // For node n, gScore[n] is the currently known cost of the cheapest path from start to n.
48- val gScore = mutableMapOf<N , Int >() // map with default value of Infinity
52+ val gScore = mutableMapOf<N , Int >().withDefault({ _ -> LARGE_VALUE }) // map with default value of Infinity
4953 gScore[start] = 0
5054
5155 fScore[start] = h(start)
5256
5357 while (openSet.isNotEmpty()) {
5458 // This operation can occur in O(Log(N)) time if openSet is a min-heap or a priority queue
5559 val current = openSet.peek()
56- if (current == goal ) {
57- return ShortestPath (reconstructPath( cameFrom, current), gScore.getValue(current))
60+ if (current.isGoal() ) {
61+ return reconstructPaths( cameFrom, current).map { x -> ShortestPath (x , gScore.getValue(current)) }
5862 }
5963
6064 openSet.remove(current)
6165 for (neighbor in neighbors(current)) {
6266 // d(current,neighbor) is the weight of the edge from current to neighbor
6367 // tentative_gScore is the distance from start to the neighbor through current
64- val tentativeGScore = gScore.getOrDefault(current, LARGE_VALUE ) + d(current, neighbor)
65- if (tentativeGScore < gScore.getOrDefault(neighbor, LARGE_VALUE )) {
66- // This path to neighbor is better than any previous one. Record it!
67- cameFrom[neighbor] = current
68+ 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+ }
6877 gScore[neighbor] = tentativeGScore
6978 fScore[neighbor] = tentativeGScore + h(neighbor)
7079 if (neighbor !in openSet) {
@@ -78,5 +87,5 @@ fun <N> aStar(start: N, goal: N, neighbors: (N) -> List<N>, d: (N, N) -> Int, h:
7887 }
7988
8089 // Open set is empty but goal was never reached
81- error(" No path found from $start to $ goal" )
90+ error(" No path found from $start to goal" )
8291}
0 commit comments