1#![allow(dead_code, clippy::legacy_numeric_constants)]
2
3use std::cmp::Ordering;
9use std::collections::{BinaryHeap, HashSet};
10
11#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)]
12struct Node {
13 x: i32,
14 y: i32,
15}
16
17impl Node {
18 pub fn new(x: i32, y: i32) -> Self {
19 Node { x, y }
20 }
21
22 fn add(&self, other: &Node) -> Node {
23 Node {
24 x: self.x + other.x,
25 y: self.y + other.y,
26 }
27 }
28
29 fn distance_to(&self, other: &Node) -> f64 {
30 let dx = (self.x - other.x) as f64;
31 let dy = (self.y - other.y) as f64;
32 (dx * dx + dy * dy).sqrt()
33 }
34}
35
36#[derive(Debug, Clone, Copy, PartialEq)]
37struct Key {
38 k1: f64,
39 k2: f64,
40}
41
42impl Eq for Key {}
43
44impl PartialOrd for Key {
45 fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
46 Some(self.cmp(other))
47 }
48}
49
50impl Ord for Key {
51 fn cmp(&self, other: &Self) -> Ordering {
52 match self.k1.partial_cmp(&other.k1) {
53 Some(Ordering::Equal) => self.k2.partial_cmp(&other.k2).unwrap_or(Ordering::Equal),
54 Some(ord) => ord,
55 None => Ordering::Equal,
56 }
57 }
58}
59
60#[derive(Debug)]
61pub struct DStarLite {
62 x_min_world: i32,
63 y_min_world: i32,
64 x_max: i32,
65 y_max: i32,
66 start: Node,
67 goal: Node,
68 km: f64,
69 rhs: nalgebra::DMatrix<f64>,
70 g: nalgebra::DMatrix<f64>,
71 u: BinaryHeap<(Key, Node)>,
72 obstacles: HashSet<(i32, i32)>,
73 detected_obstacles: HashSet<(i32, i32)>,
74 motions: [Node; 8],
75 detected_obstacles_for_plotting: Vec<(f64, f64)>,
76 initialized: bool,
77}
78
79impl DStarLite {
80 pub fn new(ox: &[i32], oy: &[i32]) -> Self {
81 let x_min_world = *ox.iter().min().unwrap_or(&0);
82 let y_min_world = *oy.iter().min().unwrap_or(&0);
83 let x_max = ox.iter().max().unwrap_or(&0) - x_min_world + 1;
84 let y_max = oy.iter().max().unwrap_or(&0) - y_min_world + 1;
85
86 let obstacles: HashSet<_> = ox
87 .iter()
88 .zip(oy.iter())
89 .map(|(&x, &y)| (x - x_min_world, y - y_min_world))
90 .collect();
91
92 let g = nalgebra::DMatrix::from_element(x_max as usize, y_max as usize, f64::INFINITY);
93 let rhs = nalgebra::DMatrix::from_element(x_max as usize, y_max as usize, f64::INFINITY);
94
95 let motions = [
96 Node::new(1, 0),
97 Node::new(0, 1),
98 Node::new(-1, 0),
99 Node::new(0, -1),
100 Node::new(1, 1),
101 Node::new(1, -1),
102 Node::new(-1, 1),
103 Node::new(-1, -1),
104 ];
105
106 DStarLite {
107 x_min_world,
108 y_min_world,
109 x_max,
110 y_max,
111 start: Node::new(0, 0),
112 goal: Node::new(0, 0),
113 km: 0.0,
114 rhs,
115 g,
116 u: BinaryHeap::new(),
117 obstacles,
118 detected_obstacles: HashSet::new(),
119 motions,
120 detected_obstacles_for_plotting: Vec::new(),
121 initialized: false,
122 }
123 }
124
125 fn is_valid(&self, node: &Node) -> bool {
126 node.x >= 0 && node.x < self.x_max && node.y >= 0 && node.y < self.y_max
127 }
128
129 fn is_obstacle(&self, node: &Node) -> bool {
130 self.obstacles.contains(&(node.x, node.y))
131 || self.detected_obstacles.contains(&(node.x, node.y))
132 }
133
134 fn get_neighbors(&self, node: &Node) -> Vec<Node> {
135 self.motions
136 .iter()
137 .map(|m| node.add(m))
138 .filter(|n| self.is_valid(n))
139 .collect()
140 }
141
142 fn calculate_key(&self, node: &Node) -> Key {
143 let min_rhs_g = self.g[(node.x as usize, node.y as usize)]
144 .min(self.rhs[(node.x as usize, node.y as usize)]);
145 let h = self.heuristic(&self.start, node);
146 Key {
147 k1: min_rhs_g + h + self.km,
148 k2: min_rhs_g,
149 }
150 }
151
152 fn heuristic(&self, a: &Node, b: &Node) -> f64 {
153 let dx = (a.x - b.x).abs() as f64;
154 let dy = (a.y - b.y).abs() as f64;
155 (dx * dx + dy * dy).sqrt()
156 }
157
158 fn c(&self, _u: &Node, v: &Node) -> f64 {
159 if self.is_obstacle(v) {
160 return f64::INFINITY;
161 }
162 self.heuristic(_u, v)
163 }
164
165 fn initialize(&mut self, start: Node, goal: Node) {
166 self.start = Node::new(start.x - self.x_min_world, start.y - self.y_min_world);
167 self.goal = Node::new(goal.x - self.x_min_world, goal.y - self.y_min_world);
168
169 if !self.initialized {
170 self.initialized = true;
171 self.u.clear();
172 self.km = 0.0;
173 self.rhs = nalgebra::DMatrix::from_element(
174 self.x_max as usize,
175 self.y_max as usize,
176 f64::INFINITY,
177 );
178 self.g = nalgebra::DMatrix::from_element(
179 self.x_max as usize,
180 self.y_max as usize,
181 f64::INFINITY,
182 );
183 self.rhs[(self.goal.x as usize, self.goal.y as usize)] = 0.0;
184 self.u.push((self.calculate_key(&self.goal), self.goal));
185 self.detected_obstacles.clear();
186 }
187 }
188
189 fn update_vertex(&mut self, u: Node) {
190 if u.x != self.goal.x || u.y != self.goal.y {
191 let mut min_rhs = f64::INFINITY;
192 for s in self.get_neighbors(&u) {
193 let cost = self.c(&u, &s) + self.g[(s.x as usize, s.y as usize)];
194 if cost < min_rhs {
195 min_rhs = cost;
196 }
197 }
198 self.rhs[(u.x as usize, u.y as usize)] = min_rhs;
199 }
200
201 self.u = std::mem::take(&mut self.u)
202 .into_iter()
203 .filter(|(_, node)| node.x != u.x || node.y != u.y)
204 .collect();
205
206 if (self.g[(u.x as usize, u.y as usize)] - self.rhs[(u.x as usize, u.y as usize)]).abs()
207 > 1e-6
208 {
209 self.u.push((self.calculate_key(&u), u));
210 }
211 }
212
213 fn compute_shortest_path(&mut self) {
214 while !self.u.is_empty() {
215 let (k_old, u) = self.u.pop().unwrap();
216
217 if k_old < self.calculate_key(&u) {
218 self.u.push((self.calculate_key(&u), u));
219 } else if self.g[(u.x as usize, u.y as usize)] > self.rhs[(u.x as usize, u.y as usize)]
220 {
221 self.g[(u.x as usize, u.y as usize)] = self.rhs[(u.x as usize, u.y as usize)];
222
223 for s in self.get_neighbors(&u) {
224 self.update_vertex(s);
225 }
226 } else {
227 self.g[(u.x as usize, u.y as usize)] = f64::INFINITY;
228 self.update_vertex(u);
229 for s in self.get_neighbors(&u) {
230 self.update_vertex(s);
231 }
232 }
233 }
234 }
235
236 fn plan(&mut self, start: Node, goal: Node) -> Option<Vec<(i32, i32)>> {
237 self.initialize(Node::new(start.x, start.y), Node::new(goal.x, goal.y));
238 self.compute_shortest_path();
239 self.reconstruct_path()
240 }
241
242 pub fn plan_xy(&mut self, sx: i32, sy: i32, gx: i32, gy: i32) -> Option<Vec<(i32, i32)>> {
244 self.plan(Node::new(sx, sy), Node::new(gx, gy))
245 }
246
247 fn reconstruct_path(&self) -> Option<Vec<(i32, i32)>> {
248 let mut path = Vec::new();
249 let mut current = self.start;
250
251 while current.x != self.goal.x || current.y != self.goal.y {
252 path.push((current.x + self.x_min_world, current.y + self.y_min_world));
253
254 let mut min_cost = f64::INFINITY;
255 let mut next_node = current;
256
257 for neighbor in self.get_neighbors(¤t) {
258 let cost = self.c(¤t, &neighbor)
259 + self.g[(neighbor.x as usize, neighbor.y as usize)];
260 if cost < min_cost {
261 min_cost = cost;
262 next_node = neighbor;
263 }
264 }
265
266 if next_node.x == current.x && next_node.y == current.y {
267 return None;
268 }
269
270 current = next_node;
271 }
272
273 path.push((
274 self.goal.x + self.x_min_world,
275 self.goal.y + self.y_min_world,
276 ));
277
278 Some(path)
279 }
280
281 pub fn add_obstacle(&mut self, x: i32, y: i32) {
282 let node = Node::new(x - self.x_min_world, y - self.y_min_world);
283 if self.is_valid(&node) {
284 self.detected_obstacles.insert((node.x, node.y));
285 self.detected_obstacles_for_plotting
286 .push((x as f64, y as f64));
287 self.update_vertex(node);
288 }
289 }
290}
291
292#[cfg(test)]
293mod tests {
294 use super::*;
295
296 #[test]
297 fn test_d_star_lite_creation() {
298 let ox: Vec<i32> = (0..10).collect();
299 let oy: Vec<i32> = vec![0; 10];
300 let dstar = DStarLite::new(&ox, &oy);
301 assert!(dstar.x_max > 0);
302 assert!(dstar.y_max > 0);
303 }
304}