rust_robotics_planning/
dijkstra.rs

1#![allow(dead_code)]
2
3//! Dijkstra path planning algorithm
4//!
5//! Grid-based shortest path planning using Dijkstra's algorithm.
6
7use itertools::iproduct;
8use ordered_float::NotNan;
9use std::cmp::Reverse;
10use std::collections::{BTreeMap, BinaryHeap};
11
12use crate::grid_nalgebra;
13
14/// Returns a closure that computes adjacent cell coordinates for a given cell
15/// while excluding out-of-bound cells and the cell itself.
16fn calculate_neighbors(nrows: usize, ncols: usize) -> impl Fn(usize, usize) -> Vec<(usize, usize)> {
17    move |i, j| {
18        let delta = [-1, 0, 1];
19        iproduct!(delta, delta)
20            .filter_map(|(di, dj)| {
21                let ni = i as i32 + di;
22                let nj = j as i32 + dj;
23                if ni >= 0
24                    && ni < nrows as i32
25                    && nj >= 0
26                    && nj < ncols as i32
27                    && (ni, nj) != (i as i32, j as i32)
28                {
29                    Some((ni as usize, nj as usize))
30                } else {
31                    None
32                }
33            })
34            .collect()
35    }
36}
37
38/// Check if a cell or its neighbors contain an obstacle
39pub fn has_collision(obstacle_map: &grid_nalgebra::Map, ni: usize, nj: usize) -> bool {
40    let (height, width) = obstacle_map.shape();
41    let get_neighbors = calculate_neighbors(height, width);
42
43    let neighbors = get_neighbors(ni, nj);
44
45    if neighbors.iter().all(|&(i, j)| obstacle_map[(i, j)] != 1) && obstacle_map[(ni, nj)] != 1 {
46        return false;
47    }
48
49    true
50}
51
52/// Run Dijkstra's algorithm on a grid obstacle map.
53///
54/// Returns the shortest path from `start` to `goal` as a list of (row, col) coordinates,
55/// or `None` if no path exists.
56pub fn dijkstra_plan(
57    obstacle_map: &grid_nalgebra::Map,
58    start: (usize, usize),
59    goal: (usize, usize),
60) -> Option<Vec<(usize, usize)>> {
61    let (height, width) = obstacle_map.shape();
62    let get_neighbors = calculate_neighbors(height, width);
63
64    let mut dist = BTreeMap::new();
65    let mut prev: BTreeMap<(usize, usize), Option<(usize, usize)>> = BTreeMap::new();
66    let mut pq = BinaryHeap::new();
67
68    pq.push((Reverse(NotNan::new(0.0).unwrap()), start));
69    dist.insert(start, 0.0);
70
71    while let Some((distance, current)) = pq.pop() {
72        if current == goal {
73            // Reconstruct path
74            let mut path = vec![current];
75            while let Some(previous) = prev.get(path.last().unwrap()).unwrap_or(&None) {
76                path.push(*previous);
77                if *path.last().unwrap() == start {
78                    path.reverse();
79                    return Some(path);
80                }
81            }
82            return None;
83        }
84
85        let (ci, cj) = current;
86        let neighbors = get_neighbors(ci, cj);
87
88        for (ni, nj) in neighbors {
89            if !has_collision(obstacle_map, ni, nj) {
90                let delta_x = ci as f64 - ni as f64;
91                let delta_y = cj as f64 - nj as f64;
92                let edge_distance = (delta_x * delta_x + delta_y * delta_y).sqrt();
93                let alt = distance.0.into_inner() + edge_distance;
94
95                if *dist.get(&(ni, nj)).unwrap_or(&f64::MAX) > alt {
96                    dist.insert((ni, nj), alt);
97                    prev.insert((ni, nj), Some((ci, cj)));
98                    pq.push((Reverse(NotNan::new(alt).unwrap()), (ni, nj)));
99                }
100            }
101        }
102    }
103
104    None
105}
106
107#[cfg(test)]
108mod tests {
109    use super::*;
110
111    #[test]
112    fn test_dijkstra_simple() {
113        let matrix = nalgebra::DMatrix::from_row_slice(
114            5,
115            5,
116            &[
117                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,
118            ],
119        );
120        let map = grid_nalgebra::Map::new(matrix, 1).unwrap();
121        let result = dijkstra_plan(&map, (0, 0), (4, 4));
122        assert!(result.is_some());
123        let path = result.unwrap();
124        assert_eq!(*path.first().unwrap(), (0, 0));
125        assert_eq!(*path.last().unwrap(), (4, 4));
126    }
127
128    #[test]
129    fn test_has_collision() {
130        let matrix = nalgebra::DMatrix::from_row_slice(
131            5,
132            5,
133            &[
134                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,
135            ],
136        );
137        let map = grid_nalgebra::Map::new(matrix, 1).unwrap();
138        // Cell (2,2) is an obstacle
139        assert!(has_collision(&map, 2, 2));
140        // Cell (0,0) is far from obstacle, no collision
141        assert!(!has_collision(&map, 0, 0));
142        // Cell (1,1) is adjacent to obstacle (2,2), so it has collision
143        assert!(has_collision(&map, 1, 1));
144    }
145}
rust_robotics_planning/dijkstra.rs

rust_robotics_planning/
dijkstra.rs
