Merge commit 'd803bfe2b1fe7f5e219e50ac20d6801a0a58ac75' as 'vendor/ruvector'

vendor/ruvector/crates/ruvector-robotics/src/planning.rs

@@ -0,0 +1,402 @@
+//! Motion planning: A\* grid search and potential-field velocity commands.
+//!
+//! Operates on the [`OccupancyGrid`](crate::bridge::OccupancyGrid) type from
+//! the bridge module.  Two planners are provided:
+//!
+//! - [`astar`]: discrete A\* on the occupancy grid returning a cell path.
+//! - [`potential_field`]: continuous-space repulsive/attractive field producing
+//!   a velocity command.
+
+use crate::bridge::OccupancyGrid;
+
+use serde::{Deserialize, Serialize};
+use std::cmp::Ordering;
+use std::collections::{BinaryHeap, HashMap, HashSet};
+
+/// A 2-D grid cell coordinate.
+pub type Cell = (usize, usize);
+
+/// Result of an A\* search.
+#[derive(Debug, Clone, Serialize, Deserialize)]
+pub struct GridPath {
+    /// Sequence of `(x, y)` cells from start to goal (inclusive).
+    pub cells: Vec<Cell>,
+    /// Total traversal cost.
+    pub cost: f64,
+}
+
+/// Errors from planning operations.
+#[derive(Debug, thiserror::Error)]
+pub enum PlanningError {
+    #[error("start cell ({0}, {1}) is out of bounds or occupied")]
+    InvalidStart(usize, usize),
+    #[error("goal cell ({0}, {1}) is out of bounds or occupied")]
+    InvalidGoal(usize, usize),
+    #[error("no feasible path found")]
+    NoPath,
+}
+
+pub type Result<T> = std::result::Result<T, PlanningError>;
+
+// ---------------------------------------------------------------------------
+// A* search
+// ---------------------------------------------------------------------------
+
+/// Occupancy value above which a cell is considered blocked.
+const OCCUPIED_THRESHOLD: f32 = 0.5;
+
+#[derive(PartialEq)]
+struct AStarEntry {
+    cell: Cell,
+    f: f64,
+}
+impl Eq for AStarEntry {}
+impl Ord for AStarEntry {
+    fn cmp(&self, other: &Self) -> Ordering {
+        other.f.partial_cmp(&self.f).unwrap_or(Ordering::Equal)
+    }
+}
+impl PartialOrd for AStarEntry {
+    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+/// Run A\* on `grid`, returning the shortest [`GridPath`] from `start` to
+/// `goal`.  Cells with occupancy >= 0.5 are treated as impassable.
+///
+/// Diagonal moves cost √2, cardinal moves cost 1.
+pub fn astar(
+    grid: &OccupancyGrid,
+    start: Cell,
+    goal: Cell,
+) -> Result<GridPath> {
+    if !cell_free(grid, start) {
+        return Err(PlanningError::InvalidStart(start.0, start.1));
+    }
+    if !cell_free(grid, goal) {
+        return Err(PlanningError::InvalidGoal(goal.0, goal.1));
+    }
+    if start == goal {
+        return Ok(GridPath { cells: vec![start], cost: 0.0 });
+    }
+
+    let mut g_score: HashMap<Cell, f64> = HashMap::with_capacity(128);
+    let mut came_from: HashMap<Cell, Cell> = HashMap::with_capacity(128);
+    let mut open = BinaryHeap::new();
+    let mut closed: HashSet<Cell> = HashSet::with_capacity(128);
+    let mut neighbor_buf: Vec<(usize, usize, f64)> = Vec::with_capacity(8);
+
+    g_score.insert(start, 0.0);
+    open.push(AStarEntry { cell: start, f: heuristic(start, goal) });
+
+    while let Some(AStarEntry { cell, .. }) = open.pop() {
+        if cell == goal {
+            return Ok(reconstruct_path(&came_from, goal, &g_score));
+        }
+
+        // Skip already-expanded nodes (avoids re-expansion from stale heap entries).
+        if !closed.insert(cell) {
+            continue;
+        }
+
+        let current_g = g_score[&cell];
+
+        neighbors_into(grid, cell, &mut neighbor_buf);
+        for &(nx, ny, step_cost) in &neighbor_buf {
+            let neighbor = (nx, ny);
+            if closed.contains(&neighbor) {
+                continue;
+            }
+            let tentative_g = current_g + step_cost;
+            if tentative_g < *g_score.get(&neighbor).unwrap_or(&f64::INFINITY) {
+                g_score.insert(neighbor, tentative_g);
+                came_from.insert(neighbor, cell);
+                open.push(AStarEntry {
+                    cell: neighbor,
+                    f: tentative_g + heuristic(neighbor, goal),
+                });
+            }
+        }
+    }
+
+    Err(PlanningError::NoPath)
+}
+
+#[inline]
+fn cell_free(grid: &OccupancyGrid, (x, y): Cell) -> bool {
+    grid.get(x, y).is_some_and(|v| v < OCCUPIED_THRESHOLD)
+}
+
+#[inline]
+fn heuristic(a: Cell, b: Cell) -> f64 {
+    let dx = (a.0 as f64 - b.0 as f64).abs();
+    let dy = (a.1 as f64 - b.1 as f64).abs();
+    // Octile distance.
+    let (min, max) = if dx < dy { (dx, dy) } else { (dy, dx) };
+    min * std::f64::consts::SQRT_2 + (max - min)
+}
+
+/// Write neighbours of `cell` into `out`, reusing the buffer to avoid
+/// per-expansion heap allocation.
+#[inline]
+fn neighbors_into(grid: &OccupancyGrid, (cx, cy): Cell, out: &mut Vec<(usize, usize, f64)>) {
+    out.clear();
+    for dx in [-1_i64, 0, 1] {
+        for dy in [-1_i64, 0, 1] {
+            if dx == 0 && dy == 0 {
+                continue;
+            }
+            let nx = cx as i64 + dx;
+            let ny = cy as i64 + dy;
+            if nx < 0 || ny < 0 {
+                continue;
+            }
+            let (nx, ny) = (nx as usize, ny as usize);
+            if cell_free(grid, (nx, ny)) {
+                let cost = if dx != 0 && dy != 0 {
+                    std::f64::consts::SQRT_2
+                } else {
+                    1.0
+                };
+                out.push((nx, ny, cost));
+            }
+        }
+    }
+}
+
+fn reconstruct_path(
+    came_from: &HashMap<Cell, Cell>,
+    goal: Cell,
+    g_score: &HashMap<Cell, f64>,
+) -> GridPath {
+    let mut cells = vec![goal];
+    let mut current = goal;
+    while let Some(&prev) = came_from.get(&current) {
+        cells.push(prev);
+        current = prev;
+    }
+    cells.reverse();
+    let cost = g_score.get(&goal).copied().unwrap_or(0.0);
+    GridPath { cells, cost }
+}
+
+// ---------------------------------------------------------------------------
+// Potential field
+// ---------------------------------------------------------------------------
+
+/// Output of the potential field planner: a 3-D velocity command.
+#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
+pub struct VelocityCommand {
+    pub vx: f64,
+    pub vy: f64,
+    pub vz: f64,
+}
+
+/// Configuration for the potential field planner.
+#[derive(Debug, Clone, Serialize, Deserialize)]
+pub struct PotentialFieldConfig {
+    /// Attractive gain toward the goal.
+    pub attractive_gain: f64,
+    /// Repulsive gain away from obstacles.
+    pub repulsive_gain: f64,
+    /// Influence range for obstacles (metres).
+    pub obstacle_influence: f64,
+    /// Maximum output speed (m/s).
+    pub max_speed: f64,
+}
+
+impl Default for PotentialFieldConfig {
+    fn default() -> Self {
+        Self {
+            attractive_gain: 1.0,
+            repulsive_gain: 100.0,
+            obstacle_influence: 3.0,
+            max_speed: 2.0,
+        }
+    }
+}
+
+/// Compute a velocity command using attractive + repulsive potential fields.
+///
+/// * `robot` — current robot position `[x, y, z]`.
+/// * `goal` — target position `[x, y, z]`.
+/// * `obstacles` — positions of nearby obstacles.
+pub fn potential_field(
+    robot: &[f64; 3],
+    goal: &[f64; 3],
+    obstacles: &[[f64; 3]],
+    config: &PotentialFieldConfig,
+) -> VelocityCommand {
+    // Attractive force: linear pull toward goal.
+    let mut fx = config.attractive_gain * (goal[0] - robot[0]);
+    let mut fy = config.attractive_gain * (goal[1] - robot[1]);
+    let mut fz = config.attractive_gain * (goal[2] - robot[2]);
+
+    // Repulsive force: push away from each obstacle within influence range.
+    for obs in obstacles {
+        let dx = robot[0] - obs[0];
+        let dy = robot[1] - obs[1];
+        let dz = robot[2] - obs[2];
+        let dist = (dx * dx + dy * dy + dz * dz).sqrt().max(0.01);
+
+        if dist < config.obstacle_influence {
+            let strength =
+                config.repulsive_gain * (1.0 / dist - 1.0 / config.obstacle_influence) / (dist * dist);
+            fx += strength * dx / dist;
+            fy += strength * dy / dist;
+            fz += strength * dz / dist;
+        }
+    }
+
+    // Clamp to max speed.
+    let speed = (fx * fx + fy * fy + fz * fz).sqrt();
+    if speed > config.max_speed {
+        let s = config.max_speed / speed;
+        fx *= s;
+        fy *= s;
+        fz *= s;
+    }
+
+    VelocityCommand { vx: fx, vy: fy, vz: fz }
+}
+
+// ---------------------------------------------------------------------------
+// Path smoothing
+// ---------------------------------------------------------------------------
+
+/// Convert a [`GridPath`] (grid cells) to world-space waypoints using the
+/// grid resolution and origin.
+pub fn path_to_waypoints(path: &GridPath, resolution: f64, origin: &[f64; 3]) -> Vec<[f64; 3]> {
+    path.cells
+        .iter()
+        .map(|&(x, y)| {
+            [
+                origin[0] + x as f64 * resolution,
+                origin[1] + y as f64 * resolution,
+                origin[2],
+            ]
+        })
+        .collect()
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn free_grid(w: usize, h: usize) -> OccupancyGrid {
+        OccupancyGrid::new(w, h, 1.0)
+    }
+
+    #[test]
+    fn test_astar_straight_line() {
+        let grid = free_grid(10, 10);
+        let path = astar(&grid, (0, 0), (5, 0)).unwrap();
+        assert_eq!(*path.cells.first().unwrap(), (0, 0));
+        assert_eq!(*path.cells.last().unwrap(), (5, 0));
+        assert!((path.cost - 5.0).abs() < 1e-6);
+    }
+
+    #[test]
+    fn test_astar_diagonal() {
+        let grid = free_grid(10, 10);
+        let path = astar(&grid, (0, 0), (3, 3)).unwrap();
+        assert_eq!(*path.cells.last().unwrap(), (3, 3));
+        // Pure diagonal = 3 * sqrt(2) ≈ 4.24
+        assert!((path.cost - 3.0 * std::f64::consts::SQRT_2).abs() < 1e-6);
+    }
+
+    #[test]
+    fn test_astar_same_cell() {
+        let grid = free_grid(5, 5);
+        let path = astar(&grid, (2, 2), (2, 2)).unwrap();
+        assert_eq!(path.cells.len(), 1);
+        assert!((path.cost).abs() < 1e-9);
+    }
+
+    #[test]
+    fn test_astar_around_wall() {
+        let mut grid = free_grid(10, 10);
+        // Vertical wall at x=3 from y=0 to y=4.
+        for y in 0..5 {
+            grid.set(3, y, 1.0);
+        }
+        let path = astar(&grid, (1, 2), (5, 2)).unwrap();
+        assert_eq!(*path.cells.last().unwrap(), (5, 2));
+        // Path must go around the wall, so cost > 4 (straight line).
+        assert!(path.cost > 4.0);
+    }
+
+    #[test]
+    fn test_astar_blocked() {
+        let mut grid = free_grid(5, 5);
+        // Full wall across the grid.
+        for y in 0..5 {
+            grid.set(2, y, 1.0);
+        }
+        let result = astar(&grid, (0, 2), (4, 2));
+        assert!(result.is_err());
+    }
+
+    #[test]
+    fn test_astar_invalid_start() {
+        let grid = free_grid(5, 5);
+        let result = astar(&grid, (10, 10), (2, 2));
+        assert!(result.is_err());
+    }
+
+    #[test]
+    fn test_potential_field_towards_goal() {
+        let cmd = potential_field(
+            &[0.0, 0.0, 0.0],
+            &[5.0, 0.0, 0.0],
+            &[],
+            &PotentialFieldConfig::default(),
+        );
+        assert!(cmd.vx > 0.0);
+        assert!(cmd.vy.abs() < 1e-9);
+    }
+
+    #[test]
+    fn test_potential_field_obstacle_repulsion() {
+        let cmd = potential_field(
+            &[0.0, 0.0, 0.0],
+            &[5.0, 0.0, 0.0],
+            &[[1.0, 0.0, 0.0]],
+            &PotentialFieldConfig::default(),
+        );
+        // Obstacle directly ahead — repulsion should reduce forward velocity.
+        let cmd_no_obs = potential_field(
+            &[0.0, 0.0, 0.0],
+            &[5.0, 0.0, 0.0],
+            &[],
+            &PotentialFieldConfig::default(),
+        );
+        assert!(cmd.vx < cmd_no_obs.vx);
+    }
+
+    #[test]
+    fn test_potential_field_max_speed() {
+        let config = PotentialFieldConfig { max_speed: 1.0, ..Default::default() };
+        let cmd = potential_field(
+            &[0.0, 0.0, 0.0],
+            &[100.0, 100.0, 0.0],
+            &[],
+            &config,
+        );
+        let speed = (cmd.vx * cmd.vx + cmd.vy * cmd.vy + cmd.vz * cmd.vz).sqrt();
+        assert!((speed - 1.0).abs() < 1e-9);
+    }
+
+    #[test]
+    fn test_path_to_waypoints() {
+        let path = GridPath {
+            cells: vec![(0, 0), (1, 0), (2, 0)],
+            cost: 2.0,
+        };
+        let wps = path_to_waypoints(&path, 0.5, &[0.0, 0.0, 0.0]);
+        assert_eq!(wps.len(), 3);
+        assert!((wps[1][0] - 0.5).abs() < 1e-9);
+        assert!((wps[2][0] - 1.0).abs() < 1e-9);
+    }
+}