1#![allow(dead_code)]
2
3use itertools::iproduct;
8use ordered_float::NotNan;
9use std::cmp::Reverse;
10use std::collections::{BTreeMap, BinaryHeap};
11
12use crate::a_star::{AStarConfig, AStarPlanner};
13use crate::grid_nalgebra;
14use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
15
16#[derive(Debug, Clone)]
18pub struct DijkstraConfig {
19 pub resolution: f64,
21 pub robot_radius: f64,
23}
24
25impl Default for DijkstraConfig {
26 fn default() -> Self {
27 Self {
28 resolution: 1.0,
29 robot_radius: 0.5,
30 }
31 }
32}
33
34pub struct DijkstraPlanner {
41 inner: AStarPlanner,
42}
43
44impl DijkstraPlanner {
45 fn a_star_config(config: &DijkstraConfig) -> AStarConfig {
46 AStarConfig {
47 resolution: config.resolution,
48 robot_radius: config.robot_radius,
49 heuristic_weight: 0.0,
50 }
51 }
52
53 pub fn try_new(ox: &[f64], oy: &[f64], config: DijkstraConfig) -> RoboticsResult<Self> {
55 Ok(Self {
56 inner: AStarPlanner::try_new(ox, oy, Self::a_star_config(&config))?,
57 })
58 }
59
60 pub fn from_obstacle_points(
62 obstacles: &Obstacles,
63 config: DijkstraConfig,
64 ) -> RoboticsResult<Self> {
65 Ok(Self {
66 inner: AStarPlanner::from_obstacle_points(obstacles, Self::a_star_config(&config))?,
67 })
68 }
69
70 pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
72 PathPlanner::plan(&self.inner, start, goal)
73 }
74
75 pub fn plan_with_observer(
80 &self,
81 start: Point2D,
82 goal: Point2D,
83 on_expand: impl FnMut(Point2D),
84 ) -> RoboticsResult<Path2D> {
85 self.inner.plan_with_observer(start, goal, on_expand)
86 }
87}
88
89impl PathPlanner for DijkstraPlanner {
90 fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
91 DijkstraPlanner::plan(self, start, goal)
92 }
93}
94
95fn calculate_neighbors(nrows: usize, ncols: usize) -> impl Fn(usize, usize) -> Vec<(usize, usize)> {
98 move |i, j| {
99 let delta = [-1, 0, 1];
100 iproduct!(delta, delta)
101 .filter_map(|(di, dj)| {
102 let ni = i as i32 + di;
103 let nj = j as i32 + dj;
104 if ni >= 0
105 && ni < nrows as i32
106 && nj >= 0
107 && nj < ncols as i32
108 && (ni, nj) != (i as i32, j as i32)
109 {
110 Some((ni as usize, nj as usize))
111 } else {
112 None
113 }
114 })
115 .collect()
116 }
117}
118
119pub fn has_collision(obstacle_map: &grid_nalgebra::Map, ni: usize, nj: usize) -> bool {
121 let (height, width) = obstacle_map.shape();
122 let get_neighbors = calculate_neighbors(height, width);
123
124 let neighbors = get_neighbors(ni, nj);
125
126 if neighbors.iter().all(|&(i, j)| obstacle_map[(i, j)] != 1) && obstacle_map[(ni, nj)] != 1 {
127 return false;
128 }
129
130 true
131}
132
133pub fn dijkstra_plan(
138 obstacle_map: &grid_nalgebra::Map,
139 start: (usize, usize),
140 goal: (usize, usize),
141) -> Option<Vec<(usize, usize)>> {
142 let (height, width) = obstacle_map.shape();
143 let get_neighbors = calculate_neighbors(height, width);
144
145 let mut dist = BTreeMap::new();
146 let mut prev: BTreeMap<(usize, usize), Option<(usize, usize)>> = BTreeMap::new();
147 let mut pq = BinaryHeap::new();
148
149 pq.push((Reverse(NotNan::new(0.0).unwrap()), start));
150 dist.insert(start, 0.0);
151
152 while let Some((distance, current)) = pq.pop() {
153 if current == goal {
154 if current == start {
155 return Some(vec![start]);
156 }
157 let mut path = vec![current];
159 while let Some(previous) = prev.get(path.last().unwrap()).unwrap_or(&None) {
160 path.push(*previous);
161 if *path.last().unwrap() == start {
162 path.reverse();
163 return Some(path);
164 }
165 }
166 return None;
167 }
168
169 let (ci, cj) = current;
170 let neighbors = get_neighbors(ci, cj);
171
172 for (ni, nj) in neighbors {
173 if !has_collision(obstacle_map, ni, nj) {
174 let delta_x = ci as f64 - ni as f64;
175 let delta_y = cj as f64 - nj as f64;
176 let edge_distance = (delta_x * delta_x + delta_y * delta_y).sqrt();
177 let alt = distance.0.into_inner() + edge_distance;
178
179 if *dist.get(&(ni, nj)).unwrap_or(&f64::MAX) > alt {
180 dist.insert((ni, nj), alt);
181 prev.insert((ni, nj), Some((ci, cj)));
182 pq.push((Reverse(NotNan::new(alt).unwrap()), (ni, nj)));
183 }
184 }
185 }
186 }
187
188 None
189}
190
191#[cfg(test)]
192mod tests {
193 use super::*;
194
195 #[test]
196 fn start_on_the_goal_is_a_one_cell_path() {
197 let matrix = nalgebra::DMatrix::from_element(3, 3, 0);
198 let map = grid_nalgebra::Map::new(matrix, 1).unwrap();
199 assert_eq!(dijkstra_plan(&map, (1, 1), (1, 1)), Some(vec![(1, 1)]));
200 }
201
202 #[test]
203 fn test_dijkstra_simple() {
204 let matrix = nalgebra::DMatrix::from_row_slice(
205 5,
206 5,
207 &[
208 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
209 ],
210 );
211 let map = grid_nalgebra::Map::new(matrix, 1).unwrap();
212 let result = dijkstra_plan(&map, (0, 0), (4, 4));
213 assert!(result.is_some());
214 let path = result.unwrap();
215 assert_eq!(*path.first().unwrap(), (0, 0));
216 assert_eq!(*path.last().unwrap(), (4, 4));
217 }
218
219 #[test]
220 fn test_has_collision() {
221 let matrix = nalgebra::DMatrix::from_row_slice(
222 5,
223 5,
224 &[
225 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
226 ],
227 );
228 let map = grid_nalgebra::Map::new(matrix, 1).unwrap();
229 assert!(has_collision(&map, 2, 2));
231 assert!(!has_collision(&map, 0, 0));
233 assert!(has_collision(&map, 1, 1));
235 }
236}