mecha10-planning 0.6.3

//! RRT (Rapidly-exploring Random Tree) path planning algorithm
//!
//! This module implements the RRT algorithm for sampling-based planning in complex spaces.

use crate::types::{Obstacle, Path, Point2D};
use rand::{Rng, RngExt};

/// RRT path planner
#[derive(Debug, Clone)]
pub struct RRTPlanner {
    start: Point2D,
    goal: Point2D,
    max_iterations: usize,
    step_size: f32,
    goal_sample_rate: f32,
    obstacles: Vec<Obstacle>,
    bounds: (Point2D, Point2D),
}

impl RRTPlanner {
    /// Create a new RRT planner
    ///
    /// # Arguments
    ///
    /// * `start` - Starting position
    /// * `goal` - Goal position
    /// * `step_size` - Maximum distance to extend tree in one iteration
    pub fn new(start: impl Into<Point2D>, goal: impl Into<Point2D>, step_size: f32) -> Self {
        Self {
            start: start.into(),
            goal: goal.into(),
            max_iterations: 1000,
            step_size,
            goal_sample_rate: 0.1,
            obstacles: Vec::new(),
            bounds: (Point2D::new(-100.0, -100.0), Point2D::new(100.0, 100.0)),
        }
    }

    /// Set the maximum number of iterations
    pub fn with_max_iterations(mut self, max_iterations: usize) -> Self {
        self.max_iterations = max_iterations;
        self
    }

    // duplicate: false positive — matches `TracingConfig::with_sampling_rate` in
    // packages/core/src/tracing_otel.rs on AST shape only (clamp a float into `[0, 1]`, store,
    // return self); unrelated domains (path-planning goal bias vs. tracing sampling), and
    // `builder_setter!`'s `assign`/`into`/`into_some` shapes don't cover a clamp-before-store,
    // so a shared macro arm would need a one-off `clamp` variant used nowhere else in the
    // workspace — not worth adding for a single call site.
    /// Set the goal sample rate (probability of sampling goal vs random point)
    pub fn with_goal_sample_rate(mut self, rate: f32) -> Self {
        self.goal_sample_rate = rate.clamp(0.0, 1.0);
        self
    }

    /// Set the search space bounds
    pub fn with_bounds(mut self, min: Point2D, max: Point2D) -> Self {
        self.bounds = (min, max);
        self
    }

    /// Add an obstacle to the environment
    pub fn add_obstacle(&mut self, obstacle: Obstacle) {
        self.obstacles.push(obstacle);
    }

    /// Plan a path from start to goal
    pub fn plan(&self) -> Option<Path> {
        let mut rng = rand::rng();
        let mut tree = vec![RRTNode {
            point: self.start,
            parent: None,
        }];

        for _ in 0..self.max_iterations {
            let sample = if rng.random::<f32>() < self.goal_sample_rate {
                self.goal
            } else {
                self.random_point(&mut rng)
            };

            let nearest_idx = self.nearest_node(&tree, &sample);
            let nearest = &tree[nearest_idx];

            let new_point = self.steer(&nearest.point, &sample);

            if !self.is_path_clear(&nearest.point, &new_point) {
                continue;
            }

            tree.push(RRTNode {
                point: new_point,
                parent: Some(nearest_idx),
            });

            if new_point.distance_to(&self.goal) < self.step_size {
                return Some(self.extract_path(&tree));
            }
        }

        None
    }

    fn random_point(&self, rng: &mut impl Rng) -> Point2D {
        Point2D::new(
            rng.random_range(self.bounds.0.x..=self.bounds.1.x),
            rng.random_range(self.bounds.0.y..=self.bounds.1.y),
        )
    }

    fn nearest_node(&self, tree: &[RRTNode], point: &Point2D) -> usize {
        tree.iter()
            .enumerate()
            .min_by(|(_, a), (_, b)| {
                let dist_a = a.point.distance_to(point);
                let dist_b = b.point.distance_to(point);
                dist_a.partial_cmp(&dist_b).unwrap()
            })
            .map(|(idx, _)| idx)
            .unwrap()
    }

    fn steer(&self, from: &Point2D, to: &Point2D) -> Point2D {
        let distance = from.distance_to(to);
        if distance <= self.step_size {
            return *to;
        }

        let ratio = self.step_size / distance;
        Point2D::new(from.x + (to.x - from.x) * ratio, from.y + (to.y - from.y) * ratio)
    }

    fn is_path_clear(&self, from: &Point2D, to: &Point2D) -> bool {
        !self.obstacles.iter().any(|obs| obs.intersects_segment(from, to))
    }

    fn extract_path(&self, tree: &[RRTNode]) -> Path {
        let mut waypoints = Vec::new();
        let mut current_idx = tree.len() - 1;

        waypoints.push(tree[current_idx].point);

        while let Some(parent_idx) = tree[current_idx].parent {
            waypoints.push(tree[parent_idx].point);
            current_idx = parent_idx;
        }

        waypoints.reverse();
        Path::new(waypoints)
    }
}

#[derive(Debug, Clone)]
struct RRTNode {
    point: Point2D,
    parent: Option<usize>,
}