Skip to content

Commit 23c8fa2

Browse files
AvanatikerAvanatiker
committed
3D Implementation
1 parent 72a5f49 commit 23c8fa2

File tree

6 files changed

+193
-288
lines changed

6 files changed

+193
-288
lines changed

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

Lines changed: 49 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
package com.lambda.pathing.dstar
1919

20+
import com.lambda.util.world.FastVector
2021
import kotlin.math.min
2122

2223
/**
@@ -34,20 +35,20 @@ import kotlin.math.min
3435
*/
3536
class DStarLite(
3637
private val graph: Graph,
37-
private val heuristic: (Int, Int) -> Double,
38-
var start: Int,
39-
val goal: Int
38+
private val heuristic: (FastVector, FastVector) -> Double,
39+
var start: FastVector,
40+
private val goal: FastVector
4041
) {
4142
private val INF = Double.POSITIVE_INFINITY
4243

4344
// 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>()
45+
private val gMap = mutableMapOf<FastVector, Double>()
46+
private val rhsMap = mutableMapOf<FastVector, Double>()
4647

47-
// Priority queue
48+
// Priority queue holding inconsistent vertices.
4849
private val U = PriorityQueueDStar()
4950

50-
// Heuristic shift
51+
// km accumulates heuristic differences as the start changes.
5152
private var km = 0.0
5253

5354
init {
@@ -67,37 +68,36 @@ class DStarLite(
6768
U.insertOrUpdate(goal, calculateKey(goal))
6869
}
6970

70-
private fun g(u: Int): Double = gMap[u] ?: INF
71-
private fun setG(u: Int, value: Double) {
72-
gMap[u] = value
73-
}
71+
private fun g(u: FastVector): Double = gMap[u] ?: INF
72+
private fun setG(u: FastVector, value: Double) { gMap[u] = value }
7473

75-
private fun rhs(u: Int): Double = rhsMap[u] ?: INF
76-
private fun setRHS(u: Int, value: Double) {
77-
rhsMap[u] = value
78-
}
74+
private fun rhs(u: FastVector): Double = rhsMap[u] ?: INF
75+
private fun setRHS(u: FastVector, value: Double) { rhsMap[u] = value }
7976

8077
/**
81-
* Key(u) = ( min(g(u), rhs(u)) + h(start, u) + km , min(g(u), rhs(u)) ).
78+
* Calculates the key for vertex u.
79+
* Key(u) = ( min(g(u), rhs(u)) + h(start, u) + km, min(g(u), rhs(u)) )
8280
*/
83-
private fun calculateKey(u: Int): Key {
81+
private fun calculateKey(u: FastVector): Key {
8482
val minGRHS = min(g(u), rhs(u))
8583
return Key(minGRHS + heuristic(start, u) + km, minGRHS)
8684
}
8785

8886
/**
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))
87+
* Updates the vertex u.
88+
*
89+
* If u != goal, then:
90+
* rhs(u) = min_{v in Pred(u)} [g(v) + cost(v,u)]
91+
*
92+
* Then u is removed from the queue and reinserted if it is inconsistent.
9393
*/
94-
fun updateVertex(u: Int) {
94+
fun updateVertex(u: FastVector) {
9595
if (u != goal) {
9696
var tmp = INF
97-
graph.predecessors(u).forEach { (pred, c) ->
98-
val valCandidate = g(pred) + c
99-
if (valCandidate < tmp) {
100-
tmp = valCandidate
97+
graph.predecessors(u).forEach { (pred, cost) ->
98+
val candidate = g(pred) + cost
99+
if (candidate < tmp) {
100+
tmp = candidate
101101
}
102102
}
103103
setRHS(u, tmp)
@@ -109,15 +109,13 @@ class DStarLite(
109109
}
110110

111111
/**
112+
* Propagates changes until the start is locally consistent.
112113
* computeShortestPath():
113114
* While the queue top is "less" than calculateKey(start)
114115
* or g(start) < rhs(start), pop and process.
115116
*/
116117
fun computeShortestPath() {
117-
while (
118-
(U.topKey() < calculateKey(start)) ||
119-
(g(start) < rhs(start))
120-
) {
118+
while ((U.topKey() < calculateKey(start)) || (g(start) < rhs(start))) {
121119
val u = U.top() ?: break
122120
val oldKey = U.topKey()
123121
val newKey = calculateKey(u)
@@ -126,24 +124,21 @@ class DStarLite(
126124
// Priority out-of-date; update it
127125
U.insertOrUpdate(u, newKey)
128126
} else {
129-
// Remove it from queue
130127
U.pop()
131128
if (g(u) > rhs(u)) {
132129
// We found a better path for u
133130
setG(u, rhs(u))
134-
// Update successors of u
135-
graph.successors(u).forEach { (s, _) ->
136-
updateVertex(s)
131+
graph.successors(u).forEach { (succ, _) ->
132+
updateVertex(succ)
137133
}
138134
} else {
139-
// g(u) <= rhs(u)
140-
val gOld = g(u)
135+
val oldG = g(u)
141136
setG(u, INF)
142137
updateVertex(u)
143138
// 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)
139+
graph.successors(u).forEach { (succ, _) ->
140+
if (rhs(succ) == oldG + graph.cost(u, succ)) {
141+
updateVertex(succ)
147142
}
148143
}
149144
}
@@ -152,16 +147,15 @@ class DStarLite(
152147
}
153148

154149
/**
155-
* Called when the robot moves from oldStart to newStart.
156-
* We increase km by h(oldStart, newStart), then re-key all queued vertices.
150+
* When the robot moves, update the start.
151+
* The variable km is increased by h(oldStart, newStart) and
152+
* all vertices in the queue are re-keyed.
157153
*/
158-
fun updateStart(newStart: Int) {
154+
fun updateStart(newStart: FastVector) {
159155
val oldStart = start
160156
start = newStart
161157
km += heuristic(oldStart, start)
162-
163-
// Re-key everything in U
164-
val tmpList = mutableListOf<Int>()
158+
val tmpList = mutableListOf<FastVector>()
165159
while (!U.isEmpty()) {
166160
U.top()?.let { tmpList.add(it) }
167161
U.pop()
@@ -172,34 +166,29 @@ class DStarLite(
172166
}
173167

174168
/**
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.
169+
* Retrieves a path from start to goal by always choosing the successor
170+
* with the lowest g + cost value. If no path is found, the path stops early.
178171
*/
179-
fun getPath(): List<Int> {
180-
val path = mutableListOf<Int>()
172+
fun getPath(): List<FastVector> {
173+
val path = mutableListOf<FastVector>()
181174
var current = start
182175
path.add(current)
183-
184176
while (current != goal) {
185177
val successors = graph.successors(current)
186178
if (successors.isEmpty()) break
187-
188-
var bestNext: Int? = null
179+
var bestNext: FastVector? = null
189180
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
181+
for ((succ, cost) in successors) {
182+
val candidate = g(succ) + cost
183+
if (candidate < bestVal) {
184+
bestVal = candidate
185+
bestNext = succ
195186
}
196187
}
197188
// No path
198189
if (bestNext == null) break
199190
current = bestNext
200191
path.add(current)
201-
202-
// Safety net to avoid infinite loops
203192
if (path.size > 100000) break
204193
}
205194
return path

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

Lines changed: 17 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -17,41 +17,34 @@
1717

1818
package com.lambda.pathing.dstar
1919

20+
import com.lambda.util.world.FastVector
21+
2022
/**
21-
* Simple graph class that stores both forward and reverse adjacency.
23+
* A simple 3D graph that uses FastVector (a Long) to represent 3D nodes.
2224
*
23-
* @param adjMap a map from each vertex u to a list of (successor, cost) pairs.
25+
* @param adjMap A map from each vertex to a list of (neighbor, cost) pairs.
2426
*/
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>>>()
27+
class Graph(
28+
private val adjMap: Map<FastVector, List<Pair<FastVector, Double>>>
29+
) {
30+
// Build reverse adjacency from forward edges.
31+
private val revAdjMap: Map<FastVector, List<Pair<FastVector, Double>>> by lazy {
32+
val tmp = mutableMapOf<FastVector, MutableList<Pair<FastVector, Double>>>()
3033
adjMap.forEach { (u, edges) ->
3134
edges.forEach { (v, cost) ->
3235
tmp.getOrPut(v) { mutableListOf() }.add(Pair(u, cost))
3336
}
3437
}
35-
// Convert to immutable
3638
tmp.mapValues { it.value.toList() }
3739
}
3840

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()
41+
/** Returns the successors of a vertex. */
42+
fun successors(u: FastVector) = adjMap[u] ?: emptyList()
4343

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()
44+
/** Returns the predecessors of a vertex. */
45+
fun predecessors(u: FastVector) = revAdjMap[u] ?: emptyList()
4846

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
47+
/** Returns the cost of the edge from u to v (or ∞ if none exists). */
48+
fun cost(u: FastVector, v: FastVector) =
49+
adjMap[u]?.firstOrNull { it.first == v }?.second ?: Double.POSITIVE_INFINITY
5750
}

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

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -18,16 +18,16 @@
1818
package com.lambda.pathing.dstar
1919

2020
/**
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').
21+
* A Key is a pair (k1, k2) that is used to order vertices in the priority queue.
22+
* They are compared lexicographically.
2323
*/
2424
data class Key(val k1: Double, val k2: Double) : Comparable<Key> {
2525
override fun compareTo(other: Key): Int {
2626
return when {
2727
this.k1 < other.k1 -> -1
2828
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
29+
this.k2 < other.k2 -> -1
30+
this.k2 > other.k2 -> 1
3131
else -> 0
3232
}
3333
}

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

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -17,14 +17,16 @@
1717

1818
package com.lambda.pathing.dstar
1919

20+
import com.lambda.util.world.FastVector
2021
import java.util.*
2122

2223
/**
23-
* Priority queue for D* Lite
24+
* Priority queue for D* Lite 3D.
25+
* Supports: topKey, top, pop, insertOrUpdate, and remove.
2426
*/
2527
class PriorityQueueDStar {
26-
private val pq = PriorityQueue<Pair<Int, Key>>(compareBy { it.second })
27-
private val vertexToKey = mutableMapOf<Int, Key>()
28+
private val pq = PriorityQueue<Pair<FastVector, Key>>(compareBy { it.second })
29+
private val vertexToKey = mutableMapOf<FastVector, Key>()
2830

2931
fun isEmpty(): Boolean = pq.isEmpty()
3032

@@ -33,18 +35,16 @@ class PriorityQueueDStar {
3335
else pq.peek().second
3436
}
3537

36-
fun top(): Int? {
37-
return pq.peek()?.first
38-
}
38+
fun top() = pq.peek()?.first
3939

40-
fun pop(): Int? {
40+
fun pop(): FastVector? {
4141
if (pq.isEmpty()) return null
4242
val (v, _) = pq.poll()
4343
vertexToKey.remove(v)
4444
return v
4545
}
4646

47-
fun insertOrUpdate(v: Int, key: Key) {
47+
fun insertOrUpdate(v: FastVector, key: Key) {
4848
val oldKey = vertexToKey[v]
4949
if (oldKey == null || oldKey != key) {
5050
remove(v)
@@ -53,7 +53,7 @@ class PriorityQueueDStar {
5353
}
5454
}
5555

56-
fun remove(v: Int) {
56+
fun remove(v: FastVector) {
5757
val oldKey = vertexToKey[v] ?: return
5858
vertexToKey.remove(v)
5959
pq.remove(Pair(v, oldKey))

0 commit comments

Comments
 (0)