rust_robotics_planning/

d_star.rs

#![allow(dead_code, clippy::legacy_numeric_constants)]

//! D\* (original) path planning algorithm
//!
//! The original D\* algorithm by Anthony Stentz (1994) for optimal path planning
//! with dynamic obstacle detection. Unlike D\* Lite, this uses the classic OPEN list
//! with NEW/OPEN/CLOSED state tags.
//!
//! Reference: <https://en.wikipedia.org/wiki/D*>

use std::collections::HashSet;

/// Tag describing the state of a cell in the D\* search.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum Tag {
    New,
    Open,
    Closed,
}

/// Whether a cell is free or an obstacle.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum CellKind {
    Free,
    Obstacle,
}

/// Internal cell state for the D\* algorithm.
#[derive(Debug, Clone)]
struct Cell {
    tag: Tag,
    kind: CellKind,
    /// Cost-to-goal estimate (h value in Stentz's paper).
    h: f64,
    /// Key value used for priority ordering.
    k: f64,
    /// Parent cell index (row * cols + col), or `usize::MAX` if none.
    parent: usize,
}

/// Grid map used by the D\* planner.
#[derive(Debug)]
struct Map {
    rows: usize,
    cols: usize,
    cells: Vec<Cell>,
}

impl Map {
    fn new(rows: usize, cols: usize) -> Self {
        let cells = vec![
            Cell {
                tag: Tag::New,
                kind: CellKind::Free,
                h: 0.0,
                k: 0.0,
                parent: usize::MAX,
            };
            rows * cols
        ];
        Map { rows, cols, cells }
    }

    #[inline]
    fn idx(&self, r: usize, c: usize) -> usize {
        r * self.cols + c
    }

    #[inline]
    fn pos(&self, idx: usize) -> (usize, usize) {
        (idx / self.cols, idx % self.cols)
    }

    fn neighbors(&self, idx: usize) -> Vec<usize> {
        let (r, c) = self.pos(idx);
        let mut result = Vec::with_capacity(8);
        for dr in [-1i32, 0, 1] {
            for dc in [-1i32, 0, 1] {
                if dr == 0 && dc == 0 {
                    continue;
                }
                let nr = r as i32 + dr;
                let nc = c as i32 + dc;
                if nr >= 0 && (nr as usize) < self.rows && nc >= 0 && (nc as usize) < self.cols {
                    result.push(self.idx(nr as usize, nc as usize));
                }
            }
        }
        result
    }

    /// Movement cost between two adjacent cells. Returns `f64::MAX` if either is an obstacle.
    fn cost(&self, a: usize, b: usize) -> f64 {
        if self.cells[a].kind == CellKind::Obstacle || self.cells[b].kind == CellKind::Obstacle {
            return f64::MAX;
        }
        let (ar, ac) = self.pos(a);
        let (br, bc) = self.pos(b);
        let dr = (ar as f64) - (br as f64);
        let dc = (ac as f64) - (bc as f64);
        (dr * dr + dc * dc).sqrt()
    }

    fn set_obstacle(&mut self, r: usize, c: usize) {
        if r < self.rows && c < self.cols {
            let idx = self.idx(r, c);
            self.cells[idx].kind = CellKind::Obstacle;
        }
    }
}

/// D\* (original) path planner.
///
/// Constructs a grid from obstacle coordinates, plans a shortest path from
/// start to goal, and supports dynamic re-planning when new obstacles are
/// detected along the current path.
#[derive(Debug)]
pub struct DStar {
    map: Map,
    open_list: HashSet<usize>,
    x_min_world: i32,
    y_min_world: i32,
}

impl DStar {
    /// Create a new D\* planner from obstacle boundary coordinates.
    ///
    /// `ox` and `oy` define obstacle (or boundary) positions in world coordinates.
    pub fn new(ox: &[i32], oy: &[i32]) -> Self {
        let x_min = *ox.iter().min().unwrap_or(&0);
        let y_min = *oy.iter().min().unwrap_or(&0);
        let x_max = *ox.iter().max().unwrap_or(&0);
        let y_max = *oy.iter().max().unwrap_or(&0);

        let rows = (x_max - x_min + 1) as usize;
        let cols = (y_max - y_min + 1) as usize;

        let mut map = Map::new(rows, cols);

        for (&x, &y) in ox.iter().zip(oy.iter()) {
            let r = (x - x_min) as usize;
            let c = (y - y_min) as usize;
            map.set_obstacle(r, c);
        }

        DStar {
            map,
            open_list: HashSet::new(),
            x_min_world: x_min,
            y_min_world: y_min,
        }
    }

    /// Return the cell index with the minimum k value in the open list, or `None`.
    fn min_state(&self) -> Option<usize> {
        self.open_list.iter().copied().min_by(|&a, &b| {
            self.map.cells[a]
                .k
                .partial_cmp(&self.map.cells[b].k)
                .unwrap_or(std::cmp::Ordering::Equal)
        })
    }

    /// Return the minimum k value in the open list, or `-1.0` if empty.
    fn get_kmin(&self) -> f64 {
        self.open_list
            .iter()
            .map(|&i| self.map.cells[i].k)
            .fold(f64::INFINITY, f64::min)
            .min(f64::INFINITY)
            * if self.open_list.is_empty() { 0.0 } else { 1.0 }
            + if self.open_list.is_empty() { -1.0 } else { 0.0 }
    }

    /// Insert a state into the open list with a new h value.
    fn insert(&mut self, idx: usize, h_new: f64) {
        let cell = &mut self.map.cells[idx];
        match cell.tag {
            Tag::New => cell.k = h_new,
            Tag::Open => cell.k = cell.k.min(h_new),
            Tag::Closed => cell.k = cell.h.min(h_new),
        }
        cell.h = h_new;
        cell.tag = Tag::Open;
        self.open_list.insert(idx);
    }

    /// Remove a state from the open list (mark as CLOSED).
    fn remove(&mut self, idx: usize) {
        if self.map.cells[idx].tag == Tag::Open {
            self.map.cells[idx].tag = Tag::Closed;
        }
        self.open_list.remove(&idx);
    }

    /// Core D\* state processing step. Returns the minimum k value after processing,
    /// or `-1.0` if the open list is empty.
    fn process_state(&mut self) -> f64 {
        let x = match self.min_state() {
            Some(idx) => idx,
            None => return -1.0,
        };

        let k_old = self.get_kmin();
        self.remove(x);

        let neighbors = self.map.neighbors(x);
        let x_h = self.map.cells[x].h;

        if k_old < x_h {
            // RAISE state
            for &y in &neighbors {
                let y_h = self.map.cells[y].h;
                let cost_xy = self.map.cost(x, y);
                if y_h <= k_old && x_h > y_h + cost_xy {
                    self.map.cells[x].parent = y;
                    self.map.cells[x].h = y_h + cost_xy;
                }
            }
        }

        // Re-read x_h after potential update above.
        let x_h = self.map.cells[x].h;

        if (k_old - x_h).abs() < 1e-10 {
            // LOWER state
            for &y in &neighbors {
                let y_tag = self.map.cells[y].tag;
                let y_h = self.map.cells[y].h;
                let y_parent = self.map.cells[y].parent;
                let cost_xy = self.map.cost(x, y);
                if y_tag == Tag::New
                    || (y_parent == x && (y_h - (x_h + cost_xy)).abs() > 1e-10)
                    || (y_parent != x && y_h > x_h + cost_xy)
                {
                    self.map.cells[y].parent = x;
                    self.insert(y, x_h + cost_xy);
                }
            }
        } else {
            // RAISE state (propagation)
            for &y in &neighbors {
                let y_tag = self.map.cells[y].tag;
                let y_h = self.map.cells[y].h;
                let y_parent = self.map.cells[y].parent;
                let cost_xy = self.map.cost(x, y);
                if y_tag == Tag::New || (y_parent == x && (y_h - (x_h + cost_xy)).abs() > 1e-10) {
                    self.map.cells[y].parent = x;
                    self.insert(y, x_h + cost_xy);
                } else if y_parent != x && y_h > x_h + cost_xy {
                    self.insert(x, x_h);
                } else if y_parent != x
                    && x_h > y_h + cost_xy
                    && y_tag == Tag::Closed
                    && y_h > k_old
                {
                    self.insert(y, y_h);
                }
            }
        }

        self.get_kmin()
    }

    /// Trigger re-planning from a cell whose parent became an obstacle.
    ///
    /// If the current parent is an obstacle, find the best non-obstacle
    /// neighbor as a new parent before re-inserting into the open list.
    fn modify_cost(&mut self, idx: usize) {
        if self.map.cells[idx].tag == Tag::Closed {
            let parent = self.map.cells[idx].parent;
            let cost = self.map.cost(idx, parent);
            if cost < f64::MAX {
                // Parent is still valid
                let parent_h = self.map.cells[parent].h;
                self.insert(idx, parent_h + cost);
            } else {
                // Parent is an obstacle — find the best alternative neighbor
                let neighbors = self.map.neighbors(idx);
                let mut best_h = f64::MAX;
                let mut best_neighbor = usize::MAX;
                for &n in &neighbors {
                    if self.map.cells[n].kind == CellKind::Obstacle {
                        continue;
                    }
                    let c = self.map.cost(idx, n);
                    let candidate = self.map.cells[n].h + c;
                    if candidate < best_h {
                        best_h = candidate;
                        best_neighbor = n;
                    }
                }
                if best_neighbor != usize::MAX {
                    self.map.cells[idx].parent = best_neighbor;
                    self.insert(idx, best_h);
                } else {
                    // Completely surrounded — insert with MAX to propagate RAISE
                    self.insert(idx, f64::MAX);
                }
            }
        }
    }

    fn modify(&mut self, idx: usize) {
        self.modify_cost(idx);
        loop {
            let k_min = self.process_state();
            if k_min >= self.map.cells[idx].h || k_min < 0.0 {
                break;
            }
        }
    }

    /// Plan a path from `(sx, sy)` to `(gx, gy)` in world coordinates.
    ///
    /// Returns a vector of `(x, y)` world-coordinate waypoints if a path exists.
    pub fn plan_xy(&mut self, sx: i32, sy: i32, gx: i32, gy: i32) -> Option<Vec<(i32, i32)>> {
        let start_r = (sx - self.x_min_world) as usize;
        let start_c = (sy - self.y_min_world) as usize;
        let goal_r = (gx - self.x_min_world) as usize;
        let goal_c = (gy - self.y_min_world) as usize;

        if start_r >= self.map.rows
            || start_c >= self.map.cols
            || goal_r >= self.map.rows
            || goal_c >= self.map.cols
        {
            return None;
        }

        let start = self.map.idx(start_r, start_c);
        let goal = self.map.idx(goal_r, goal_c);

        // Initial search: insert goal with h=0, expand until start is CLOSED.
        self.insert(goal, 0.0);

        loop {
            let k = self.process_state();
            if self.map.cells[start].tag == Tag::Closed || k < 0.0 {
                break;
            }
        }

        if self.map.cells[start].tag != Tag::Closed || !self.map.cells[start].h.is_finite() {
            return None; // No path found
        }

        // Trace the path following parent pointers.
        self.extract_path(start, goal)
    }

    /// Plan a path, then dynamically add obstacles and re-plan.
    ///
    /// `new_obstacles` are `(x, y)` world-coordinate pairs of obstacles discovered
    /// after the initial plan. The planner re-routes around them.
    pub fn plan_with_new_obstacles(
        &mut self,
        sx: i32,
        sy: i32,
        gx: i32,
        gy: i32,
        new_obstacles: &[(i32, i32)],
    ) -> Option<Vec<(i32, i32)>> {
        let start_r = (sx - self.x_min_world) as usize;
        let start_c = (sy - self.y_min_world) as usize;
        let goal_r = (gx - self.x_min_world) as usize;
        let goal_c = (gy - self.y_min_world) as usize;

        if start_r >= self.map.rows
            || start_c >= self.map.cols
            || goal_r >= self.map.rows
            || goal_c >= self.map.cols
        {
            return None;
        }

        let start = self.map.idx(start_r, start_c);
        let goal = self.map.idx(goal_r, goal_c);

        // Initial search
        self.insert(goal, 0.0);
        loop {
            let k = self.process_state();
            if self.map.cells[start].tag == Tag::Closed || k < 0.0 {
                break;
            }
        }

        if self.map.cells[start].tag != Tag::Closed || !self.map.cells[start].h.is_finite() {
            return None;
        }

        // Add new obstacles
        for &(ox, oy) in new_obstacles {
            let r = (ox - self.x_min_world) as usize;
            let c = (oy - self.y_min_world) as usize;
            if r < self.map.rows && c < self.map.cols {
                self.map.set_obstacle(r, c);
            }
        }

        // Walk the path; when hitting an obstacle, re-plan from that cell.
        let mut path = Vec::new();
        let mut current = start;

        let max_steps = self.map.rows * self.map.cols;
        let mut steps = 0;

        while current != goal {
            if steps > max_steps {
                return None; // prevent infinite loop
            }
            steps += 1;

            let (r, c) = self.map.pos(current);
            path.push((r as i32 + self.x_min_world, c as i32 + self.y_min_world));

            let parent = self.map.cells[current].parent;
            if parent == usize::MAX {
                return None;
            }

            if self.map.cells[parent].kind == CellKind::Obstacle {
                // Parent is blocked, trigger re-plan
                self.modify(current);
                // After re-plan, continue from current
                continue;
            }

            current = parent;
        }

        let (r, c) = self.map.pos(goal);
        path.push((r as i32 + self.x_min_world, c as i32 + self.y_min_world));

        Some(path)
    }

    /// Add a single obstacle at world coordinates and trigger local re-planning.
    pub fn add_obstacle(&mut self, x: i32, y: i32) {
        let r = (x - self.x_min_world) as usize;
        let c = (y - self.y_min_world) as usize;
        if r < self.map.rows && c < self.map.cols {
            let idx = self.map.idx(r, c);
            self.map.set_obstacle(r, c);
            // Notify affected neighbors
            let neighbors = self.map.neighbors(idx);
            for n in neighbors {
                if self.map.cells[n].parent == idx && self.map.cells[n].tag == Tag::Closed {
                    self.modify(n);
                }
            }
        }
    }

    /// Extract path by following parent pointers from start to goal.
    fn extract_path(&self, start: usize, goal: usize) -> Option<Vec<(i32, i32)>> {
        let mut path = Vec::new();
        let mut current = start;
        let max_steps = self.map.rows * self.map.cols;

        for _ in 0..max_steps {
            let (r, c) = self.map.pos(current);
            path.push((r as i32 + self.x_min_world, c as i32 + self.y_min_world));

            if current == goal {
                return Some(path);
            }

            let parent = self.map.cells[current].parent;
            if parent == usize::MAX || parent == current {
                return None;
            }
            current = parent;
        }

        None
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    /// Build a simple box boundary: walls at x=-1,11 and y=-1,11.
    fn make_box_obstacles() -> (Vec<i32>, Vec<i32>) {
        let mut ox = Vec::new();
        let mut oy = Vec::new();
        for i in -1..=11 {
            ox.push(i);
            oy.push(-1); // bottom wall
            ox.push(i);
            oy.push(11); // top wall
            ox.push(-1);
            oy.push(i); // left wall
            ox.push(11);
            oy.push(i); // right wall
        }
        (ox, oy)
    }

    #[test]
    fn test_dstar_creation() {
        let (ox, oy) = make_box_obstacles();
        let dstar = DStar::new(&ox, &oy);
        assert!(dstar.map.rows > 0);
        assert!(dstar.map.cols > 0);
    }

    #[test]
    fn test_dstar_simple_path() {
        let (ox, oy) = make_box_obstacles();
        let mut dstar = DStar::new(&ox, &oy);
        let path = dstar.plan_xy(0, 0, 5, 5);
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(path.first(), Some(&(0, 0)));
        assert_eq!(path.last(), Some(&(5, 5)));
        assert!(path.len() >= 2);
    }

    #[test]
    fn test_dstar_start_equals_goal() {
        let (ox, oy) = make_box_obstacles();
        let mut dstar = DStar::new(&ox, &oy);
        let path = dstar.plan_xy(3, 3, 3, 3);
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(path.len(), 1);
        assert_eq!(path[0], (3, 3));
    }

    #[test]
    fn test_dstar_path_avoids_obstacle() {
        let (mut ox, mut oy) = make_box_obstacles();
        // Add a vertical wall at x=5 from y=0 to y=8
        for y in 0..=8 {
            ox.push(5);
            oy.push(y);
        }
        let mut dstar = DStar::new(&ox, &oy);
        let path = dstar.plan_xy(2, 2, 8, 2);
        assert!(path.is_some());
        let path = path.unwrap();
        // Path must not pass through the wall
        for &(px, py) in &path {
            if px == 5 {
                assert!(py > 8, "path should go around the wall");
            }
        }
        assert_eq!(path.first(), Some(&(2, 2)));
        assert_eq!(path.last(), Some(&(8, 2)));
    }

    #[test]
    fn test_dstar_no_path_blocked() {
        // Start == goal but surrounded by obstacles should still find trivial path,
        // so instead test truly disconnected components:
        // place start and goal on opposite sides of a complete wall
        let mut ox = Vec::new();
        let mut oy = Vec::new();
        // Outer boundary 10x10
        for i in -1..=10 {
            ox.push(i);
            oy.push(-1);
            ox.push(i);
            oy.push(10);
            ox.push(-1);
            oy.push(i);
            ox.push(10);
            oy.push(i);
        }
        // Complete wall at x=5 from y=-1 to y=10
        for j in -1..=10 {
            ox.push(5);
            oy.push(j);
        }
        let mut dstar = DStar::new(&ox, &oy);
        let path = dstar.plan_xy(2, 5, 8, 5);
        // Start and goal are on opposite sides of a complete wall
        assert!(
            path.is_none(),
            "goal should be unreachable through complete wall"
        );
    }

    #[test]
    fn test_dstar_with_new_obstacles() {
        let (ox, oy) = make_box_obstacles();
        let mut dstar = DStar::new(&ox, &oy);

        // New obstacles: partial wall at x=5 from y=0 to y=6, leaving gap at y=7..9
        let new_obs: Vec<(i32, i32)> = (0..=6).map(|i| (5, i)).collect();

        let path = dstar.plan_with_new_obstacles(2, 5, 8, 5, &new_obs);
        assert!(path.is_some(), "should find path through gap above wall");
        let path = path.unwrap();
        assert_eq!(path.first(), Some(&(2, 5)));
        assert_eq!(path.last(), Some(&(8, 5)));
    }

    #[test]
    fn test_dstar_out_of_bounds() {
        let (ox, oy) = make_box_obstacles();
        let mut dstar = DStar::new(&ox, &oy);
        let path = dstar.plan_xy(-100, -100, 200, 200);
        assert!(path.is_none());
    }

    #[test]
    fn test_dstar_adjacent_cells() {
        let (ox, oy) = make_box_obstacles();
        let mut dstar = DStar::new(&ox, &oy);
        let path = dstar.plan_xy(3, 3, 4, 4);
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(path.len(), 2); // direct diagonal neighbor
    }

    #[test]
    fn test_dstar_straight_line() {
        let (ox, oy) = make_box_obstacles();
        let mut dstar = DStar::new(&ox, &oy);
        let path = dstar.plan_xy(1, 1, 1, 9);
        assert!(path.is_some());
        let path = path.unwrap();
        assert_eq!(path.first(), Some(&(1, 1)));
        assert_eq!(path.last(), Some(&(1, 9)));
        // All waypoints should have x=1 (straight line)
        for &(px, _) in &path {
            assert_eq!(px, 1);
        }
    }

    #[test]
    fn test_map_neighbors() {
        let map = Map::new(5, 5);
        // Corner cell (0,0) should have 3 neighbors
        let n = map.neighbors(map.idx(0, 0));
        assert_eq!(n.len(), 3);
        // Edge cell (0,2) should have 5 neighbors
        let n = map.neighbors(map.idx(0, 2));
        assert_eq!(n.len(), 5);
        // Center cell (2,2) should have 8 neighbors
        let n = map.neighbors(map.idx(2, 2));
        assert_eq!(n.len(), 8);
    }

    #[test]
    fn test_map_cost_with_obstacle() {
        let mut map = Map::new(5, 5);
        let a = map.idx(1, 1);
        let b = map.idx(1, 2);
        // Normal cost
        let cost = map.cost(a, b);
        assert!((cost - 1.0).abs() < 1e-10);
        // After making b an obstacle
        map.set_obstacle(1, 2);
        let cost = map.cost(a, b);
        assert_eq!(cost, f64::MAX);
    }
}