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 com.lambda.util.world.FastVector
21+ import kotlin.math.min
22+
23+ /* *
24+ * D* Lite Implementation.
25+ *
26+ * We perform a backward search from the goal to the start, so:
27+ * - rhs(goal) = 0, g(goal) = ∞
28+ * - "start" is the robot's current location from which we want a path *to* the goal
29+ * - 'km' accumulates the heuristic shift so we don't reorder the entire queue after each move
30+ *
31+ * @param graph The graph on which we plan (with forward + reverse adjacency).
32+ * @param heuristic A consistent (or at least nonnegative) heuristic function h(a,b).
33+ * @param start The robot's current position.
34+ * @param goal The fixed goal vertex.
35+ */
36+ class DStarLite (
37+ private val graph : Graph ,
38+ private val heuristic : (FastVector , FastVector ) -> Double ,
39+ var start : FastVector ,
40+ private val goal : FastVector
41+ ) {
42+ private val INF = Double .POSITIVE_INFINITY
43+
44+ // gMap[u], rhsMap[u] store g(u) and rhs(u) or default to ∞ if not present
45+ private val gMap = mutableMapOf<FastVector , Double >()
46+ private val rhsMap = mutableMapOf<FastVector , Double >()
47+
48+ // Priority queue holding inconsistent vertices.
49+ private val U = PriorityQueueDStar ()
50+
51+ // km accumulates heuristic differences as the start changes.
52+ private var km = 0.0
53+
54+ init {
55+ initialize()
56+ }
57+
58+ /* *
59+ * Initialize D* Lite:
60+ * - g(goal)=∞, rhs(goal)=0
61+ * - Insert goal into U with key = calculateKey(goal).
62+ */
63+ private fun initialize () {
64+ gMap.clear()
65+ rhsMap.clear()
66+ setG(goal, INF )
67+ setRHS(goal, 0.0 )
68+ U .insertOrUpdate(goal, calculateKey(goal))
69+ }
70+
71+ private fun g (u : FastVector ): Double = gMap[u] ? : INF
72+ private fun setG (u : FastVector , value : Double ) { gMap[u] = value }
73+
74+ private fun rhs (u : FastVector ): Double = rhsMap[u] ? : INF
75+ private fun setRHS (u : FastVector , value : Double ) { rhsMap[u] = value }
76+
77+ /* *
78+ * Calculates the key for vertex u.
79+ * Key(u) = ( min(g(u), rhs(u)) + h(start, u) + km, min(g(u), rhs(u)) )
80+ */
81+ private fun calculateKey (u : FastVector ): Key {
82+ val minGRHS = min(g(u), rhs(u))
83+ return Key (minGRHS + heuristic(start, u) + km, minGRHS)
84+ }
85+
86+ /* *
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.
93+ */
94+ fun updateVertex (u : FastVector ) {
95+ if (u != goal) {
96+ var tmp = INF
97+ graph.predecessors(u).forEach { (pred, cost) ->
98+ val candidate = g(pred) + cost
99+ if (candidate < tmp) {
100+ tmp = candidate
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+ * Propagates changes until the start is locally consistent.
113+ * computeShortestPath():
114+ * While the queue top is "less" than calculateKey(start)
115+ * or g(start) < rhs(start), pop and process.
116+ */
117+ fun computeShortestPath () {
118+ while ((U .topKey() < calculateKey(start)) || (g(start) < rhs(start))) {
119+ val u = U .top() ? : break
120+ val oldKey = U .topKey()
121+ val newKey = calculateKey(u)
122+
123+ if (oldKey < newKey) {
124+ // Priority out-of-date; update it
125+ U .insertOrUpdate(u, newKey)
126+ } else {
127+ U .pop()
128+ if (g(u) > rhs(u)) {
129+ // We found a better path for u
130+ setG(u, rhs(u))
131+ graph.successors(u).forEach { (succ, _) ->
132+ updateVertex(succ)
133+ }
134+ } else {
135+ val oldG = g(u)
136+ setG(u, INF )
137+ updateVertex(u)
138+ // Update successors that may have relied on old g(u)
139+ graph.successors(u).forEach { (succ, _) ->
140+ if (rhs(succ) == oldG + graph.cost(u, succ)) {
141+ updateVertex(succ)
142+ }
143+ }
144+ }
145+ }
146+ }
147+ }
148+
149+ /* *
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.
153+ */
154+ fun updateStart (newStart : FastVector ) {
155+ val oldStart = start
156+ start = newStart
157+ km + = heuristic(oldStart, start)
158+ val tmpList = mutableListOf<FastVector >()
159+ while (! U .isEmpty()) {
160+ U .top()?.let { tmpList.add(it) }
161+ U .pop()
162+ }
163+ tmpList.forEach { v ->
164+ U .insertOrUpdate(v, calculateKey(v))
165+ }
166+ }
167+
168+ /* *
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.
171+ */
172+ fun getPath (): List <FastVector > {
173+ val path = mutableListOf<FastVector >()
174+ var current = start
175+ path.add(current)
176+ while (current != goal) {
177+ val successors = graph.successors(current)
178+ if (successors.isEmpty()) break
179+ var bestNext: FastVector ? = null
180+ var bestVal = INF
181+ for ((succ, cost) in successors) {
182+ val candidate = g(succ) + cost
183+ if (candidate < bestVal) {
184+ bestVal = candidate
185+ bestNext = succ
186+ }
187+ }
188+ // No path
189+ if (bestNext == null ) break
190+ current = bestNext
191+ path.add(current)
192+ if (path.size > 100000 ) break
193+ }
194+ return path
195+ }
196+ }
0 commit comments