Skip to content

Commit 72a5f49

Browse files
AvanatikerAvanatiker
committed
D* Lite - 2D with tests
1 parent 9f23026 commit 72a5f49

File tree

5 files changed

+550
-0
lines changed

5 files changed

+550
-0
lines changed

‎common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt‎

Lines changed: 207 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,207 @@
1+
/*
2+
* Copyright 2025 Lambda
3+
*
4+
* This program is free software: you can redistribute it and/or modify
5+
* it under the terms of the GNU General Public License as published by
6+
* the Free Software Foundation, either version 3 of the License, or
7+
* (at your option) any later version.
8+
*
9+
* This program is distributed in the hope that it will be useful,
10+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12+
* GNU General Public License for more details.
13+
*
14+
* You should have received a copy of the GNU General Public License
15+
* along with this program. If not, see <http://www.gnu.org/licenses/>.
16+
*/
17+
18+
package com.lambda.pathing.dstar
19+
20+
import kotlin.math.min
21+
22+
/**
23+
* D* Lite Implementation.
24+
*
25+
* We perform a backward search from the goal to the start, so:
26+
* - rhs(goal) = 0, g(goal) = ∞
27+
* - "start" is the robot's current location from which we want a path *to* the goal
28+
* - 'km' accumulates the heuristic shift so we don't reorder the entire queue after each move
29+
*
30+
* @param graph The graph on which we plan (with forward + reverse adjacency).
31+
* @param heuristic A consistent (or at least nonnegative) heuristic function h(a,b).
32+
* @param start The robot's current position.
33+
* @param goal The fixed goal vertex.
34+
*/
35+
class DStarLite(
36+
private val graph: Graph,
37+
private val heuristic: (Int, Int) -> Double,
38+
var start: Int,
39+
val goal: Int
40+
) {
41+
private val INF = Double.POSITIVE_INFINITY
42+
43+
// gMap[u], rhsMap[u] store g(u) and rhs(u) or default to ∞ if not present
44+
private val gMap = mutableMapOf<Int, Double>()
45+
private val rhsMap = mutableMapOf<Int, Double>()
46+
47+
// Priority queue
48+
private val U = PriorityQueueDStar()
49+
50+
// Heuristic shift
51+
private var km = 0.0
52+
53+
init {
54+
initialize()
55+
}
56+
57+
/**
58+
* Initialize D* Lite:
59+
* - g(goal)=∞, rhs(goal)=0
60+
* - Insert goal into U with key = calculateKey(goal).
61+
*/
62+
private fun initialize() {
63+
gMap.clear()
64+
rhsMap.clear()
65+
setG(goal, INF)
66+
setRHS(goal, 0.0)
67+
U.insertOrUpdate(goal, calculateKey(goal))
68+
}
69+
70+
private fun g(u: Int): Double = gMap[u] ?: INF
71+
private fun setG(u: Int, value: Double) {
72+
gMap[u] = value
73+
}
74+
75+
private fun rhs(u: Int): Double = rhsMap[u] ?: INF
76+
private fun setRHS(u: Int, value: Double) {
77+
rhsMap[u] = value
78+
}
79+
80+
/**
81+
* Key(u) = ( min(g(u), rhs(u)) + h(start, u) + km , min(g(u), rhs(u)) ).
82+
*/
83+
private fun calculateKey(u: Int): Key {
84+
val minGRHS = min(g(u), rhs(u))
85+
return Key(minGRHS + heuristic(start, u) + km, minGRHS)
86+
}
87+
88+
/**
89+
* UpdateVertex(u):
90+
* 1) If u != goal, rhs(u) = min_{v in Pred(u)} [g(v) + cost(v,u)]
91+
* 2) Remove u from U
92+
* 3) If g(u) != rhs(u), insertOrUpdate(u, calculateKey(u))
93+
*/
94+
fun updateVertex(u: Int) {
95+
if (u != goal) {
96+
var tmp = INF
97+
graph.predecessors(u).forEach { (pred, c) ->
98+
val valCandidate = g(pred) + c
99+
if (valCandidate < tmp) {
100+
tmp = valCandidate
101+
}
102+
}
103+
setRHS(u, tmp)
104+
}
105+
U.remove(u)
106+
if (g(u) != rhs(u)) {
107+
U.insertOrUpdate(u, calculateKey(u))
108+
}
109+
}
110+
111+
/**
112+
* computeShortestPath():
113+
* While the queue top is "less" than calculateKey(start)
114+
* or g(start) < rhs(start), pop and process.
115+
*/
116+
fun computeShortestPath() {
117+
while (
118+
(U.topKey() < calculateKey(start)) ||
119+
(g(start) < rhs(start))
120+
) {
121+
val u = U.top() ?: break
122+
val oldKey = U.topKey()
123+
val newKey = calculateKey(u)
124+
125+
if (oldKey < newKey) {
126+
// Priority out-of-date; update it
127+
U.insertOrUpdate(u, newKey)
128+
} else {
129+
// Remove it from queue
130+
U.pop()
131+
if (g(u) > rhs(u)) {
132+
// We found a better path for u
133+
setG(u, rhs(u))
134+
// Update successors of u
135+
graph.successors(u).forEach { (s, _) ->
136+
updateVertex(s)
137+
}
138+
} else {
139+
// g(u) <= rhs(u)
140+
val gOld = g(u)
141+
setG(u, INF)
142+
updateVertex(u)
143+
// Update successors that may have relied on old g(u)
144+
graph.successors(u).forEach { (s, _) ->
145+
if (rhs(s) == gOld + graph.cost(u, s)) {
146+
updateVertex(s)
147+
}
148+
}
149+
}
150+
}
151+
}
152+
}
153+
154+
/**
155+
* Called when the robot moves from oldStart to newStart.
156+
* We increase km by h(oldStart, newStart), then re-key all queued vertices.
157+
*/
158+
fun updateStart(newStart: Int) {
159+
val oldStart = start
160+
start = newStart
161+
km += heuristic(oldStart, start)
162+
163+
// Re-key everything in U
164+
val tmpList = mutableListOf<Int>()
165+
while (!U.isEmpty()) {
166+
U.top()?.let { tmpList.add(it) }
167+
U.pop()
168+
}
169+
tmpList.forEach { v ->
170+
U.insertOrUpdate(v, calculateKey(v))
171+
}
172+
}
173+
174+
/**
175+
* Returns a path from 'start' to 'goal' by always choosing
176+
* the successor s of the current vertex that minimizes g(s)+cost(current->s).
177+
* If no path is found, the path ends prematurely.
178+
*/
179+
fun getPath(): List<Int> {
180+
val path = mutableListOf<Int>()
181+
var current = start
182+
path.add(current)
183+
184+
while (current != goal) {
185+
val successors = graph.successors(current)
186+
if (successors.isEmpty()) break
187+
188+
var bestNext: Int? = null
189+
var bestVal = INF
190+
for ((s, c) in successors) {
191+
val valCandidate = g(s) + c
192+
if (valCandidate < bestVal) {
193+
bestVal = valCandidate
194+
bestNext = s
195+
}
196+
}
197+
// No path
198+
if (bestNext == null) break
199+
current = bestNext
200+
path.add(current)
201+
202+
// Safety net to avoid infinite loops
203+
if (path.size > 100000) break
204+
}
205+
return path
206+
}
207+
}

‎common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
/*
2+
* Copyright 2025 Lambda
3+
*
4+
* This program is free software: you can redistribute it and/or modify
5+
* it under the terms of the GNU General Public License as published by
6+
* the Free Software Foundation, either version 3 of the License, or
7+
* (at your option) any later version.
8+
*
9+
* This program is distributed in the hope that it will be useful,
10+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12+
* GNU General Public License for more details.
13+
*
14+
* You should have received a copy of the GNU General Public License
15+
* along with this program. If not, see <http://www.gnu.org/licenses/>.
16+
*/
17+
18+
package com.lambda.pathing.dstar
19+
20+
/**
21+
* Simple graph class that stores both forward and reverse adjacency.
22+
*
23+
* @param adjMap a map from each vertex u to a list of (successor, cost) pairs.
24+
*/
25+
class Graph(private val adjMap: Map<Int, List<Pair<Int, Double>>>) {
26+
27+
// Build reverse adjacency by scanning all forward edges
28+
private val revAdjMap: Map<Int, List<Pair<Int, Double>>> by lazy {
29+
val tmp = mutableMapOf<Int, MutableList<Pair<Int, Double>>>()
30+
adjMap.forEach { (u, edges) ->
31+
edges.forEach { (v, cost) ->
32+
tmp.getOrPut(v) { mutableListOf() }.add(Pair(u, cost))
33+
}
34+
}
35+
// Convert to immutable
36+
tmp.mapValues { it.value.toList() }
37+
}
38+
39+
/**
40+
* Returns the successors of u, i.e., all (v, cost) for edges u->v.
41+
*/
42+
fun successors(u: Int) = adjMap[u] ?: emptyList()
43+
44+
/**
45+
* Returns the predecessors of u, i.e., all (v, cost) for edges v->u.
46+
*/
47+
fun predecessors(u: Int) = revAdjMap[u] ?: emptyList()
48+
49+
/**
50+
* Helper to get the cost of an edge u->v, or ∞ if none.
51+
*/
52+
fun cost(u: Int, v: Int) =
53+
adjMap[u]
54+
?.firstOrNull { it.first == v }
55+
?.second
56+
?: Double.POSITIVE_INFINITY
57+
}

‎common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
/*
2+
* Copyright 2025 Lambda
3+
*
4+
* This program is free software: you can redistribute it and/or modify
5+
* it under the terms of the GNU General Public License as published by
6+
* the Free Software Foundation, either version 3 of the License, or
7+
* (at your option) any later version.
8+
*
9+
* This program is distributed in the hope that it will be useful,
10+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12+
* GNU General Public License for more details.
13+
*
14+
* You should have received a copy of the GNU General Public License
15+
* along with this program. If not, see <http://www.gnu.org/licenses/>.
16+
*/
17+
18+
package com.lambda.pathing.dstar
19+
20+
/**
21+
* A Key is a pair (k1, k2) used in D* Lite's priority queue. We compare them lexicographically:
22+
* (k1, k2) < (k1', k2') iff k1 < k1' or (k1 == k1' and k2 < k2').
23+
*/
24+
data class Key(val k1: Double, val k2: Double) : Comparable<Key> {
25+
override fun compareTo(other: Key): Int {
26+
return when {
27+
this.k1 < other.k1 -> -1
28+
this.k1 > other.k1 -> 1
29+
this.k2 < other.k2 -> if (this.k1 == other.k1) -1 else 0
30+
this.k2 > other.k2 -> if (this.k1 == other.k1) 1 else 0
31+
else -> 0
32+
}
33+
}
34+
}

‎common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
/*
2+
* Copyright 2025 Lambda
3+
*
4+
* This program is free software: you can redistribute it and/or modify
5+
* it under the terms of the GNU General Public License as published by
6+
* the Free Software Foundation, either version 3 of the License, or
7+
* (at your option) any later version.
8+
*
9+
* This program is distributed in the hope that it will be useful,
10+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12+
* GNU General Public License for more details.
13+
*
14+
* You should have received a copy of the GNU General Public License
15+
* along with this program. If not, see <http://www.gnu.org/licenses/>.
16+
*/
17+
18+
package com.lambda.pathing.dstar
19+
20+
import java.util.*
21+
22+
/**
23+
* Priority queue for D* Lite
24+
*/
25+
class PriorityQueueDStar {
26+
private val pq = PriorityQueue<Pair<Int, Key>>(compareBy { it.second })
27+
private val vertexToKey = mutableMapOf<Int, Key>()
28+
29+
fun isEmpty(): Boolean = pq.isEmpty()
30+
31+
fun topKey(): Key {
32+
return if (pq.isEmpty()) Key(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY)
33+
else pq.peek().second
34+
}
35+
36+
fun top(): Int? {
37+
return pq.peek()?.first
38+
}
39+
40+
fun pop(): Int? {
41+
if (pq.isEmpty()) return null
42+
val (v, _) = pq.poll()
43+
vertexToKey.remove(v)
44+
return v
45+
}
46+
47+
fun insertOrUpdate(v: Int, key: Key) {
48+
val oldKey = vertexToKey[v]
49+
if (oldKey == null || oldKey != key) {
50+
remove(v)
51+
vertexToKey[v] = key
52+
pq.add(Pair(v, key))
53+
}
54+
}
55+
56+
fun remove(v: Int) {
57+
val oldKey = vertexToKey[v] ?: return
58+
vertexToKey.remove(v)
59+
pq.remove(Pair(v, oldKey))
60+
}
61+
}

0 commit comments

Comments
 (0)