mecha10-planning 0.6.3

//! Common types for path planning

use serde::{Deserialize, Serialize};

/// A 2D point in space
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Point2D {
    pub x: f32,
    pub y: f32,
}

impl Point2D {
    /// Create a new point
    pub fn new(x: f32, y: f32) -> Self {
        Self { x, y }
    }

    /// Calculate Euclidean distance to another point
    pub fn distance_to(&self, other: &Point2D) -> f32 {
        let dx = self.x - other.x;
        let dy = self.y - other.y;
        (dx * dx + dy * dy).sqrt()
    }

    /// Calculate Manhattan distance to another point
    pub fn manhattan_distance_to(&self, other: &Point2D) -> f32 {
        (self.x - other.x).abs() + (self.y - other.y).abs()
    }
}

impl From<(f32, f32)> for Point2D {
    fn from((x, y): (f32, f32)) -> Self {
        Self::new(x, y)
    }
}

/// A path represented as a sequence of waypoints
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Path {
    /// Waypoints along the path
    pub waypoints: Vec<Point2D>,
    /// Total path length
    pub length: f32,
}

impl Path {
    /// Create a new path from waypoints
    pub fn new(waypoints: Vec<Point2D>) -> Self {
        let length = Self::calculate_length(&waypoints);
        Self { waypoints, length }
    }

    /// Calculate total path length
    fn calculate_length(waypoints: &[Point2D]) -> f32 {
        waypoints.windows(2).map(|w| w[0].distance_to(&w[1])).sum()
    }

    /// Check if path is empty
    pub fn is_empty(&self) -> bool {
        self.waypoints.is_empty()
    }

    /// Get number of waypoints
    pub fn len(&self) -> usize {
        self.waypoints.len()
    }

    /// Simplify path by removing redundant waypoints
    pub fn simplify(&mut self, tolerance: f32) {
        if self.waypoints.len() <= 2 {
            return;
        }

        let mut keep = vec![true; self.waypoints.len()];
        self.simplify_recursive(0, self.waypoints.len() - 1, tolerance, &mut keep);

        self.waypoints = self
            .waypoints
            .iter()
            .enumerate()
            .filter(|(i, _)| keep[*i])
            .map(|(_, p)| *p)
            .collect();

        self.length = Self::calculate_length(&self.waypoints);
    }

    fn simplify_recursive(&self, start: usize, end: usize, tolerance: f32, keep: &mut [bool]) {
        if end - start <= 1 {
            return;
        }

        let mut max_dist = 0.0;
        let mut max_idx = start;

        for i in (start + 1)..end {
            let dist = self.perpendicular_distance(i, start, end);
            if dist > max_dist {
                max_dist = dist;
                max_idx = i;
            }
        }

        if max_dist > tolerance {
            keep[max_idx] = true;
            self.simplify_recursive(start, max_idx, tolerance, keep);
            self.simplify_recursive(max_idx, end, tolerance, keep);
        }
    }

    fn perpendicular_distance(&self, point_idx: usize, line_start: usize, line_end: usize) -> f32 {
        let p = self.waypoints[point_idx];
        let a = self.waypoints[line_start];
        let b = self.waypoints[line_end];

        let dx = b.x - a.x;
        let dy = b.y - a.y;
        let norm = (dx * dx + dy * dy).sqrt();

        if norm < f32::EPSILON {
            return p.distance_to(&a);
        }

        ((dy * p.x - dx * p.y + b.x * a.y - b.y * a.x).abs()) / norm
    }
}

/// An obstacle in the environment
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Obstacle {
    /// Center point of the obstacle
    pub center: Point2D,
    /// Radius of the obstacle (circular obstacles)
    pub radius: f32,
}

impl Obstacle {
    /// Create a new circular obstacle
    pub fn new(center: Point2D, radius: f32) -> Self {
        Self { center, radius }
    }

    /// Check if a point is inside this obstacle
    pub fn contains(&self, point: &Point2D) -> bool {
        self.center.distance_to(point) < self.radius
    }

    /// Check if a line segment intersects this obstacle
    pub fn intersects_segment(&self, start: &Point2D, end: &Point2D) -> bool {
        let dx = end.x - start.x;
        let dy = end.y - start.y;
        let len_sq = dx * dx + dy * dy;

        if len_sq < f32::EPSILON {
            return self.contains(start);
        }

        let t = ((self.center.x - start.x) * dx + (self.center.y - start.y) * dy) / len_sq;
        let t = t.clamp(0.0, 1.0);

        let closest = Point2D::new(start.x + t * dx, start.y + t * dy);
        self.center.distance_to(&closest) < self.radius
    }
}