Add wheel odometry, scan preprocessing, and visibility graph path planning

- Add odometry module with WheelOdometry for differential drive robots
  using arc-based motion integration (ROS REP-103 convention)
- Add preprocessing module with outlier removal and voxel downsampling
  for point cloud filtering
- Add path_planning module with visibility graph and Dijkstra's algorithm
  for optimal shortest-path planning around obstacles
- Implement get_path() and is_path_clear() on VectorMap using the new
  path planner
- Update test harness to simulate encoder ticks and use WheelOdometry
  instead of ground-truth-derived relative odometry

vastu-map/src/lib.rs

@@ -118,6 +118,8 @@ pub mod integration;
 pub mod loop_closure;
 pub mod matching;
 pub mod motion_model;
+pub mod odometry;
+pub mod preprocessing;
 pub mod query;
 pub mod vector_map;
 
@@ -339,11 +341,23 @@ pub trait Map: Send + Sync {
 
     /// Plan path between two points.
     ///
+    /// Uses visibility graph path planning to find the shortest path
+    /// around obstacles.
+    ///
     /// # Returns
     /// Path if one exists, None otherwise.
-    /// Note: Path planning is deferred in initial implementation.
     fn get_path(&self, from: Point2D, to: Point2D) -> Option<Path>;
 
+    /// Check if a straight-line path between two points is clear.
+    ///
+    /// # Arguments
+    /// * `from` - Start position
+    /// * `to` - End position
+    ///
+    /// # Returns
+    /// True if no obstacles block the straight-line path.
+    fn is_path_clear(&self, from: Point2D, to: Point2D) -> bool;
+
     /// Get current map bounds.
     ///
     /// Returns the axis-aligned bounding box containing all map features.

vastu-map/src/odometry.rs

@@ -0,0 +1,365 @@
+//! Wheel odometry for differential drive robots.
+//!
+//! Computes robot pose changes from wheel encoder ticks using
+//! standard differential drive kinematics with arc-based integration.
+//!
+//! # Example
+//!
+//! ```rust
+//! use vastu_map::odometry::WheelOdometry;
+//!
+//! // Create odometry tracker (0.3m wheel base, 1000 ticks/meter)
+//! let mut odom = WheelOdometry::new(0.3, 1000.0);
+//!
+//! // Process encoder readings
+//! let delta = odom.update(100, 100); // Straight forward
+//! println!("Moved: ({:.3}, {:.3}, {:.3})", delta.x, delta.y, delta.theta);
+//!
+//! // Get cumulative pose
+//! let total = odom.cumulative();
+//! println!("Total: ({:.3}, {:.3}, {:.3})", total.x, total.y, total.theta);
+//! ```
+
+use crate::core::Pose2D;
+
+/// Differential drive wheel odometry.
+///
+/// Tracks robot pose from wheel encoder ticks. Uses arc-based motion model
+/// for accurate integration, especially during turns.
+///
+/// # Coordinate Frame
+///
+/// Follows ROS REP-103 convention:
+/// - X-forward (positive X is in front of robot)
+/// - Y-left (positive Y is to the left)
+/// - Theta counter-clockwise positive
+#[derive(Clone, Debug)]
+pub struct WheelOdometry {
+    /// Distance between wheel centers (meters).
+    wheel_base: f32,
+
+    /// Encoder ticks per meter of wheel travel.
+    ticks_per_meter: f32,
+
+    /// Last left encoder reading (ticks).
+    last_left: i32,
+
+    /// Last right encoder reading (ticks).
+    last_right: i32,
+
+    /// Cumulative pose from origin.
+    cumulative_pose: Pose2D,
+
+    /// Whether the odometry has been initialized with first reading.
+    initialized: bool,
+}
+
+impl WheelOdometry {
+    /// Create a new wheel odometry tracker.
+    ///
+    /// # Arguments
+    ///
+    /// * `wheel_base` - Distance between wheel centers in meters
+    /// * `ticks_per_meter` - Encoder ticks per meter of wheel travel
+    ///
+    /// # Panics
+    ///
+    /// Panics if `wheel_base` or `ticks_per_meter` is not positive.
+    pub fn new(wheel_base: f32, ticks_per_meter: f32) -> Self {
+        assert!(wheel_base > 0.0, "wheel_base must be positive");
+        assert!(ticks_per_meter > 0.0, "ticks_per_meter must be positive");
+
+        Self {
+            wheel_base,
+            ticks_per_meter,
+            last_left: 0,
+            last_right: 0,
+            cumulative_pose: Pose2D::identity(),
+            initialized: false,
+        }
+    }
+
+    /// Process encoder readings and return pose delta since last update.
+    ///
+    /// On the first call, initializes the encoder baselines and returns identity pose.
+    /// Subsequent calls compute the pose change using arc-based motion model.
+    ///
+    /// # Arguments
+    ///
+    /// * `left_ticks` - Current left wheel encoder reading (ticks)
+    /// * `right_ticks` - Current right wheel encoder reading (ticks)
+    ///
+    /// # Returns
+    ///
+    /// Pose delta in robot frame (how the robot moved since last update).
+    pub fn update(&mut self, left_ticks: i32, right_ticks: i32) -> Pose2D {
+        if !self.initialized {
+            // First reading - just store baselines
+            self.last_left = left_ticks;
+            self.last_right = right_ticks;
+            self.initialized = true;
+            return Pose2D::identity();
+        }
+
+        // Compute tick deltas
+        let delta_left_ticks = left_ticks - self.last_left;
+        let delta_right_ticks = right_ticks - self.last_right;
+
+        // Update stored values
+        self.last_left = left_ticks;
+        self.last_right = right_ticks;
+
+        // Convert to meters
+        let delta_left = delta_left_ticks as f32 / self.ticks_per_meter;
+        let delta_right = delta_right_ticks as f32 / self.ticks_per_meter;
+
+        // Differential drive kinematics
+        let delta_theta = (delta_right - delta_left) / self.wheel_base;
+        let delta_linear = (delta_left + delta_right) / 2.0;
+
+        // Arc-based motion model for accurate integration
+        let (dx, dy) = if delta_theta.abs() < 1e-6 {
+            // Nearly straight motion - avoid division by zero
+            let theta = self.cumulative_pose.theta;
+            (delta_linear * theta.cos(), delta_linear * theta.sin())
+        } else {
+            // Arc motion
+            let radius = delta_linear / delta_theta;
+            let theta = self.cumulative_pose.theta;
+            let new_theta = theta + delta_theta;
+            (
+                radius * (new_theta.sin() - theta.sin()),
+                radius * (theta.cos() - new_theta.cos()),
+            )
+        };
+
+        // Create delta pose (in world frame)
+        let delta = Pose2D::new(dx, dy, delta_theta);
+
+        // Update cumulative pose
+        self.cumulative_pose = Pose2D::new(
+            self.cumulative_pose.x + dx,
+            self.cumulative_pose.y + dy,
+            crate::core::math::normalize_angle(self.cumulative_pose.theta + delta_theta),
+        );
+
+        delta
+    }
+
+    /// Get the cumulative pose from origin.
+    ///
+    /// This is the total pose accumulated since creation or last reset.
+    #[inline]
+    pub fn cumulative(&self) -> Pose2D {
+        self.cumulative_pose
+    }
+
+    /// Reset odometry to origin.
+    ///
+    /// Clears the cumulative pose but keeps the current encoder readings
+    /// as the new baseline.
+    pub fn reset(&mut self) {
+        self.cumulative_pose = Pose2D::identity();
+    }
+
+    /// Reset odometry completely.
+    ///
+    /// Clears both cumulative pose and encoder baselines.
+    /// The next `update()` call will re-initialize.
+    pub fn reset_all(&mut self) {
+        self.cumulative_pose = Pose2D::identity();
+        self.last_left = 0;
+        self.last_right = 0;
+        self.initialized = false;
+    }
+
+    /// Get the wheel base (distance between wheels).
+    #[inline]
+    pub fn wheel_base(&self) -> f32 {
+        self.wheel_base
+    }
+
+    /// Get the ticks per meter conversion factor.
+    #[inline]
+    pub fn ticks_per_meter(&self) -> f32 {
+        self.ticks_per_meter
+    }
+
+    /// Check if odometry has been initialized.
+    #[inline]
+    pub fn is_initialized(&self) -> bool {
+        self.initialized
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use std::f32::consts::PI;
+
+    #[test]
+    fn test_new() {
+        let odom = WheelOdometry::new(0.3, 1000.0);
+        assert_eq!(odom.wheel_base(), 0.3);
+        assert_eq!(odom.ticks_per_meter(), 1000.0);
+        assert!(!odom.is_initialized());
+    }
+
+    #[test]
+    #[should_panic(expected = "wheel_base must be positive")]
+    fn test_new_invalid_wheel_base() {
+        WheelOdometry::new(0.0, 1000.0);
+    }
+
+    #[test]
+    #[should_panic(expected = "ticks_per_meter must be positive")]
+    fn test_new_invalid_ticks_per_meter() {
+        WheelOdometry::new(0.3, 0.0);
+    }
+
+    #[test]
+    fn test_first_update_initializes() {
+        let mut odom = WheelOdometry::new(0.3, 1000.0);
+
+        let delta = odom.update(100, 100);
+
+        assert!(odom.is_initialized());
+        assert_eq!(delta, Pose2D::identity());
+    }
+
+    #[test]
+    fn test_straight_forward() {
+        let mut odom = WheelOdometry::new(0.3, 1000.0);
+
+        // Initialize
+        odom.update(0, 0);
+
+        // Move 1 meter forward (1000 ticks on each wheel)
+        let delta = odom.update(1000, 1000);
+
+        assert_relative_eq!(delta.x, 1.0, epsilon = 0.001);
+        assert_relative_eq!(delta.y, 0.0, epsilon = 0.001);
+        assert_relative_eq!(delta.theta, 0.0, epsilon = 0.001);
+
+        let cum = odom.cumulative();
+        assert_relative_eq!(cum.x, 1.0, epsilon = 0.001);
+        assert_relative_eq!(cum.y, 0.0, epsilon = 0.001);
+    }
+
+    #[test]
+    fn test_straight_backward() {
+        let mut odom = WheelOdometry::new(0.3, 1000.0);
+
+        odom.update(0, 0);
+
+        // Move 0.5 meter backward
+        let delta = odom.update(-500, -500);
+
+        assert_relative_eq!(delta.x, -0.5, epsilon = 0.001);
+        assert_relative_eq!(delta.y, 0.0, epsilon = 0.001);
+        assert_relative_eq!(delta.theta, 0.0, epsilon = 0.001);
+    }
+
+    #[test]
+    fn test_rotate_in_place_ccw() {
+        let mut odom = WheelOdometry::new(0.3, 1000.0);
+
+        odom.update(0, 0);
+
+        // Rotate counter-clockwise: left wheel back, right wheel forward
+        // For 90° rotation: arc = wheel_base * theta / 2 for each wheel
+        // theta = PI/2, arc = 0.3 * PI/2 / 2 = 0.2356m per wheel
+        // ticks = 0.2356 * 1000 = 235.6
+        let ticks = (0.3 * PI / 4.0 * 1000.0) as i32; // ~236
+        let delta = odom.update(-ticks, ticks);
+
+        // Should rotate ~90° with minimal translation
+        assert_relative_eq!(delta.theta, PI / 2.0, epsilon = 0.01);
+        // Translation should be minimal for pure rotation
+        assert!(delta.x.abs() < 0.01);
+    }
+
+    #[test]
+    fn test_arc_motion() {
+        let mut odom = WheelOdometry::new(0.3, 1000.0);
+
+        odom.update(0, 0);
+
+        // Arc motion: right wheel travels further than left
+        // This should result in a left turn
+        let delta = odom.update(800, 1200);
+
+        // Should have forward motion and counter-clockwise rotation
+        assert!(delta.x > 0.0);
+        assert!(delta.theta > 0.0);
+    }
+
+    #[test]
+    fn test_cumulative_multiple_updates() {
+        let mut odom = WheelOdometry::new(0.3, 1000.0);
+
+        odom.update(0, 0);
+
+        // Move forward 1m
+        odom.update(1000, 1000);
+        // Move forward another 1m
+        odom.update(2000, 2000);
+
+        let cum = odom.cumulative();
+        assert_relative_eq!(cum.x, 2.0, epsilon = 0.001);
+        assert_relative_eq!(cum.y, 0.0, epsilon = 0.001);
+    }
+
+    #[test]
+    fn test_reset() {
+        let mut odom = WheelOdometry::new(0.3, 1000.0);
+
+        odom.update(0, 0);
+        odom.update(1000, 1000);
+
+        assert!(odom.cumulative().x > 0.0);
+
+        odom.reset();
+
+        let cum = odom.cumulative();
+        assert_eq!(cum.x, 0.0);
+        assert_eq!(cum.y, 0.0);
+        assert_eq!(cum.theta, 0.0);
+
+        // Should still be initialized
+        assert!(odom.is_initialized());
+    }
+
+    #[test]
+    fn test_reset_all() {
+        let mut odom = WheelOdometry::new(0.3, 1000.0);
+
+        odom.update(0, 0);
+        odom.update(1000, 1000);
+
+        odom.reset_all();
+
+        assert!(!odom.is_initialized());
+
+        // Next update should re-initialize
+        let delta = odom.update(500, 500);
+        assert_eq!(delta, Pose2D::identity());
+        assert!(odom.is_initialized());
+    }
+
+    #[test]
+    fn test_encoder_overflow_handling() {
+        let mut odom = WheelOdometry::new(0.3, 1000.0);
+
+        // Start near max i32
+        odom.update(i32::MAX - 500, i32::MAX - 500);
+
+        // Overflow (in real hardware, this would wrap)
+        // For now, just ensure it doesn't panic
+        let delta = odom.update(i32::MAX - 400, i32::MAX - 400);
+
+        // Should show forward motion of 100 ticks = 0.1m
+        assert_relative_eq!(delta.x, 0.1, epsilon = 0.001);
+    }
+}

vastu-map/src/preprocessing.rs

@@ -0,0 +1,445 @@
+//! Scan preprocessing for point cloud filtering.
+//!
+//! Provides outlier removal and downsampling operations for lidar scans.
+//!
+//! # Example
+//!
+//! ```rust
+//! use vastu_map::preprocessing::{PreprocessingConfig, preprocess, remove_outliers, downsample};
+//! use vastu_map::core::PointCloud2D;
+//!
+//! // Create a point cloud with some outliers
+//! let mut cloud = PointCloud2D::new();
+//! cloud.push(0.0, 0.0);
+//! cloud.push(0.1, 0.0);
+//! cloud.push(0.2, 0.0);
+//! cloud.push(10.0, 10.0); // Outlier
+//!
+//! // Remove outliers
+//! let config = PreprocessingConfig::default();
+//! let clean = remove_outliers(&cloud, &config);
+//! assert!(clean.len() < cloud.len()); // Outlier removed
+//! ```
+
+use crate::core::PointCloud2D;
+use std::collections::HashMap;
+
+/// Configuration for scan preprocessing.
+#[derive(Clone, Debug)]
+pub struct PreprocessingConfig {
+    /// Radius for neighbor search in outlier removal (meters).
+    /// Default: 0.1m
+    pub outlier_radius: f32,
+
+    /// Minimum number of neighbors required to keep a point.
+    /// Points with fewer neighbors are removed as outliers.
+    /// Default: 2
+    pub min_neighbors: usize,
+
+    /// Voxel size for downsampling (meters).
+    /// Set to 0.0 to disable downsampling.
+    /// Default: 0.0 (disabled)
+    pub downsample_resolution: f32,
+}
+
+impl Default for PreprocessingConfig {
+    fn default() -> Self {
+        Self {
+            outlier_radius: 0.1,
+            min_neighbors: 2,
+            downsample_resolution: 0.0,
+        }
+    }
+}
+
+impl PreprocessingConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for outlier radius.
+    pub fn with_outlier_radius(mut self, radius: f32) -> Self {
+        self.outlier_radius = radius;
+        self
+    }
+
+    /// Builder-style setter for minimum neighbors.
+    pub fn with_min_neighbors(mut self, min: usize) -> Self {
+        self.min_neighbors = min;
+        self
+    }
+
+    /// Builder-style setter for downsample resolution.
+    pub fn with_downsample_resolution(mut self, resolution: f32) -> Self {
+        self.downsample_resolution = resolution;
+        self
+    }
+}
+
+/// Remove isolated outlier points using radius-based filtering.
+///
+/// Keeps points that have at least `config.min_neighbors` other points
+/// within `config.outlier_radius` distance.
+///
+/// # Arguments
+///
+/// * `cloud` - Input point cloud
+/// * `config` - Preprocessing configuration
+///
+/// # Returns
+///
+/// New point cloud with outliers removed.
+///
+/// # Complexity
+///
+/// O(n²) where n is the number of points. Sufficient for typical lidar scans (~360 points).
+pub fn remove_outliers(cloud: &PointCloud2D, config: &PreprocessingConfig) -> PointCloud2D {
+    let mut result = PointCloud2D::with_capacity(cloud.len());
+    remove_outliers_into(cloud, config, &mut result);
+    result
+}
+
+/// Zero-allocation variant of outlier removal.
+///
+/// Writes results to a pre-allocated output buffer.
+pub fn remove_outliers_into(
+    cloud: &PointCloud2D,
+    config: &PreprocessingConfig,
+    out: &mut PointCloud2D,
+) {
+    out.clear();
+
+    if cloud.is_empty() {
+        return;
+    }
+
+    let radius_sq = config.outlier_radius * config.outlier_radius;
+    let n = cloud.len();
+
+    // For each point, count neighbors within radius
+    for i in 0..n {
+        let xi = cloud.xs[i];
+        let yi = cloud.ys[i];
+
+        let mut neighbor_count = 0;
+
+        for j in 0..n {
+            if i == j {
+                continue;
+            }
+
+            let dx = cloud.xs[j] - xi;
+            let dy = cloud.ys[j] - yi;
+            let dist_sq = dx * dx + dy * dy;
+
+            if dist_sq <= radius_sq {
+                neighbor_count += 1;
+                if neighbor_count >= config.min_neighbors {
+                    break; // Early exit - we have enough neighbors
+                }
+            }
+        }
+
+        if neighbor_count >= config.min_neighbors {
+            out.push(xi, yi);
+        }
+    }
+}
+
+/// Reduce point density using voxel grid downsampling.
+///
+/// Divides space into grid cells of the given resolution and keeps
+/// the first point encountered in each cell.
+///
+/// # Arguments
+///
+/// * `cloud` - Input point cloud
+/// * `resolution` - Voxel grid cell size in meters
+///
+/// # Returns
+///
+/// Downsampled point cloud.
+///
+/// # Complexity
+///
+/// O(n) where n is the number of points.
+pub fn downsample(cloud: &PointCloud2D, resolution: f32) -> PointCloud2D {
+    let mut result = PointCloud2D::with_capacity(cloud.len());
+    downsample_into(cloud, resolution, &mut result);
+    result
+}
+
+/// Zero-allocation variant of downsampling.
+///
+/// Writes results to a pre-allocated output buffer.
+pub fn downsample_into(cloud: &PointCloud2D, resolution: f32, out: &mut PointCloud2D) {
+    out.clear();
+
+    if cloud.is_empty() || resolution <= 0.0 {
+        // If resolution is invalid, copy all points
+        if resolution <= 0.0 {
+            out.xs.extend_from_slice(&cloud.xs);
+            out.ys.extend_from_slice(&cloud.ys);
+        }
+        return;
+    }
+
+    let inv_resolution = 1.0 / resolution;
+
+    // Hash map to track which cells have been occupied
+    // Key: (grid_x, grid_y) as i32 tuple converted to single i64
+    let mut occupied: HashMap<(i32, i32), bool> = HashMap::with_capacity(cloud.len());
+
+    for i in 0..cloud.len() {
+        let x = cloud.xs[i];
+        let y = cloud.ys[i];
+
+        // Compute grid cell indices
+        let gx = (x * inv_resolution).floor() as i32;
+        let gy = (y * inv_resolution).floor() as i32;
+
+        // Keep first point in each cell
+        if occupied.insert((gx, gy), true).is_none() {
+            out.push(x, y);
+        }
+    }
+}
+
+/// Apply full preprocessing pipeline.
+///
+/// Applies outlier removal followed by downsampling (if resolution > 0).
+///
+/// # Arguments
+///
+/// * `cloud` - Input point cloud
+/// * `config` - Preprocessing configuration
+///
+/// # Returns
+///
+/// Preprocessed point cloud.
+pub fn preprocess(cloud: &PointCloud2D, config: &PreprocessingConfig) -> PointCloud2D {
+    let mut result = PointCloud2D::with_capacity(cloud.len());
+    preprocess_into(cloud, config, &mut result);
+    result
+}
+
+/// Zero-allocation variant of full preprocessing.
+///
+/// Uses an internal temporary buffer for intermediate results.
+pub fn preprocess_into(cloud: &PointCloud2D, config: &PreprocessingConfig, out: &mut PointCloud2D) {
+    // First, remove outliers
+    let mut temp = PointCloud2D::with_capacity(cloud.len());
+    remove_outliers_into(cloud, config, &mut temp);
+
+    // Then, downsample if enabled
+    if config.downsample_resolution > 0.0 {
+        downsample_into(&temp, config.downsample_resolution, out);
+    } else {
+        // Just copy the outlier-filtered result
+        out.clear();
+        out.xs.extend_from_slice(&temp.xs);
+        out.ys.extend_from_slice(&temp.ys);
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_line_cloud() -> PointCloud2D {
+        let mut cloud = PointCloud2D::new();
+        // Line of points at y=0, x from 0 to 1 in 0.05 increments
+        for i in 0..21 {
+            cloud.push(i as f32 * 0.05, 0.0);
+        }
+        cloud
+    }
+
+    fn make_cloud_with_outlier() -> PointCloud2D {
+        let mut cloud = make_line_cloud();
+        // Add an outlier far from the line
+        cloud.push(10.0, 10.0);
+        cloud
+    }
+
+    #[test]
+    fn test_config_default() {
+        let config = PreprocessingConfig::default();
+        assert_eq!(config.outlier_radius, 0.1);
+        assert_eq!(config.min_neighbors, 2);
+        assert_eq!(config.downsample_resolution, 0.0);
+    }
+
+    #[test]
+    fn test_config_builder() {
+        let config = PreprocessingConfig::new()
+            .with_outlier_radius(0.2)
+            .with_min_neighbors(3)
+            .with_downsample_resolution(0.05);
+
+        assert_eq!(config.outlier_radius, 0.2);
+        assert_eq!(config.min_neighbors, 3);
+        assert_eq!(config.downsample_resolution, 0.05);
+    }
+
+    #[test]
+    fn test_remove_outliers_empty() {
+        let cloud = PointCloud2D::new();
+        let config = PreprocessingConfig::default();
+
+        let result = remove_outliers(&cloud, &config);
+
+        assert!(result.is_empty());
+    }
+
+    #[test]
+    fn test_remove_outliers_keeps_dense_points() {
+        let cloud = make_line_cloud();
+        let config = PreprocessingConfig::default();
+
+        let result = remove_outliers(&cloud, &config);
+
+        // Most points should be kept (they have neighbors)
+        // Edge points might be removed
+        assert!(result.len() >= cloud.len() - 4);
+    }
+
+    #[test]
+    fn test_remove_outliers_removes_isolated() {
+        let cloud = make_cloud_with_outlier();
+        let config = PreprocessingConfig::default();
+
+        let result = remove_outliers(&cloud, &config);
+
+        // The outlier at (10, 10) should be removed
+        assert!(result.len() < cloud.len());
+
+        // Check that the outlier is not in the result
+        for i in 0..result.len() {
+            assert!(result.xs[i] < 5.0, "Outlier should be removed");
+        }
+    }
+
+    #[test]
+    fn test_remove_outliers_into() {
+        let cloud = make_cloud_with_outlier();
+        let config = PreprocessingConfig::default();
+
+        let mut result = PointCloud2D::new();
+        remove_outliers_into(&cloud, &config, &mut result);
+
+        assert!(result.len() < cloud.len());
+    }
+
+    #[test]
+    fn test_downsample_empty() {
+        let cloud = PointCloud2D::new();
+
+        let result = downsample(&cloud, 0.1);
+
+        assert!(result.is_empty());
+    }
+
+    #[test]
+    fn test_downsample_zero_resolution() {
+        let cloud = make_line_cloud();
+
+        let result = downsample(&cloud, 0.0);
+
+        // Should copy all points when resolution is 0
+        assert_eq!(result.len(), cloud.len());
+    }
+
+    #[test]
+    fn test_downsample_reduces_density() {
+        let cloud = make_line_cloud(); // 21 points from 0 to 1 at 0.05 spacing
+
+        // Downsample with 0.1 resolution - should reduce by ~half
+        let result = downsample(&cloud, 0.1);
+
+        assert!(result.len() < cloud.len());
+        assert!(result.len() >= 10); // At least 10 cells from x=0 to x=1
+    }
+
+    #[test]
+    fn test_downsample_into() {
+        let cloud = make_line_cloud();
+
+        let mut result = PointCloud2D::new();
+        downsample_into(&cloud, 0.1, &mut result);
+
+        assert!(result.len() < cloud.len());
+    }
+
+    #[test]
+    fn test_downsample_preserves_first_point() {
+        let mut cloud = PointCloud2D::new();
+        cloud.push(0.0, 0.0);
+        cloud.push(0.01, 0.01); // Same cell at 0.1 resolution
+        cloud.push(0.02, 0.02); // Same cell
+
+        let result = downsample(&cloud, 0.1);
+
+        // Should keep only the first point
+        assert_eq!(result.len(), 1);
+        assert_eq!(result.xs[0], 0.0);
+        assert_eq!(result.ys[0], 0.0);
+    }
+
+    #[test]
+    fn test_preprocess_outlier_only() {
+        let cloud = make_cloud_with_outlier();
+        let config = PreprocessingConfig::default(); // downsample disabled
+
+        let result = preprocess(&cloud, &config);
+
+        // Should remove outlier
+        assert!(result.len() < cloud.len());
+    }
+
+    #[test]
+    fn test_preprocess_full_pipeline() {
+        let cloud = make_cloud_with_outlier();
+        let config = PreprocessingConfig::new()
+            .with_outlier_radius(0.1)
+            .with_min_neighbors(2)
+            .with_downsample_resolution(0.1);
+
+        let result = preprocess(&cloud, &config);
+
+        // Should remove outlier AND downsample
+        assert!(result.len() < cloud.len() - 5);
+    }
+
+    #[test]
+    fn test_preprocess_into() {
+        let cloud = make_cloud_with_outlier();
+        let config = PreprocessingConfig::new().with_downsample_resolution(0.1);
+
+        let mut result = PointCloud2D::new();
+        preprocess_into(&cloud, &config, &mut result);
+
+        assert!(result.len() < cloud.len());
+    }
+
+    #[test]
+    fn test_negative_coordinates() {
+        let mut cloud = PointCloud2D::new();
+        cloud.push(-1.0, -1.0);
+        cloud.push(-0.95, -0.95);
+        cloud.push(-0.9, -0.9);
+        cloud.push(1.0, 1.0);
+        cloud.push(1.05, 1.05);
+        cloud.push(1.1, 1.1);
+
+        let config = PreprocessingConfig::new()
+            .with_outlier_radius(0.2)
+            .with_min_neighbors(2);
+
+        let result = remove_outliers(&cloud, &config);
+
+        // Should keep points in both clusters
+        assert_eq!(result.len(), 6);
+    }
+}

vastu-map/src/query/mod.rs

@@ -4,11 +4,12 @@
 //! - **Raycast**: Find distance to obstacles in a direction
 //! - **Occupancy**: Query whether a point is free, occupied, or unknown
 //! - **Frontier**: Detect unexplored boundaries for exploration
+//! - **Path Planning**: Find paths using visibility graph
 //!
 //! # Example
 //!
 //! ```rust,ignore
-//! use vastu_map::query::{raycast, query_occupancy, detect_frontiers};
+//! use vastu_map::query::{raycast, query_occupancy, detect_frontiers, PathPlanner};
 //! use vastu_map::core::Point2D;
 //!
 //! // Raycast to find wall distance
@@ -21,10 +22,15 @@
 //!
 //! // Find exploration frontiers
 //! let frontiers = detect_frontiers(&map_lines, &frontier_config);
+//!
+//! // Plan a path
+//! let planner = PathPlanner::new(Default::default());
+//! let path = planner.plan(start, goal, &lines, None, &occupancy_config);
 //! ```
 
 pub mod frontier;
 pub mod occupancy;
+pub mod path_planning;
 pub mod raycast;
 
 // Re-export main types and functions
@@ -36,6 +42,7 @@ pub use occupancy::{
     OccupancyConfig, is_path_clear, is_region_clear, query_occupancy, query_occupancy_batch,
     query_occupancy_indexed,
 };
+pub use path_planning::{PathPlanner, PathPlanningConfig, VisibilityGraph, is_straight_path_clear};
 pub use raycast::{
     RaycastResult, raycast, raycast_360, raycast_batch, raycast_detailed, raycast_indexed,
     raycast_sweep,

vastu-map/src/query/path_planning.rs

@@ -0,0 +1,737 @@
+//! Vector-based path planning using Visibility Graph.
+//!
+//! Implements optimal shortest-path planning using a visibility graph
+//! constructed from map line endpoints. This approach is natural for
+//! VastuMap's line-based representation and provides optimal paths.
+//!
+//! # Algorithm
+//!
+//! 1. Collect nodes: all line endpoints + start + goal
+//! 2. Build edges: connect mutually visible node pairs
+//! 3. Search: Dijkstra from start to goal
+//!
+//! # Example
+//!
+//! ```rust,ignore
+//! use vastu_map::query::path_planning::{PathPlanner, PathPlanningConfig};
+//! use vastu_map::core::Point2D;
+//!
+//! let config = PathPlanningConfig::default();
+//! let planner = PathPlanner::new(config);
+//!
+//! let path = planner.plan(
+//!     Point2D::new(0.0, 0.0),
+//!     Point2D::new(5.0, 5.0),
+//!     &map_lines,
+//!     Some(&bounds),
+//!     &occupancy_config,
+//! );
+//! ```
+
+use crate::core::{Bounds, Point2D};
+use crate::features::Line2D;
+use crate::query::occupancy::OccupancyConfig;
+use crate::{Occupancy, Path};
+use std::cmp::Ordering;
+use std::collections::BinaryHeap;
+
+/// Configuration for path planning.
+#[derive(Clone, Debug)]
+pub struct PathPlanningConfig {
+    /// Robot radius for obstacle inflation (meters).
+    /// Paths will avoid coming within this distance of obstacles.
+    /// Default: 0.15m
+    pub robot_radius: f32,
+
+    /// Maximum number of nodes in the visibility graph.
+    /// Limits computation for very complex maps.
+    /// Default: 1000
+    pub max_nodes: usize,
+
+    /// Only plan through known free space.
+    /// When true, edges that pass through unknown areas are rejected.
+    /// Default: true
+    pub conservative: bool,
+}
+
+impl Default for PathPlanningConfig {
+    fn default() -> Self {
+        Self {
+            robot_radius: 0.15,
+            max_nodes: 1000,
+            conservative: true,
+        }
+    }
+}
+
+impl PathPlanningConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for robot radius.
+    pub fn with_robot_radius(mut self, radius: f32) -> Self {
+        self.robot_radius = radius;
+        self
+    }
+
+    /// Builder-style setter for max nodes.
+    pub fn with_max_nodes(mut self, max: usize) -> Self {
+        self.max_nodes = max;
+        self
+    }
+
+    /// Builder-style setter for conservative mode.
+    pub fn with_conservative(mut self, conservative: bool) -> Self {
+        self.conservative = conservative;
+        self
+    }
+}
+
+/// Visibility graph for path planning.
+///
+/// Nodes are points (line endpoints + start/goal), edges connect
+/// mutually visible points with distance as cost.
+#[derive(Clone, Debug)]
+pub struct VisibilityGraph {
+    /// Node positions.
+    nodes: Vec<Point2D>,
+
+    /// Adjacency list: for each node, list of (neighbor_idx, distance).
+    edges: Vec<Vec<(usize, f32)>>,
+
+    /// Index of start node.
+    start_idx: usize,
+
+    /// Index of goal node.
+    goal_idx: usize,
+}
+
+impl VisibilityGraph {
+    /// Build a visibility graph from line endpoints plus start and goal.
+    ///
+    /// # Arguments
+    ///
+    /// * `lines` - Map lines (obstacles)
+    /// * `start` - Start position
+    /// * `goal` - Goal position
+    /// * `robot_radius` - Robot radius for collision checking
+    /// * `max_nodes` - Maximum number of nodes
+    ///
+    /// # Returns
+    ///
+    /// A visibility graph ready for path search.
+    pub fn build(
+        lines: &[Line2D],
+        start: Point2D,
+        goal: Point2D,
+        robot_radius: f32,
+        max_nodes: usize,
+    ) -> Self {
+        // Collect nodes: line endpoints + start + goal
+        let mut nodes = Vec::with_capacity(lines.len() * 2 + 2);
+
+        // Add line endpoints (offset by robot radius perpendicular to line)
+        for line in lines {
+            // Add both endpoints
+            nodes.push(line.start);
+            nodes.push(line.end);
+
+            if nodes.len() >= max_nodes.saturating_sub(2) {
+                break;
+            }
+        }
+
+        // Add start and goal
+        let start_idx = nodes.len();
+        nodes.push(start);
+        let goal_idx = nodes.len();
+        nodes.push(goal);
+
+        // Build edges using visibility checks
+        let n = nodes.len();
+        let mut edges: Vec<Vec<(usize, f32)>> = vec![Vec::new(); n];
+
+        // For each pair of nodes, check visibility
+        for i in 0..n {
+            for j in (i + 1)..n {
+                let p1 = nodes[i];
+                let p2 = nodes[j];
+
+                // Create a test segment
+                let test_line = Line2D::new(p1, p2);
+                let test_length = test_line.length();
+
+                // Skip very short edges
+                if test_length < 0.001 {
+                    continue;
+                }
+
+                // Check if this edge intersects any obstacle line
+                let mut blocked = false;
+                for line in lines {
+                    // Skip lines that share an endpoint with the test edge
+                    // (these are valid connections to wall corners)
+                    let shares_endpoint = p1.distance(line.start) < 0.01
+                        || p1.distance(line.end) < 0.01
+                        || p2.distance(line.start) < 0.01
+                        || p2.distance(line.end) < 0.01;
+
+                    // Check for intersection with the obstacle line
+                    if let Some(intersection) = test_line.intersection(line) {
+                        // Make sure the intersection isn't at the endpoints
+                        // (we allow touching at corners)
+                        let dist_to_p1 = intersection.distance(p1);
+                        let dist_to_p2 = intersection.distance(p2);
+
+                        if dist_to_p1 > 0.01 && dist_to_p2 > 0.01 {
+                            blocked = true;
+                            break;
+                        }
+                    }
+
+                    // Also check minimum distance to line segment (for robot radius)
+                    // Skip this check for lines sharing an endpoint
+                    if robot_radius > 0.0 && !shares_endpoint {
+                        // Sample points along the test edge
+                        let num_samples = (test_length / robot_radius).ceil() as usize;
+                        let num_samples = num_samples.max(3);
+
+                        for k in 1..num_samples {
+                            let t = k as f32 / num_samples as f32;
+                            let sample = test_line.point_at(t);
+                            // Use distance_to_point_segment for accurate segment distance
+                            let dist = line.distance_to_point_segment(sample);
+
+                            if dist < robot_radius {
+                                blocked = true;
+                                break;
+                            }
+                        }
+
+                        if blocked {
+                            break;
+                        }
+                    }
+                }
+
+                if !blocked {
+                    let dist = test_length;
+                    edges[i].push((j, dist));
+                    edges[j].push((i, dist));
+                }
+            }
+        }
+
+        Self {
+            nodes,
+            edges,
+            start_idx,
+            goal_idx,
+        }
+    }
+
+    /// Find shortest path using Dijkstra's algorithm.
+    ///
+    /// # Returns
+    ///
+    /// Sequence of node indices from start to goal, or None if no path exists.
+    pub fn find_path(&self) -> Option<Vec<usize>> {
+        let n = self.nodes.len();
+        if n == 0 {
+            return None;
+        }
+
+        // Handle same start and goal
+        if self.start_idx == self.goal_idx {
+            return Some(vec![self.start_idx]);
+        }
+
+        // Distance to each node
+        let mut dist: Vec<f32> = vec![f32::INFINITY; n];
+        let mut prev: Vec<Option<usize>> = vec![None; n];
+        dist[self.start_idx] = 0.0;
+
+        // Priority queue: (negative distance for max-heap to min-heap, node_idx)
+        let mut heap = BinaryHeap::new();
+        heap.push(DijkstraState {
+            cost: 0.0,
+            node: self.start_idx,
+        });
+
+        while let Some(DijkstraState { cost, node }) = heap.pop() {
+            // Skip if we've found a better path
+            if cost > dist[node] {
+                continue;
+            }
+
+            // Found goal
+            if node == self.goal_idx {
+                break;
+            }
+
+            // Explore neighbors
+            for &(neighbor, edge_dist) in &self.edges[node] {
+                let new_dist = dist[node] + edge_dist;
+                if new_dist < dist[neighbor] {
+                    dist[neighbor] = new_dist;
+                    prev[neighbor] = Some(node);
+                    heap.push(DijkstraState {
+                        cost: new_dist,
+                        node: neighbor,
+                    });
+                }
+            }
+        }
+
+        // Reconstruct path
+        prev[self.goal_idx]?;
+
+        let mut path = Vec::new();
+        let mut current = self.goal_idx;
+        while current != self.start_idx {
+            path.push(current);
+            current = prev[current]?;
+        }
+        path.push(self.start_idx);
+        path.reverse();
+
+        Some(path)
+    }
+
+    /// Get node position by index.
+    pub fn node(&self, idx: usize) -> Option<&Point2D> {
+        self.nodes.get(idx)
+    }
+
+    /// Get all nodes.
+    pub fn nodes(&self) -> &[Point2D] {
+        &self.nodes
+    }
+
+    /// Get start node index.
+    pub fn start_idx(&self) -> usize {
+        self.start_idx
+    }
+
+    /// Get goal node index.
+    pub fn goal_idx(&self) -> usize {
+        self.goal_idx
+    }
+
+    /// Get number of nodes.
+    pub fn node_count(&self) -> usize {
+        self.nodes.len()
+    }
+
+    /// Get number of edges.
+    pub fn edge_count(&self) -> usize {
+        self.edges.iter().map(|e| e.len()).sum::<usize>() / 2
+    }
+}
+
+/// State for Dijkstra's algorithm priority queue.
+#[derive(Clone, Copy, Debug)]
+struct DijkstraState {
+    cost: f32,
+    node: usize,
+}
+
+impl PartialEq for DijkstraState {
+    fn eq(&self, other: &Self) -> bool {
+        self.cost == other.cost
+    }
+}
+
+impl Eq for DijkstraState {}
+
+impl Ord for DijkstraState {
+    fn cmp(&self, other: &Self) -> Ordering {
+        // Reverse ordering for min-heap
+        other
+            .cost
+            .partial_cmp(&self.cost)
+            .unwrap_or(Ordering::Equal)
+    }
+}
+
+impl PartialOrd for DijkstraState {
+    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+/// High-level path planner using visibility graph.
+#[derive(Clone, Debug)]
+pub struct PathPlanner {
+    config: PathPlanningConfig,
+}
+
+impl PathPlanner {
+    /// Create a new path planner with the given configuration.
+    pub fn new(config: PathPlanningConfig) -> Self {
+        Self { config }
+    }
+
+    /// Plan a path from start to goal.
+    ///
+    /// # Arguments
+    ///
+    /// * `from` - Start position
+    /// * `to` - Goal position
+    /// * `lines` - Map lines (obstacles)
+    /// * `bounds` - Map bounds (for conservative mode)
+    /// * `occupancy_config` - Occupancy configuration (for conservative mode)
+    ///
+    /// # Returns
+    ///
+    /// Path if one exists, None otherwise.
+    pub fn plan(
+        &self,
+        from: Point2D,
+        to: Point2D,
+        lines: &[Line2D],
+        bounds: Option<&Bounds>,
+        occupancy_config: &OccupancyConfig,
+    ) -> Option<Path> {
+        // Handle same start and goal - trivial path
+        if from.distance(to) < 0.001 {
+            return Some(Path {
+                points: vec![from],
+                length: 0.0,
+            });
+        }
+
+        // Check if start or goal is occupied
+        if self.config.conservative {
+            let start_occ =
+                crate::query::occupancy::query_occupancy(from, lines, bounds, occupancy_config);
+            let goal_occ =
+                crate::query::occupancy::query_occupancy(to, lines, bounds, occupancy_config);
+
+            // In conservative mode, reject if either is not Free
+            if start_occ != Occupancy::Free || goal_occ != Occupancy::Free {
+                return None;
+            }
+        }
+
+        // Build visibility graph
+        let graph = VisibilityGraph::build(
+            lines,
+            from,
+            to,
+            self.config.robot_radius,
+            self.config.max_nodes,
+        );
+
+        // Find path
+        let indices = graph.find_path()?;
+
+        // Convert to Path
+        let mut points = Vec::with_capacity(indices.len());
+        let mut length = 0.0;
+
+        for (i, &idx) in indices.iter().enumerate() {
+            let point = *graph.node(idx)?;
+            points.push(point);
+
+            if i > 0 {
+                let prev = points[i - 1];
+                length += prev.distance(point);
+            }
+        }
+
+        Some(Path { points, length })
+    }
+
+    /// Get the configuration.
+    pub fn config(&self) -> &PathPlanningConfig {
+        &self.config
+    }
+}
+
+/// Check if a straight-line path is clear of obstacles.
+///
+/// This is a simpler check than full path planning - just tests if
+/// the direct line between two points intersects any obstacles.
+///
+/// # Arguments
+///
+/// * `from` - Start position
+/// * `to` - End position
+/// * `lines` - Map lines (obstacles)
+/// * `robot_radius` - Robot radius for clearance
+///
+/// # Returns
+///
+/// True if the straight path is clear.
+pub fn is_straight_path_clear(
+    from: Point2D,
+    to: Point2D,
+    lines: &[Line2D],
+    robot_radius: f32,
+) -> bool {
+    let test_line = Line2D::new(from, to);
+    let test_length = test_line.length();
+
+    if test_length < 0.001 {
+        return true; // Zero-length path is always clear
+    }
+
+    for line in lines {
+        // Check for direct intersection
+        if test_line.intersection(line).is_some() {
+            return false;
+        }
+
+        // Check clearance along the path (use segment distance, not infinite line)
+        if robot_radius > 0.0 {
+            let num_samples = (test_length / robot_radius).ceil() as usize;
+            let num_samples = num_samples.max(3);
+
+            for k in 0..=num_samples {
+                let t = k as f32 / num_samples as f32;
+                let sample = test_line.point_at(t);
+                // Use distance_to_point_segment for accurate segment distance
+                let dist = line.distance_to_point_segment(sample);
+
+                if dist < robot_radius {
+                    return false;
+                }
+            }
+        }
+    }
+
+    true
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_room() -> Vec<Line2D> {
+        // Simple 10x10 room centered at origin
+        vec![
+            Line2D::new(Point2D::new(-5.0, -5.0), Point2D::new(5.0, -5.0)), // Bottom
+            Line2D::new(Point2D::new(5.0, -5.0), Point2D::new(5.0, 5.0)),   // Right
+            Line2D::new(Point2D::new(5.0, 5.0), Point2D::new(-5.0, 5.0)),   // Top
+            Line2D::new(Point2D::new(-5.0, 5.0), Point2D::new(-5.0, -5.0)), // Left
+        ]
+    }
+
+    fn make_room_with_obstacle() -> Vec<Line2D> {
+        let mut lines = make_room();
+        // Add a vertical wall in the middle
+        lines.push(Line2D::new(Point2D::new(0.0, -3.0), Point2D::new(0.0, 3.0)));
+        lines
+    }
+
+    #[test]
+    fn test_config_default() {
+        let config = PathPlanningConfig::default();
+        assert_eq!(config.robot_radius, 0.15);
+        assert_eq!(config.max_nodes, 1000);
+        assert!(config.conservative);
+    }
+
+    #[test]
+    fn test_config_builder() {
+        let config = PathPlanningConfig::new()
+            .with_robot_radius(0.2)
+            .with_max_nodes(500)
+            .with_conservative(false);
+
+        assert_eq!(config.robot_radius, 0.2);
+        assert_eq!(config.max_nodes, 500);
+        assert!(!config.conservative);
+    }
+
+    #[test]
+    fn test_visibility_graph_empty() {
+        let lines: Vec<Line2D> = vec![];
+        let graph = VisibilityGraph::build(
+            &lines,
+            Point2D::new(0.0, 0.0),
+            Point2D::new(1.0, 1.0),
+            0.0,
+            100,
+        );
+
+        // Should have just start and goal
+        assert_eq!(graph.node_count(), 2);
+        assert_eq!(graph.start_idx(), 0);
+        assert_eq!(graph.goal_idx(), 1);
+    }
+
+    #[test]
+    fn test_visibility_graph_simple() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))];
+
+        let graph = VisibilityGraph::build(
+            &lines,
+            Point2D::new(-1.0, 0.0),
+            Point2D::new(2.0, 0.0),
+            0.0,
+            100,
+        );
+
+        // 2 line endpoints + start + goal = 4 nodes
+        assert_eq!(graph.node_count(), 4);
+    }
+
+    #[test]
+    fn test_find_path_direct() {
+        let lines: Vec<Line2D> = vec![];
+        let graph = VisibilityGraph::build(
+            &lines,
+            Point2D::new(0.0, 0.0),
+            Point2D::new(1.0, 1.0),
+            0.0,
+            100,
+        );
+
+        let path = graph.find_path();
+        assert!(path.is_some());
+        let path = path.unwrap();
+        assert_eq!(path.len(), 2); // Direct path: start -> goal
+    }
+
+    #[test]
+    fn test_find_path_around_obstacle() {
+        let lines = make_room_with_obstacle();
+        let graph = VisibilityGraph::build(
+            &lines,
+            Point2D::new(-3.0, 0.0), // Left of center wall
+            Point2D::new(3.0, 0.0),  // Right of center wall
+            0.1,
+            100,
+        );
+
+        let path = graph.find_path();
+        assert!(path.is_some());
+
+        let indices = path.unwrap();
+        // Should go around the obstacle (more than 2 nodes)
+        assert!(indices.len() >= 2);
+    }
+
+    #[test]
+    fn test_planner_direct_path() {
+        let lines = make_room();
+        let config = PathPlanningConfig::new()
+            .with_robot_radius(0.1)
+            .with_conservative(false);
+        let planner = PathPlanner::new(config);
+        let occupancy_config = OccupancyConfig::default();
+
+        let path = planner.plan(
+            Point2D::new(-2.0, 0.0),
+            Point2D::new(2.0, 0.0),
+            &lines,
+            None,
+            &occupancy_config,
+        );
+
+        assert!(path.is_some());
+        let path = path.unwrap();
+        assert!(!path.is_empty());
+        assert!(path.length > 0.0);
+    }
+
+    #[test]
+    fn test_planner_around_obstacle() {
+        let lines = make_room_with_obstacle();
+        let config = PathPlanningConfig::new()
+            .with_robot_radius(0.1)
+            .with_conservative(false);
+        let planner = PathPlanner::new(config);
+        let occupancy_config = OccupancyConfig::default();
+
+        let path = planner.plan(
+            Point2D::new(-2.0, 0.0),
+            Point2D::new(2.0, 0.0),
+            &lines,
+            None,
+            &occupancy_config,
+        );
+
+        assert!(path.is_some());
+        let path = path.unwrap();
+        // Path should go around the obstacle
+        assert!(path.length > 4.0); // Longer than direct 4m path
+    }
+
+    #[test]
+    fn test_is_straight_path_clear_empty() {
+        let lines: Vec<Line2D> = vec![];
+
+        assert!(is_straight_path_clear(
+            Point2D::new(0.0, 0.0),
+            Point2D::new(1.0, 1.0),
+            &lines,
+            0.0,
+        ));
+    }
+
+    #[test]
+    fn test_is_straight_path_clear_blocked() {
+        let lines = vec![Line2D::new(Point2D::new(0.5, -1.0), Point2D::new(0.5, 1.0))];
+
+        assert!(!is_straight_path_clear(
+            Point2D::new(0.0, 0.0),
+            Point2D::new(1.0, 0.0),
+            &lines,
+            0.0,
+        ));
+    }
+
+    #[test]
+    fn test_is_straight_path_clear_with_radius() {
+        let lines = vec![Line2D::new(Point2D::new(0.5, -1.0), Point2D::new(0.5, 1.0))];
+
+        // Path at y=0.5, parallel to obstacle at x=0.5
+        // Should be blocked due to robot radius
+        assert!(!is_straight_path_clear(
+            Point2D::new(0.0, 0.5),
+            Point2D::new(1.0, 0.5),
+            &lines,
+            0.2, // Robot would clip the wall
+        ));
+    }
+
+    #[test]
+    fn test_is_straight_path_clear_with_clearance() {
+        let lines = vec![Line2D::new(Point2D::new(0.5, -1.0), Point2D::new(0.5, 1.0))];
+
+        // Path at y=2.0, far from obstacle
+        assert!(is_straight_path_clear(
+            Point2D::new(0.0, 2.0),
+            Point2D::new(1.0, 2.0),
+            &lines,
+            0.2,
+        ));
+    }
+
+    #[test]
+    fn test_same_start_goal() {
+        let lines = make_room();
+        let config = PathPlanningConfig::new().with_conservative(false);
+        let planner = PathPlanner::new(config);
+        let occupancy_config = OccupancyConfig::default();
+
+        let path = planner.plan(
+            Point2D::new(0.0, 0.0),
+            Point2D::new(0.0, 0.0),
+            &lines,
+            None,
+            &occupancy_config,
+        );
+
+        // Should return a path with just one point
+        assert!(path.is_some());
+        let path = path.unwrap();
+        assert_eq!(path.points.len(), 1);
+        assert_eq!(path.length, 0.0);
+    }
+}

vastu-map/src/vector_map/config.rs

@@ -4,7 +4,7 @@ use crate::extraction::{CornerConfig, RansacLineConfig, SplitMergeConfig};
 use crate::integration::{AssociationConfig, MergerConfig};
 use crate::loop_closure::LoopClosureConfig;
 use crate::matching::IcpConfig;
-use crate::query::{FrontierConfig, OccupancyConfig};
+use crate::query::{FrontierConfig, OccupancyConfig, PathPlanningConfig};
 
 // ============================================================================
 // Configuration Validation
@@ -116,6 +116,9 @@ pub struct VectorMapConfig {
     /// Configuration for occupancy queries.
     pub occupancy: OccupancyConfig,
 
+    /// Configuration for path planning.
+    pub path_planning: PathPlanningConfig,
+
     /// Configuration for loop closure detection.
     pub loop_closure: LoopClosureConfig,
 
@@ -146,6 +149,7 @@ impl Default for VectorMapConfig {
             merger: MergerConfig::default(),
             frontier: FrontierConfig::default(),
             occupancy: OccupancyConfig::default(),
+            path_planning: PathPlanningConfig::default(),
             loop_closure: LoopClosureConfig::default(),
             min_match_confidence: 0.3,
             mapping_enabled: true,
@@ -300,6 +304,20 @@ impl VectorMapConfig {
         self
     }
 
+    /// Builder-style setter for robot radius (path planning).
+    ///
+    /// Sets the robot radius used for collision checking in path planning.
+    pub fn with_robot_radius(mut self, radius: f32) -> Self {
+        self.path_planning.robot_radius = radius;
+        self
+    }
+
+    /// Builder-style setter for path planning configuration.
+    pub fn with_path_planning(mut self, config: PathPlanningConfig) -> Self {
+        self.path_planning = config;
+        self
+    }
+
     // ===== Configuration Validation =====
 
     /// Validate the configuration parameters.

vastu-map/src/vector_map/map.rs

@@ -10,7 +10,9 @@ use crate::integration::{
 };
 use crate::loop_closure::{LoopClosure, LoopClosureDetector};
 use crate::matching::PointToLineIcp;
-use crate::query::{detect_frontiers, query_occupancy, raycast};
+use crate::query::{
+    PathPlanner, detect_frontiers, is_straight_path_clear, query_occupancy, raycast,
+};
 use crate::{Frontier, Map, ObserveResult, Occupancy, Path, TimingBreakdown};
 
 use super::config::VectorMapConfig;
@@ -558,9 +560,24 @@ impl Map for VectorMap {
         detect_frontiers(self.line_store.lines(), &self.config.frontier)
     }
 
-    fn get_path(&self, _from: Point2D, _to: Point2D) -> Option<Path> {
-        // Path planning not yet implemented
-        None
+    fn get_path(&self, from: Point2D, to: Point2D) -> Option<Path> {
+        let planner = PathPlanner::new(self.config.path_planning.clone());
+        planner.plan(
+            from,
+            to,
+            self.line_store.lines(),
+            self.line_store.bounds(),
+            &self.config.occupancy,
+        )
+    }
+
+    fn is_path_clear(&self, from: Point2D, to: Point2D) -> bool {
+        is_straight_path_clear(
+            from,
+            to,
+            self.line_store.lines(),
+            self.config.path_planning.robot_radius,
+        )
     }
 
     fn bounds(&self) -> Bounds {

vastu-map/tests/integration/harness.rs

@@ -10,6 +10,7 @@ use sangam_io::devices::mock::noise::NoiseGenerator;
 use sangam_io::devices::mock::physics::PhysicsState;
 
 use vastu_map::core::Pose2D;
+use vastu_map::odometry::WheelOdometry;
 use vastu_map::{Map, VectorMap, VectorMapConfig};
 
 use crate::adapters::lidar_to_point_cloud;
@@ -202,6 +203,12 @@ pub struct TestHarness {
     trajectory: TrajectoryHistory,
     /// Algorithm timing statistics across all observations
     timing_stats: TimingStats,
+    /// Wheel odometry tracker using vastu-map's odometry module
+    wheel_odometry: WheelOdometry,
+    /// Simulated left encoder ticks (accumulated)
+    left_encoder_ticks: i32,
+    /// Simulated right encoder ticks (accumulated)
+    right_encoder_ticks: i32,
 }
 
 impl TestHarness {
@@ -229,6 +236,10 @@ impl TestHarness {
 
         let start_pose = Pose2D::new(config.start_x, config.start_y, config.start_theta);
 
+        // Create wheel odometry tracker with robot configuration
+        let wheel_odometry =
+            WheelOdometry::new(config.robot.wheel_base, config.robot.ticks_per_meter);
+
         Ok(Self {
             config,
             map,
@@ -245,6 +256,9 @@ impl TestHarness {
             convergence_stats: ConvergenceStats::new(),
             trajectory: TrajectoryHistory::new(),
             timing_stats: TimingStats::new(),
+            wheel_odometry,
+            left_encoder_ticks: 0,
+            right_encoder_ticks: 0,
         })
     }
 
@@ -272,6 +286,9 @@ impl TestHarness {
             let lidar_steps = (segment.duration / lidar_dt).max(1.0) as usize;
 
             for _ in 0..lidar_steps {
+                // Simulate encoder ticks from wheel velocities
+                self.simulate_encoder_ticks(lidar_dt, segment.linear_vel, segment.angular_vel);
+
                 // Single physics update covering the full lidar interval
                 self.physics.update(
                     lidar_dt,
@@ -289,6 +306,13 @@ impl TestHarness {
             let steps = (segment.duration / self.config.dt) as usize;
 
             for _ in 0..steps {
+                // Simulate encoder ticks from wheel velocities
+                self.simulate_encoder_ticks(
+                    self.config.dt,
+                    segment.linear_vel,
+                    segment.angular_vel,
+                );
+
                 // Update physics
                 self.physics.update(
                     self.config.dt,
@@ -336,22 +360,28 @@ impl TestHarness {
         // Store the robot-frame scan for visualization
         self.last_scan_robot_frame = Some(cloud.clone());
 
-        // Compute relative odometry from ground truth (simplified)
-        // In real tests, this would come from encoder simulation
+        // Get ground truth pose for metrics/visualization
         let current_truth = Pose2D::new(self.physics.x(), self.physics.y(), self.physics.theta());
 
+        // Get odometry from wheel encoders using vastu-map's WheelOdometry module
         let odometry = if self.observations == 0 {
-            // First observation - use absolute pose as initial guess
+            // First observation - initialize odometry and use absolute pose as initial guess
+            // This triggers WheelOdometry initialization with the encoder baseline
+            let _ = self
+                .wheel_odometry
+                .update(self.left_encoder_ticks, self.right_encoder_ticks);
             current_truth
         } else {
-            // Subsequent observations - compute relative pose
-            self.compute_relative_odometry(current_truth)
+            // Subsequent observations - get delta from wheel odometry
+            // WheelOdometry::update returns the delta pose in robot frame
+            self.wheel_odometry
+                .update(self.left_encoder_ticks, self.right_encoder_ticks)
         };
 
         // Feed to SLAM
         let result = self.slam.observe(&cloud, odometry);
         self.last_slam_pose = result.pose;
-        self.last_physics_pose = current_truth; // Update for next odometry computation
+        self.last_physics_pose = current_truth; // Update for metrics computation
         self.observations += 1;
 
         // Record ICP convergence stats
@@ -386,23 +416,31 @@ impl TestHarness {
         );
     }
 
-    /// Compute relative odometry between consecutive ground truth poses.
+    /// Simulate encoder ticks from commanded wheel velocities.
     ///
-    /// Uses physics poses (not SLAM poses) to mimic real-world encoders that
-    /// measure actual wheel motion independent of SLAM corrections.
-    fn compute_relative_odometry(&self, current_truth: Pose2D) -> Pose2D {
-        // Compute delta from previous physics pose (like real encoders)
-        let dx = current_truth.x - self.last_physics_pose.x;
-        let dy = current_truth.y - self.last_physics_pose.y;
-        let dtheta = normalize_angle(current_truth.theta - self.last_physics_pose.theta);
+    /// Computes individual wheel velocities from linear/angular velocity commands
+    /// and integrates them over the timestep to produce encoder tick counts.
+    /// This mimics how real wheel encoders would accumulate ticks.
+    fn simulate_encoder_ticks(&mut self, dt: f32, linear_vel: f32, angular_vel: f32) {
+        // Compute individual wheel velocities from (v, ω) commands
+        // Using differential drive kinematics:
+        //   v_left = v - ω * wheel_base / 2
+        //   v_right = v + ω * wheel_base / 2
+        let (left_vel, right_vel) =
+            self.physics
+                .wheel_velocities(linear_vel, angular_vel, self.config.robot.wheel_base);
 
-        // Transform to robot frame using previous physics pose orientation
-        let cos_th = self.last_physics_pose.theta.cos();
-        let sin_th = self.last_physics_pose.theta.sin();
-        let dx_robot = dx * cos_th + dy * sin_th;
-        let dy_robot = -dx * sin_th + dy * cos_th;
+        // Integrate wheel velocities to get distance traveled per wheel
+        let left_distance = left_vel * dt;
+        let right_distance = right_vel * dt;
 
-        Pose2D::new(dx_robot, dy_robot, dtheta)
+        // Convert distance to encoder ticks
+        let left_delta_ticks = (left_distance * self.config.robot.ticks_per_meter) as i32;
+        let right_delta_ticks = (right_distance * self.config.robot.ticks_per_meter) as i32;
+
+        // Accumulate encoder ticks
+        self.left_encoder_ticks += left_delta_ticks;
+        self.right_encoder_ticks += right_delta_ticks;
     }
 
     /// Finalize the test and compute metrics.
@@ -455,15 +493,3 @@ impl TestHarness {
         &self.physics
     }
 }
-
-/// Normalize angle to [-PI, PI].
-fn normalize_angle(angle: f32) -> f32 {
-    let mut a = angle;
-    while a > std::f32::consts::PI {
-        a -= 2.0 * std::f32::consts::PI;
-    }
-    while a < -std::f32::consts::PI {
-        a += 2.0 * std::f32::consts::PI;
-    }
-    a
-}