Add Clearance-Based Visibility Graph (CBVG) for safe path planning

Replace the corner-based visibility graph with a new CBVG algorithm
that places navigation nodes along the medial axis of free space.

Key changes:
- Add cbvg module with Voronoi-based medial axis approximation
- Nodes placed at safe distances from walls (configurable clearance)
- Adaptive clearance for narrow passages
- Visibility region tracking for explored area awareness
- Integrate CBVG into VectorMap with nearest_cbvg_node() API

PathFollower changes:
- Remove pure pursuit lookahead logic
- Follow CBVG waypoints sequentially for predictable navigation

Configuration:
- New cbvg config section with min_clearance, voronoi_sample_step, etc.
- Remove old path_planning config

This provides safer robot navigation by routing through corridor
centerlines rather than wall corners.

.gitignore

@@ -58,3 +58,5 @@ analysis/
 *.log
 
 results/
+dataset/
+maps/

vastu-map/configs/default.yaml

@@ -169,15 +169,29 @@ occupancy:
   known_distance: 2.0
 
 # ============================================================================
-# Path Planning
+# Clearance-Based Visibility Graph (CBVG) - Path Planning
 # ============================================================================
-path_planning:
-  # Robot radius for collision checking (m)
-  robot_radius: 0.16
-  # Maximum visibility graph nodes
-  max_nodes: 1000
-  # Only plan through known free space
-  conservative: true
+cbvg:
+  # Minimum clearance from walls in open areas (m)
+  min_clearance: 0.3
+  # Minimum clearance in narrow passages (m)
+  # Used when corridor is too narrow for full clearance
+  min_clearance_narrow: 0.18
+  # Sample resolution along Voronoi edges (m)
+  voronoi_sample_step: 0.1
+  # Maximum edge length between nodes (m)
+  max_edge_length: 0.8
+  # Interval for placing coverage nodes in open areas (m)
+  # Ensures nodes exist even in areas without medial axis
+  coverage_node_interval: 0.35
+  # Distance threshold for merging nearby nodes (m)
+  node_merge_distance: 0.15
+  # Maximum nodes in the graph
+  max_nodes: 500
+  # Clearance sample step for edge validation (m)
+  edge_clearance_step: 0.05
+  # Wall sampling step for Voronoi approximation (m)
+  wall_sample_step: 0.1
 
 # ============================================================================
 # Loop Closure Detection

vastu-map/src/exploration/config.rs

@@ -1,7 +1,6 @@
 //! Configuration for exploration behavior.
 
 use crate::query::frontier::FrontierConfig;
-use crate::query::path_planning::PathPlanningConfig;
 
 use super::planner::PlannerConfig;
 
@@ -40,9 +39,6 @@ pub struct ExplorationConfig {
     /// Note: With shadow-based frontiers, viewpoints are already safe observation points.
     pub frontier_config: FrontierConfig,
 
-    /// Path planning configuration.
-    pub path_planning_config: PathPlanningConfig,
-
     /// Region-based exploration planner configuration.
     pub planner_config: PlannerConfig,
 
@@ -66,7 +62,6 @@ impl Default for ExplorationConfig {
             robot_radius: 0.15,
             lookahead_distance: 0.3,
             frontier_config: FrontierConfig::default(),
-            path_planning_config: PathPlanningConfig::default().with_robot_radius(0.15),
             planner_config: PlannerConfig::default(),
             history_cell_size: 0.5,
             history_max_recent: 100,
@@ -113,7 +108,6 @@ impl ExplorationConfig {
     /// Builder-style setter for robot radius.
     pub fn with_robot_radius(mut self, radius: f32) -> Self {
         self.robot_radius = radius;
-        self.path_planning_config = self.path_planning_config.with_robot_radius(radius);
         self
     }
 
@@ -129,12 +123,6 @@ impl ExplorationConfig {
         self
     }
 
-    /// Builder-style setter for path planning config.
-    pub fn with_path_planning_config(mut self, config: PathPlanningConfig) -> Self {
-        self.path_planning_config = config;
-        self
-    }
-
     /// Builder-style setter for planner config.
     pub fn with_planner_config(mut self, config: PlannerConfig) -> Self {
         self.planner_config = config;
@@ -177,6 +165,5 @@ mod tests {
 
         assert_eq!(config.max_linear_speed, 0.5);
         assert_eq!(config.robot_radius, 0.2);
-        assert_eq!(config.path_planning_config.robot_radius, 0.2);
     }
 }

vastu-map/src/exploration/controller.rs

@@ -452,9 +452,16 @@ impl ExplorationController {
                 self.planner
                     .select_frontier_in_region(region, robot_pos, current_pose.theta);
 
-            // With shadow-based frontiers, viewpoint is already a safe observation point
+            // Find nearest CBVG node to the frontier - guaranteed reachable
+            // CBVG nodes are in visible areas with proper wall clearance
             let target = match selected_frontier {
-                Some(f) => f.viewpoint, // Use viewpoint directly - it's already safe!
+                Some(f) => {
+                    // Try to find nearest CBVG node (within 2m of frontier viewpoint)
+                    match map.nearest_cbvg_node(f.viewpoint, 2.0) {
+                        Some(node_pos) => node_pos,
+                        None => f.viewpoint, // Fallback to viewpoint if no node nearby
+                    }
+                }
                 None => {
                     // Current region exhausted, advance to next
                     log::debug!("Region {} exhausted, advancing to next", region.id);
@@ -724,6 +731,11 @@ mod tests {
             })
         }
 
+        fn nearest_cbvg_node(&self, target: Point2D, _max_distance: f32) -> Option<Point2D> {
+            // For testing, just return the target point
+            Some(target)
+        }
+
         fn is_path_clear(&self, _from: Point2D, _to: Point2D) -> bool {
             true
         }

vastu-map/src/exploration/path_follower.rs

@@ -55,16 +55,18 @@ pub struct FollowResult {
     pub distance_to_waypoint: f32,
 }
 
-/// Pure pursuit path follower.
+/// Path follower for waypoint-based navigation.
 ///
-/// Generates velocity commands to follow a path of waypoints using
-/// a simple pure pursuit algorithm:
+/// Generates velocity commands to follow a path of waypoints:
 ///
 /// 1. Transform target waypoint to robot frame
 /// 2. Compute angle to target
 /// 3. Generate proportional angular velocity for steering
 /// 4. Reduce linear velocity when turning sharply
 ///
+/// The follower visits each waypoint in sequence to ensure the robot
+/// follows the planned CBVG path through the medial axis of free space.
+///
 /// # Example
 ///
 /// ```rust,ignore
@@ -84,8 +86,6 @@ pub struct FollowResult {
 /// ```
 #[derive(Clone, Debug)]
 pub struct PathFollower {
-    /// Lookahead distance for pure pursuit.
-    lookahead_distance: f32,
     /// Maximum linear velocity.
     max_linear: f32,
     /// Maximum angular velocity.
@@ -100,7 +100,6 @@ impl PathFollower {
     /// Create a new path follower from exploration config.
     pub fn new(config: &ExplorationConfig) -> Self {
         Self {
-            lookahead_distance: config.lookahead_distance,
             max_linear: config.max_linear_speed,
             max_angular: config.max_angular_speed,
             angular_gain: 2.0, // Responsive steering
@@ -109,9 +108,8 @@ impl PathFollower {
     }
 
     /// Create a path follower with custom parameters.
-    pub fn with_params(lookahead_distance: f32, max_linear: f32, max_angular: f32) -> Self {
+    pub fn with_params(max_linear: f32, max_angular: f32) -> Self {
         Self {
-            lookahead_distance,
             max_linear,
             max_angular,
             angular_gain: 2.0,
@@ -171,9 +169,9 @@ impl PathFollower {
             };
         }
 
-        // Get target waypoint (use lookahead if possible)
-        let target_idx = self.find_lookahead_point(path, current_idx, robot_pos);
-        let target = path.points[target_idx];
+        // Target current waypoint directly (no lookahead skipping)
+        // This ensures the robot follows each CBVG node in sequence
+        let target = path.points[current_idx];
 
         // Transform target to robot frame
         let local_target = current_pose.inverse_transform_point(target);
@@ -193,33 +191,6 @@ impl PathFollower {
         }
     }
 
-    /// Find the lookahead point on the path.
-    fn find_lookahead_point(&self, path: &Path, start_idx: usize, robot_pos: Point2D) -> usize {
-        let mut best_idx = start_idx;
-        let mut accumulated_dist = 0.0;
-
-        for i in start_idx..path.points.len() {
-            let distance_to_point = robot_pos.distance(path.points[i]);
-
-            // If this point is beyond lookahead, use it
-            if distance_to_point >= self.lookahead_distance {
-                return i;
-            }
-
-            // Track accumulated path distance
-            if i > start_idx {
-                accumulated_dist += path.points[i - 1].distance(path.points[i]);
-                if accumulated_dist >= self.lookahead_distance {
-                    return i;
-                }
-            }
-
-            best_idx = i;
-        }
-
-        best_idx
-    }
-
     /// Compute velocity command from angle and distance to target.
     fn compute_velocity(&self, angle_to_target: f32, distance: f32) -> VelocityCommand {
         // Angular velocity: proportional to angle error

vastu-map/src/lib.rs

@@ -389,6 +389,20 @@ pub trait Map: Send + Sync {
     /// Path if one exists, None otherwise.
     fn get_path(&self, from: Point2D, to: Point2D) -> Option<Path>;
 
+    /// Find the nearest CBVG node to a target point.
+    ///
+    /// This is useful for exploration to find reachable navigation targets
+    /// near frontiers. CBVG nodes are guaranteed to be in visible, reachable
+    /// areas with proper wall clearance.
+    ///
+    /// # Arguments
+    /// * `target` - The point to find the nearest node to
+    /// * `max_distance` - Maximum distance to search
+    ///
+    /// # Returns
+    /// The position of the nearest CBVG node, or None if no node is within range.
+    fn nearest_cbvg_node(&self, target: Point2D, max_distance: f32) -> Option<Point2D>;
+
     /// Check if a straight-line path between two points is clear.
     ///
     /// # Arguments

vastu-map/src/query/cbvg/clearance.rs

@@ -0,0 +1,302 @@
+//! Clearance computation utilities for CBVG.
+//!
+//! Provides functions to compute distances from points to walls
+//! and check path clearance.
+
+use crate::core::Point2D;
+use crate::features::Line2D;
+
+/// Compute minimum distance from a point to any wall segment.
+///
+/// # Arguments
+/// * `point` - The query point
+/// * `lines` - Wall segments in the map
+///
+/// # Returns
+/// Minimum distance to any wall, or `f32::INFINITY` if no walls exist.
+#[inline]
+pub fn min_distance_to_walls(point: Point2D, lines: &[Line2D]) -> f32 {
+    lines
+        .iter()
+        .map(|line| line.distance_to_point_segment(point))
+        .min_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal))
+        .unwrap_or(f32::INFINITY)
+}
+
+/// Find the nearest wall to a point.
+///
+/// # Arguments
+/// * `point` - The query point
+/// * `lines` - Wall segments in the map
+///
+/// # Returns
+/// Tuple of (distance, wall_index), or None if no walls exist.
+pub fn nearest_wall(point: Point2D, lines: &[Line2D]) -> Option<(f32, usize)> {
+    lines
+        .iter()
+        .enumerate()
+        .map(|(idx, line)| (line.distance_to_point_segment(point), idx))
+        .min_by(|(a, _), (b, _)| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal))
+}
+
+/// Check if a straight path between two points has sufficient clearance.
+///
+/// Samples points along the path and verifies each has minimum clearance.
+///
+/// # Arguments
+/// * `from` - Start position
+/// * `to` - End position
+/// * `lines` - Wall segments
+/// * `min_clearance` - Required minimum distance from walls
+/// * `sample_step` - Distance between sample points
+///
+/// # Returns
+/// True if the entire path has sufficient clearance.
+pub fn is_path_clear_with_clearance(
+    from: Point2D,
+    to: Point2D,
+    lines: &[Line2D],
+    min_clearance: f32,
+    sample_step: f32,
+) -> bool {
+    let segment = Line2D::new(from, to);
+    let length = segment.length();
+
+    if length < 0.001 {
+        // Zero-length path - just check the point
+        return min_distance_to_walls(from, lines) >= min_clearance;
+    }
+
+    // Sample at regular intervals
+    let step = sample_step.min(min_clearance * 0.5);
+    let num_samples = (length / step).ceil() as usize;
+    let num_samples = num_samples.max(2); // At least check start and end
+
+    for k in 0..=num_samples {
+        let t = k as f32 / num_samples as f32;
+        let sample = segment.point_at(t);
+
+        if min_distance_to_walls(sample, lines) < min_clearance {
+            return false;
+        }
+    }
+
+    true
+}
+
+/// Check if an edge between two nodes is safe for robot traversal.
+///
+/// Uses adaptive clearance: full clearance in open areas,
+/// reduced clearance in narrow passages.
+///
+/// # Arguments
+/// * `from` - Start position
+/// * `to` - End position
+/// * `lines` - Wall segments
+/// * `min_clearance` - Full clearance for open areas
+/// * `min_clearance_narrow` - Reduced clearance for narrow passages
+/// * `sample_step` - Distance between sample points
+///
+/// # Returns
+/// True if the edge is safe for traversal.
+pub fn is_edge_safe(
+    from: Point2D,
+    to: Point2D,
+    lines: &[Line2D],
+    min_clearance: f32,
+    min_clearance_narrow: f32,
+    sample_step: f32,
+) -> bool {
+    let segment = Line2D::new(from, to);
+    let length = segment.length();
+
+    if length < 0.001 {
+        return min_distance_to_walls(from, lines) >= min_clearance_narrow;
+    }
+
+    let num_samples = (length / sample_step).ceil() as usize;
+    let num_samples = num_samples.max(2);
+
+    for k in 0..=num_samples {
+        let t = k as f32 / num_samples as f32;
+        let sample = segment.point_at(t);
+
+        let clearance = min_distance_to_walls(sample, lines);
+
+        // Adaptive clearance: use narrow clearance if we're in a tight spot
+        let required = if clearance < min_clearance {
+            min_clearance_narrow
+        } else {
+            min_clearance
+        };
+
+        if clearance < required {
+            return false;
+        }
+    }
+
+    true
+}
+
+/// Sample points along a line segment at regular intervals.
+///
+/// # Arguments
+/// * `line` - The line segment to sample
+/// * `step` - Distance between samples
+///
+/// # Returns
+/// Vector of sampled points including start and end.
+pub fn sample_line(line: &Line2D, step: f32) -> Vec<Point2D> {
+    let length = line.length();
+
+    if length < step {
+        return vec![line.start, line.end];
+    }
+
+    let num_samples = (length / step).ceil() as usize;
+    let mut points = Vec::with_capacity(num_samples + 1);
+
+    for i in 0..=num_samples {
+        let t = i as f32 / num_samples as f32;
+        points.push(line.point_at(t));
+    }
+
+    points
+}
+
+/// Find the closest point on any wall to the given point.
+///
+/// # Arguments
+/// * `point` - The query point
+/// * `lines` - Wall segments
+///
+/// # Returns
+/// The closest point on any wall, or the input point if no walls exist.
+pub fn closest_point_on_walls(point: Point2D, lines: &[Line2D]) -> Point2D {
+    lines
+        .iter()
+        .map(|line| {
+            let t = line.project_point(point).clamp(0.0, 1.0);
+            let projected = line.point_at(t);
+            let dist = point.distance(projected);
+            (projected, dist)
+        })
+        .min_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal))
+        .map(|(p, _)| p)
+        .unwrap_or(point)
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_test_walls() -> Vec<Line2D> {
+        vec![
+            // Horizontal wall at y=2
+            Line2D::new(Point2D::new(-5.0, 2.0), Point2D::new(5.0, 2.0)),
+            // Vertical wall at x=3
+            Line2D::new(Point2D::new(3.0, -2.0), Point2D::new(3.0, 2.0)),
+        ]
+    }
+
+    #[test]
+    fn test_min_distance_to_walls() {
+        let walls = make_test_walls();
+
+        // Point at origin
+        let dist = min_distance_to_walls(Point2D::new(0.0, 0.0), &walls);
+        assert!((dist - 2.0).abs() < 0.01); // 2m from horizontal wall
+
+        // Point near vertical wall
+        let dist = min_distance_to_walls(Point2D::new(2.5, 0.0), &walls);
+        assert!((dist - 0.5).abs() < 0.01); // 0.5m from vertical wall
+
+        // Empty walls
+        let dist = min_distance_to_walls(Point2D::new(0.0, 0.0), &[]);
+        assert!(dist.is_infinite());
+    }
+
+    #[test]
+    fn test_nearest_wall() {
+        let walls = make_test_walls();
+
+        let result = nearest_wall(Point2D::new(0.0, 0.0), &walls);
+        assert!(result.is_some());
+        let (dist, idx) = result.unwrap();
+        assert!((dist - 2.0).abs() < 0.01);
+        assert_eq!(idx, 0); // Horizontal wall is closer
+
+        let result = nearest_wall(Point2D::new(0.0, 0.0), &[]);
+        assert!(result.is_none());
+    }
+
+    #[test]
+    fn test_is_path_clear_with_clearance() {
+        let walls = make_test_walls();
+
+        // Path in open area (far from walls)
+        assert!(is_path_clear_with_clearance(
+            Point2D::new(-2.0, 0.0),
+            Point2D::new(0.0, 0.0),
+            &walls,
+            0.3,
+            0.1
+        ));
+
+        // Path too close to wall
+        assert!(!is_path_clear_with_clearance(
+            Point2D::new(-2.0, 1.9),
+            Point2D::new(2.0, 1.9),
+            &walls,
+            0.3,
+            0.1
+        ));
+    }
+
+    #[test]
+    fn test_is_edge_safe() {
+        let walls = make_test_walls();
+
+        // Safe edge in open area
+        assert!(is_edge_safe(
+            Point2D::new(-2.0, 0.0),
+            Point2D::new(0.0, 0.0),
+            &walls,
+            0.3,
+            0.18,
+            0.05
+        ));
+
+        // Edge too close to wall
+        assert!(!is_edge_safe(
+            Point2D::new(-2.0, 1.95),
+            Point2D::new(2.0, 1.95),
+            &walls,
+            0.3,
+            0.18,
+            0.05
+        ));
+    }
+
+    #[test]
+    fn test_sample_line() {
+        let line = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0));
+
+        let samples = sample_line(&line, 0.25);
+        assert_eq!(samples.len(), 5); // 0, 0.25, 0.5, 0.75, 1.0
+
+        // Short line
+        let short = Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(0.1, 0.0));
+        let samples = sample_line(&short, 0.5);
+        assert_eq!(samples.len(), 2); // Just start and end
+    }
+
+    #[test]
+    fn test_closest_point_on_walls() {
+        let walls = make_test_walls();
+
+        let closest = closest_point_on_walls(Point2D::new(0.0, 3.0), &walls);
+        assert!((closest.x - 0.0).abs() < 0.01);
+        assert!((closest.y - 2.0).abs() < 0.01); // Closest to horizontal wall
+    }
+}

vastu-map/src/query/cbvg/config.rs

@@ -0,0 +1,185 @@
+//! Configuration for Clearance-Based Visibility Graph.
+
+use serde::{Deserialize, Serialize};
+
+/// Configuration for clearance-based visibility graph path planning.
+///
+/// The CBVG places nodes along the medial axis of free space, ensuring
+/// all waypoints are at a safe distance from obstacles.
+#[derive(Clone, Debug, Serialize, Deserialize)]
+pub struct CBVGConfig {
+    /// Minimum clearance from walls in open areas (meters).
+    /// Nodes will be placed at least this distance from any wall.
+    /// Default: 0.3
+    pub min_clearance: f32,
+
+    /// Minimum clearance in narrow passages (meters).
+    /// Used when corridor is too narrow for full clearance.
+    /// Default: 0.18 (robot_radius + 0.02)
+    pub min_clearance_narrow: f32,
+
+    /// Sample resolution along Voronoi edges (meters).
+    /// Controls density of nodes along the medial axis.
+    /// Default: 0.1
+    pub voronoi_sample_step: f32,
+
+    /// Maximum edge length between nodes (meters).
+    /// Longer edges are split with intermediate nodes.
+    /// Default: 0.8
+    pub max_edge_length: f32,
+
+    /// Interval for placing coverage nodes in open areas (meters).
+    /// Ensures nodes exist even in areas without medial axis.
+    /// Default: 0.5
+    pub coverage_node_interval: f32,
+
+    /// Distance threshold for merging nearby nodes (meters).
+    /// Nodes closer than this are merged to reduce graph size.
+    /// Default: 0.15
+    pub node_merge_distance: f32,
+
+    /// Maximum nodes in the graph.
+    /// Limits computation for very complex maps.
+    /// Default: 500
+    pub max_nodes: usize,
+
+    /// Clearance sample step for edge validation (meters).
+    /// Points are sampled along edges to verify clearance.
+    /// Default: 0.05
+    pub edge_clearance_step: f32,
+
+    /// Wall sampling step for Voronoi approximation (meters).
+    /// Points are sampled along walls for Voronoi computation.
+    /// Default: 0.1
+    pub wall_sample_step: f32,
+}
+
+impl Default for CBVGConfig {
+    fn default() -> Self {
+        Self {
+            min_clearance: 0.3,
+            min_clearance_narrow: 0.18,
+            voronoi_sample_step: 0.1,
+            max_edge_length: 0.8,
+            coverage_node_interval: 0.35,
+            node_merge_distance: 0.15,
+            max_nodes: 500,
+            edge_clearance_step: 0.05,
+            wall_sample_step: 0.1,
+        }
+    }
+}
+
+impl CBVGConfig {
+    /// Create a new configuration with default values.
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    /// Builder-style setter for minimum clearance.
+    pub fn with_min_clearance(mut self, clearance: f32) -> Self {
+        self.min_clearance = clearance;
+        self
+    }
+
+    /// Builder-style setter for narrow passage clearance.
+    pub fn with_min_clearance_narrow(mut self, clearance: f32) -> Self {
+        self.min_clearance_narrow = clearance;
+        self
+    }
+
+    /// Builder-style setter for Voronoi sample step.
+    pub fn with_voronoi_sample_step(mut self, step: f32) -> Self {
+        self.voronoi_sample_step = step;
+        self
+    }
+
+    /// Builder-style setter for maximum edge length.
+    pub fn with_max_edge_length(mut self, length: f32) -> Self {
+        self.max_edge_length = length;
+        self
+    }
+
+    /// Builder-style setter for coverage node interval.
+    pub fn with_coverage_node_interval(mut self, interval: f32) -> Self {
+        self.coverage_node_interval = interval;
+        self
+    }
+
+    /// Builder-style setter for node merge distance.
+    pub fn with_node_merge_distance(mut self, distance: f32) -> Self {
+        self.node_merge_distance = distance;
+        self
+    }
+
+    /// Builder-style setter for maximum nodes.
+    pub fn with_max_nodes(mut self, max: usize) -> Self {
+        self.max_nodes = max;
+        self
+    }
+
+    /// Builder-style setter for edge clearance step.
+    pub fn with_edge_clearance_step(mut self, step: f32) -> Self {
+        self.edge_clearance_step = step;
+        self
+    }
+
+    /// Builder-style setter for wall sample step.
+    pub fn with_wall_sample_step(mut self, step: f32) -> Self {
+        self.wall_sample_step = step;
+        self
+    }
+
+    /// Get the effective clearance for a given measured clearance.
+    ///
+    /// Returns `min_clearance` for open areas, or `min_clearance_narrow`
+    /// for narrow passages where full clearance isn't achievable.
+    #[inline]
+    pub fn effective_clearance(&self, measured_clearance: f32) -> f32 {
+        if measured_clearance >= self.min_clearance {
+            self.min_clearance
+        } else {
+            self.min_clearance_narrow
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_default_config() {
+        let config = CBVGConfig::default();
+        assert_eq!(config.min_clearance, 0.3);
+        assert_eq!(config.min_clearance_narrow, 0.18);
+        assert_eq!(config.voronoi_sample_step, 0.1);
+        assert_eq!(config.max_edge_length, 0.8);
+        assert_eq!(config.coverage_node_interval, 0.35);
+        assert_eq!(config.node_merge_distance, 0.15);
+        assert_eq!(config.max_nodes, 500);
+        assert_eq!(config.edge_clearance_step, 0.05);
+        assert_eq!(config.wall_sample_step, 0.1);
+    }
+
+    #[test]
+    fn test_builder_pattern() {
+        let config = CBVGConfig::new()
+            .with_min_clearance(0.25)
+            .with_max_nodes(1000);
+
+        assert_eq!(config.min_clearance, 0.25);
+        assert_eq!(config.max_nodes, 1000);
+    }
+
+    #[test]
+    fn test_effective_clearance() {
+        let config = CBVGConfig::default();
+
+        // Open area - use full clearance
+        assert_eq!(config.effective_clearance(0.5), 0.3);
+
+        // Narrow passage - use reduced clearance
+        assert_eq!(config.effective_clearance(0.2), 0.18);
+    }
+}

vastu-map/src/query/cbvg/dijkstra.rs

@@ -0,0 +1,360 @@
+//! Dijkstra's shortest path algorithm for CBVG.
+
+use std::cmp::Ordering;
+use std::collections::BinaryHeap;
+
+/// State for Dijkstra's algorithm priority queue.
+#[derive(Clone, Copy, Debug)]
+pub struct DijkstraState {
+    /// Current path cost (distance).
+    pub cost: f32,
+    /// Current node index.
+    pub node: usize,
+}
+
+impl PartialEq for DijkstraState {
+    fn eq(&self, other: &Self) -> bool {
+        self.cost == other.cost && self.node == other.node
+    }
+}
+
+impl Eq for DijkstraState {}
+
+impl Ord for DijkstraState {
+    fn cmp(&self, other: &Self) -> Ordering {
+        // Reverse ordering for min-heap (BinaryHeap is max-heap by default)
+        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))
+    }
+}
+
+/// Result of a Dijkstra search.
+#[derive(Clone, Debug)]
+pub struct DijkstraResult {
+    /// Sequence of node indices from start to goal.
+    pub path: Vec<usize>,
+    /// Total path distance.
+    pub distance: f32,
+}
+
+/// Find shortest path using Dijkstra's algorithm.
+///
+/// # Arguments
+/// * `edges` - Adjacency list: edges[i] = [(neighbor_idx, distance), ...]
+/// * `start` - Start node index
+/// * `goal` - Goal node index
+///
+/// # Returns
+/// DijkstraResult with path and distance, or None if no path exists.
+pub fn dijkstra(edges: &[Vec<(usize, f32)>], start: usize, goal: usize) -> Option<DijkstraResult> {
+    let n = edges.len();
+
+    if n == 0 || start >= n || goal >= n {
+        return None;
+    }
+
+    // Handle same start and goal
+    if start == goal {
+        return Some(DijkstraResult {
+            path: vec![start],
+            distance: 0.0,
+        });
+    }
+
+    // Distance to each node
+    let mut dist: Vec<f32> = vec![f32::INFINITY; n];
+    let mut prev: Vec<Option<usize>> = vec![None; n];
+    dist[start] = 0.0;
+
+    // Priority queue
+    let mut heap = BinaryHeap::new();
+    heap.push(DijkstraState {
+        cost: 0.0,
+        node: start,
+    });
+
+    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 == goal {
+            break;
+        }
+
+        // Explore neighbors
+        for &(neighbor, edge_dist) in &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,
+                });
+            }
+        }
+    }
+
+    // Check if goal is reachable
+    if prev[goal].is_none() && start != goal {
+        return None;
+    }
+
+    // Reconstruct path
+    let mut path = Vec::new();
+    let mut current = goal;
+
+    while current != start {
+        path.push(current);
+        match prev[current] {
+            Some(p) => current = p,
+            None => return None, // No path
+        }
+    }
+    path.push(start);
+    path.reverse();
+
+    Some(DijkstraResult {
+        path,
+        distance: dist[goal],
+    })
+}
+
+/// Find shortest path with early termination if goal is found within max_distance.
+///
+/// Same as `dijkstra` but stops early if the goal is found.
+pub fn dijkstra_with_limit(
+    edges: &[Vec<(usize, f32)>],
+    start: usize,
+    goal: usize,
+    max_distance: f32,
+) -> Option<DijkstraResult> {
+    let n = edges.len();
+
+    if n == 0 || start >= n || goal >= n {
+        return None;
+    }
+
+    if start == goal {
+        return Some(DijkstraResult {
+            path: vec![start],
+            distance: 0.0,
+        });
+    }
+
+    let mut dist: Vec<f32> = vec![f32::INFINITY; n];
+    let mut prev: Vec<Option<usize>> = vec![None; n];
+    dist[start] = 0.0;
+
+    let mut heap = BinaryHeap::new();
+    heap.push(DijkstraState {
+        cost: 0.0,
+        node: start,
+    });
+
+    while let Some(DijkstraState { cost, node }) = heap.pop() {
+        // Early termination if exceeded max distance
+        if cost > max_distance {
+            break;
+        }
+
+        if cost > dist[node] {
+            continue;
+        }
+
+        if node == goal {
+            // Reconstruct path
+            let mut path = Vec::new();
+            let mut current = goal;
+            while current != start {
+                path.push(current);
+                current = prev[current]?;
+            }
+            path.push(start);
+            path.reverse();
+
+            return Some(DijkstraResult {
+                path,
+                distance: cost,
+            });
+        }
+
+        for &(neighbor, edge_dist) in &edges[node] {
+            let new_dist = dist[node] + edge_dist;
+            if new_dist < dist[neighbor] && new_dist <= max_distance {
+                dist[neighbor] = new_dist;
+                prev[neighbor] = Some(node);
+                heap.push(DijkstraState {
+                    cost: new_dist,
+                    node: neighbor,
+                });
+            }
+        }
+    }
+
+    None
+}
+
+/// Compute distances from a source node to all reachable nodes.
+///
+/// # Arguments
+/// * `edges` - Adjacency list
+/// * `source` - Source node index
+///
+/// # Returns
+/// Vector of distances (f32::INFINITY for unreachable nodes).
+pub fn dijkstra_distances(edges: &[Vec<(usize, f32)>], source: usize) -> Vec<f32> {
+    let n = edges.len();
+
+    if n == 0 || source >= n {
+        return vec![f32::INFINITY; n];
+    }
+
+    let mut dist: Vec<f32> = vec![f32::INFINITY; n];
+    dist[source] = 0.0;
+
+    let mut heap = BinaryHeap::new();
+    heap.push(DijkstraState {
+        cost: 0.0,
+        node: source,
+    });
+
+    while let Some(DijkstraState { cost, node }) = heap.pop() {
+        if cost > dist[node] {
+            continue;
+        }
+
+        for &(neighbor, edge_dist) in &edges[node] {
+            let new_dist = dist[node] + edge_dist;
+            if new_dist < dist[neighbor] {
+                dist[neighbor] = new_dist;
+                heap.push(DijkstraState {
+                    cost: new_dist,
+                    node: neighbor,
+                });
+            }
+        }
+    }
+
+    dist
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_simple_graph() -> Vec<Vec<(usize, f32)>> {
+        // Graph:
+        // 0 --1.0-- 1 --1.0-- 2
+        // |         |
+        // 2.0       1.5
+        // |         |
+        // 3 --1.0-- 4
+        vec![
+            vec![(1, 1.0), (3, 2.0)],           // Node 0
+            vec![(0, 1.0), (2, 1.0), (4, 1.5)], // Node 1
+            vec![(1, 1.0)],                     // Node 2
+            vec![(0, 2.0), (4, 1.0)],           // Node 3
+            vec![(1, 1.5), (3, 1.0)],           // Node 4
+        ]
+    }
+
+    #[test]
+    fn test_dijkstra_simple() {
+        let edges = make_simple_graph();
+
+        // Direct path
+        let result = dijkstra(&edges, 0, 1).unwrap();
+        assert_eq!(result.path, vec![0, 1]);
+        assert!((result.distance - 1.0).abs() < 0.01);
+
+        // Path through intermediate node
+        let result = dijkstra(&edges, 0, 2).unwrap();
+        assert_eq!(result.path, vec![0, 1, 2]);
+        assert!((result.distance - 2.0).abs() < 0.01);
+    }
+
+    #[test]
+    fn test_dijkstra_same_node() {
+        let edges = make_simple_graph();
+        let result = dijkstra(&edges, 0, 0).unwrap();
+        assert_eq!(result.path, vec![0]);
+        assert_eq!(result.distance, 0.0);
+    }
+
+    #[test]
+    fn test_dijkstra_no_path() {
+        // Disconnected graph
+        let edges: Vec<Vec<(usize, f32)>> = vec![
+            vec![(1, 1.0)], // Node 0 -> 1
+            vec![(0, 1.0)], // Node 1 -> 0
+            vec![],         // Node 2 (isolated)
+        ];
+
+        let result = dijkstra(&edges, 0, 2);
+        assert!(result.is_none());
+    }
+
+    #[test]
+    fn test_dijkstra_with_limit() {
+        let edges = make_simple_graph();
+
+        // Within limit
+        let result = dijkstra_with_limit(&edges, 0, 1, 5.0).unwrap();
+        assert_eq!(result.path, vec![0, 1]);
+
+        // Exceeds limit
+        let result = dijkstra_with_limit(&edges, 0, 2, 1.5);
+        assert!(result.is_none());
+    }
+
+    #[test]
+    fn test_dijkstra_distances() {
+        let edges = make_simple_graph();
+        let dist = dijkstra_distances(&edges, 0);
+
+        assert!((dist[0] - 0.0).abs() < 0.01);
+        assert!((dist[1] - 1.0).abs() < 0.01);
+        assert!((dist[2] - 2.0).abs() < 0.01);
+        assert!((dist[3] - 2.0).abs() < 0.01);
+        assert!((dist[4] - 2.5).abs() < 0.01);
+    }
+
+    #[test]
+    fn test_dijkstra_shortest_path() {
+        let edges = make_simple_graph();
+
+        // 0 -> 4: Can go 0->1->4 (2.5) or 0->3->4 (3.0)
+        let result = dijkstra(&edges, 0, 4).unwrap();
+        assert_eq!(result.path, vec![0, 1, 4]);
+        assert!((result.distance - 2.5).abs() < 0.01);
+    }
+
+    #[test]
+    fn test_dijkstra_empty_graph() {
+        let edges: Vec<Vec<(usize, f32)>> = vec![];
+        let result = dijkstra(&edges, 0, 1);
+        assert!(result.is_none());
+    }
+
+    #[test]
+    fn test_dijkstra_state_ordering() {
+        // Test that lower cost has higher priority
+        let state1 = DijkstraState { cost: 1.0, node: 0 };
+        let state2 = DijkstraState { cost: 2.0, node: 1 };
+
+        // state1 should come before state2 (lower cost = higher priority)
+        assert!(state1 > state2);
+    }
+}

vastu-map/src/query/cbvg/graph.rs

@@ -0,0 +1,817 @@
+//! Clearance-Based Visibility Graph implementation.
+//!
+//! The main graph structure that maintains nodes along the medial axis
+//! and edges connecting them for path planning.
+
+use crate::Path;
+use crate::core::{Bounds, Point2D};
+use crate::features::Line2D;
+
+use super::clearance::{is_edge_safe, is_path_clear_with_clearance, min_distance_to_walls};
+use super::config::CBVGConfig;
+use super::dijkstra::{DijkstraResult, dijkstra};
+use super::node::CBVGNode;
+use super::visibility::VisibilityRegion;
+use super::voronoi::{generate_medial_axis_nodes, generate_nodes_from_visibility};
+
+/// Clearance-Based Visibility Graph for safe robot navigation.
+///
+/// The CBVG places nodes along the medial axis of free space, ensuring
+/// all waypoints are at a safe distance from obstacles. This provides
+/// safer paths compared to traditional visibility graphs that place
+/// nodes at obstacle corners.
+///
+/// # Example
+///
+/// ```rust,ignore
+/// use vastu_map::query::cbvg::{ClearanceVisibilityGraph, CBVGConfig};
+/// use vastu_map::core::Point2D;
+///
+/// let config = CBVGConfig::default();
+/// let mut graph = ClearanceVisibilityGraph::new(config);
+///
+/// // Build graph from walls
+/// graph.build(&walls, Some(&bounds));
+///
+/// // Find path
+/// if let Some(path) = graph.find_path(start, goal, &walls) {
+///     println!("Path found: {} waypoints", path.points.len());
+/// }
+/// ```
+#[derive(Clone, Debug)]
+pub struct ClearanceVisibilityGraph {
+    /// All nodes in the graph.
+    nodes: Vec<CBVGNode>,
+
+    /// Adjacency list: edges[i] = [(neighbor_idx, distance), ...]
+    edges: Vec<Vec<(usize, f32)>>,
+
+    /// Configuration.
+    config: CBVGConfig,
+}
+
+impl ClearanceVisibilityGraph {
+    /// Create a new empty graph with the given configuration.
+    pub fn new(config: CBVGConfig) -> Self {
+        Self {
+            nodes: Vec::new(),
+            edges: Vec::new(),
+            config,
+        }
+    }
+
+    /// Build the graph from wall segments.
+    ///
+    /// This clears any existing nodes and rebuilds the entire graph
+    /// by computing the medial axis of free space.
+    ///
+    /// # Arguments
+    /// * `lines` - Wall segments
+    /// * `bounds` - Optional map bounds (computed from lines if not provided)
+    pub fn build(&mut self, lines: &[Line2D], bounds: Option<&Bounds>) {
+        self.nodes.clear();
+        self.edges.clear();
+
+        if lines.is_empty() {
+            return;
+        }
+
+        // Generate nodes along the medial axis
+        self.nodes = generate_medial_axis_nodes(lines, bounds, &self.config);
+
+        // Build edges between nodes
+        self.build_edges(lines);
+    }
+
+    /// Build the graph from a visibility region (lidar-scanned areas).
+    ///
+    /// This only creates nodes in areas that have been observed by lidar,
+    /// ensuring the robot doesn't plan paths through unexplored territory.
+    ///
+    /// # Arguments
+    /// * `visibility` - Visibility region tracking scanned areas
+    /// * `lines` - Wall segments for clearance computation
+    pub fn build_from_visibility(&mut self, visibility: &VisibilityRegion, lines: &[Line2D]) {
+        self.nodes.clear();
+        self.edges.clear();
+
+        if visibility.is_empty() || lines.is_empty() {
+            return;
+        }
+
+        // Generate nodes only in visible (scanned) areas
+        self.nodes = generate_nodes_from_visibility(visibility, lines, &self.config);
+
+        // Build edges between nodes
+        self.build_edges(lines);
+    }
+
+    /// Incrementally update the graph with new visibility data.
+    ///
+    /// This adds nodes in newly visible areas without regenerating
+    /// the entire graph.
+    ///
+    /// # Arguments
+    /// * `visibility` - Updated visibility region
+    /// * `lines` - All wall segments
+    pub fn update_from_visibility(&mut self, visibility: &VisibilityRegion, lines: &[Line2D]) {
+        if visibility.is_empty() {
+            return;
+        }
+
+        // 1. Invalidate nodes that are no longer valid (too close to walls)
+        let mut invalid_indices: Vec<usize> = Vec::new();
+        for (idx, node) in self.nodes.iter().enumerate() {
+            let clearance = min_distance_to_walls(node.position, lines);
+            if clearance < self.config.min_clearance_narrow {
+                invalid_indices.push(idx);
+            }
+        }
+
+        // Remove invalid nodes (in reverse order)
+        for idx in invalid_indices.into_iter().rev() {
+            self.remove_node(idx);
+        }
+
+        // 2. Generate new nodes from visibility
+        let new_nodes = generate_nodes_from_visibility(visibility, lines, &self.config);
+
+        // 3. Add new nodes that don't already exist
+        for node in new_nodes {
+            if !self.has_nearby_node(node.position) {
+                self.add_node(node, lines);
+            }
+        }
+    }
+
+    /// Build edges between all nodes that can be safely connected.
+    fn build_edges(&mut self, lines: &[Line2D]) {
+        let n = self.nodes.len();
+        self.edges = vec![Vec::new(); n];
+
+        for i in 0..n {
+            for j in (i + 1)..n {
+                let dist = self.nodes[i].position.distance(self.nodes[j].position);
+
+                // Skip if too far
+                if dist > self.config.max_edge_length {
+                    continue;
+                }
+
+                // Check if edge is safe
+                if is_edge_safe(
+                    self.nodes[i].position,
+                    self.nodes[j].position,
+                    lines,
+                    self.config.min_clearance,
+                    self.config.min_clearance_narrow,
+                    self.config.edge_clearance_step,
+                ) {
+                    self.edges[i].push((j, dist));
+                    self.edges[j].push((i, dist));
+                }
+            }
+        }
+    }
+
+    /// Update the graph after new walls are added.
+    ///
+    /// This performs an incremental update:
+    /// 1. Invalidates nodes too close to new walls
+    /// 2. Generates new nodes in the updated space
+    /// 3. Rebuilds affected edges
+    ///
+    /// # Arguments
+    /// * `all_lines` - All wall segments in the map
+    /// * `new_lines` - Only the newly added wall segments
+    pub fn update(&mut self, all_lines: &[Line2D], new_lines: &[Line2D]) {
+        if new_lines.is_empty() {
+            return;
+        }
+
+        // 1. Invalidate nodes too close to new walls
+        let mut invalid_indices: Vec<usize> = Vec::new();
+        for (idx, node) in self.nodes.iter().enumerate() {
+            for line in new_lines {
+                let dist = line.distance_to_point_segment(node.position);
+                if dist < self.config.min_clearance_narrow {
+                    invalid_indices.push(idx);
+                    break;
+                }
+            }
+        }
+
+        // Remove invalid nodes (in reverse order to preserve indices)
+        for idx in invalid_indices.into_iter().rev() {
+            self.remove_node(idx);
+        }
+
+        // 2. Generate new nodes
+        let new_nodes = generate_medial_axis_nodes(all_lines, None, &self.config);
+
+        // 3. Add new nodes, avoiding duplicates
+        for node in new_nodes {
+            if !self.has_nearby_node(node.position) {
+                self.add_node(node, all_lines);
+            }
+        }
+    }
+
+    /// Remove a node from the graph.
+    fn remove_node(&mut self, idx: usize) {
+        if idx >= self.nodes.len() {
+            return;
+        }
+
+        // Remove from nodes
+        self.nodes.remove(idx);
+
+        // Remove from edges and update indices
+        self.edges.remove(idx);
+        for neighbors in &mut self.edges {
+            neighbors.retain(|(neighbor_idx, _)| *neighbor_idx != idx);
+            for (neighbor_idx, _) in neighbors.iter_mut() {
+                if *neighbor_idx > idx {
+                    *neighbor_idx -= 1;
+                }
+            }
+        }
+    }
+
+    /// Add a node to the graph and connect it to nearby nodes.
+    fn add_node(&mut self, node: CBVGNode, lines: &[Line2D]) {
+        let new_idx = self.nodes.len();
+        self.nodes.push(node);
+        self.edges.push(Vec::new());
+
+        // Connect to existing nodes
+        for i in 0..new_idx {
+            let dist = self.nodes[i]
+                .position
+                .distance(self.nodes[new_idx].position);
+
+            if dist <= self.config.max_edge_length
+                && is_edge_safe(
+                    self.nodes[i].position,
+                    self.nodes[new_idx].position,
+                    lines,
+                    self.config.min_clearance,
+                    self.config.min_clearance_narrow,
+                    self.config.edge_clearance_step,
+                )
+            {
+                self.edges[i].push((new_idx, dist));
+                self.edges[new_idx].push((i, dist));
+            }
+        }
+    }
+
+    /// Check if there's a node near the given position.
+    fn has_nearby_node(&self, position: Point2D) -> bool {
+        self.nodes
+            .iter()
+            .any(|n| n.position.distance(position) < self.config.node_merge_distance)
+    }
+
+    /// Find a path from start to goal.
+    ///
+    /// Always routes through CBVG nodes for safe navigation with proper
+    /// wall clearance. This ensures predictable paths that follow the
+    /// medial axis of free space.
+    ///
+    /// # Arguments
+    /// * `from` - Start position
+    /// * `to` - Goal position
+    /// * `lines` - Wall segments (for clearance checks)
+    ///
+    /// # Returns
+    /// Path if one exists, None otherwise.
+    pub fn find_path(&self, from: Point2D, to: Point2D, lines: &[Line2D]) -> Option<Path> {
+        // Always route through CBVG nodes for safe navigation
+
+        // 1. Find nearest reachable node to start
+        let start_node = self.find_nearest_reachable(from, lines)?;
+
+        // 2. Find nearest reachable node to goal
+        let goal_node = self.find_nearest_reachable(to, lines)?;
+
+        // 3. If same node, just go from -> node -> to
+        if start_node == goal_node {
+            let node_pos = self.nodes[start_node].position;
+            let len1 = from.distance(node_pos);
+            let len2 = node_pos.distance(to);
+            return Some(Path {
+                points: vec![from, node_pos, to],
+                length: len1 + len2,
+            });
+        }
+
+        // 4. Dijkstra search through CBVG nodes
+        let result = dijkstra(&self.edges, start_node, goal_node)?;
+
+        // 5. Build full path
+        self.build_path(from, to, &result)
+    }
+
+    /// Find the nearest node reachable from a point.
+    fn find_nearest_reachable(&self, point: Point2D, lines: &[Line2D]) -> Option<usize> {
+        self.nodes
+            .iter()
+            .enumerate()
+            .filter(|(_, node)| {
+                is_path_clear_with_clearance(
+                    point,
+                    node.position,
+                    lines,
+                    self.config.min_clearance_narrow,
+                    self.config.edge_clearance_step,
+                )
+            })
+            .min_by(|(_, a), (_, b)| {
+                point
+                    .distance(a.position)
+                    .partial_cmp(&point.distance(b.position))
+                    .unwrap_or(std::cmp::Ordering::Equal)
+            })
+            .map(|(idx, _)| idx)
+    }
+
+    /// Build the final path from Dijkstra result.
+    fn build_path(&self, from: Point2D, to: Point2D, result: &DijkstraResult) -> Option<Path> {
+        let mut points = Vec::with_capacity(result.path.len() + 2);
+        let mut length = 0.0;
+
+        points.push(from);
+
+        for (i, &idx) in result.path.iter().enumerate() {
+            let node_pos = self.nodes[idx].position;
+
+            if i == 0 {
+                length += from.distance(node_pos);
+            } else {
+                length += points.last()?.distance(node_pos);
+            }
+
+            points.push(node_pos);
+        }
+
+        length += points.last()?.distance(to);
+        points.push(to);
+
+        Some(Path { points, length })
+    }
+
+    /// Get all nodes in the graph.
+    pub fn nodes(&self) -> &[CBVGNode] {
+        &self.nodes
+    }
+
+    /// Get the number of nodes.
+    pub fn node_count(&self) -> usize {
+        self.nodes.len()
+    }
+
+    /// Get the number of edges.
+    pub fn edge_count(&self) -> usize {
+        self.edges.iter().map(|e| e.len()).sum::<usize>() / 2
+    }
+
+    /// Get the adjacency list.
+    pub fn edges(&self) -> &[Vec<(usize, f32)>] {
+        &self.edges
+    }
+
+    /// Get the configuration.
+    pub fn config(&self) -> &CBVGConfig {
+        &self.config
+    }
+
+    /// Check if the graph is empty.
+    pub fn is_empty(&self) -> bool {
+        self.nodes.is_empty()
+    }
+
+    /// Find the nearest node to a target point.
+    ///
+    /// Returns the node position if found within max_distance.
+    /// This is useful for exploration to find reachable navigation targets.
+    ///
+    /// # Arguments
+    /// * `target` - The point to find the nearest node to
+    /// * `max_distance` - Maximum distance to search (nodes farther are ignored)
+    ///
+    /// # Returns
+    /// The position of the nearest node, or None if no node is within range.
+    pub fn nearest_node(&self, target: Point2D, max_distance: f32) -> Option<Point2D> {
+        self.nodes
+            .iter()
+            .filter(|n| n.position.distance(target) <= max_distance)
+            .min_by(|a, b| {
+                a.position
+                    .distance(target)
+                    .partial_cmp(&b.position.distance(target))
+                    .unwrap_or(std::cmp::Ordering::Equal)
+            })
+            .map(|n| n.position)
+    }
+
+    /// Clear all nodes and edges.
+    pub fn clear(&mut self) {
+        self.nodes.clear();
+        self.edges.clear();
+    }
+
+    /// Export graph to SVG for visualization.
+    ///
+    /// # Arguments
+    /// * `lines` - Wall segments to draw
+    /// * `path` - Optional path to highlight
+    ///
+    /// # Returns
+    /// SVG string that can be written to a file.
+    pub fn to_svg(&self, lines: &[Line2D], path: Option<&Path>) -> String {
+        // Calculate bounds
+        let (min_x, min_y, max_x, max_y) = self.calculate_svg_bounds(lines);
+
+        let width = max_x - min_x;
+        let height = max_y - min_y;
+        let margin = 0.5;
+
+        let view_min_x = min_x - margin;
+        let view_min_y = min_y - margin;
+        let view_width = width + 2.0 * margin;
+        let view_height = height + 2.0 * margin;
+
+        let mut svg = format!(
+            r#"<?xml version="1.0" encoding="UTF-8"?>
+<svg xmlns="http://www.w3.org/2000/svg" viewBox="{} {} {} {}" width="800" height="600">
+  <rect x="{}" y="{}" width="{}" height="{}" fill="white"/>
+  <g transform="scale(1, -1) translate(0, -{})">
+"#,
+            view_min_x,
+            -view_min_y - view_height,
+            view_width,
+            view_height,
+            view_min_x,
+            -view_min_y - view_height,
+            view_width,
+            view_height,
+            2.0 * view_min_y + view_height
+        );
+
+        // Draw walls (black)
+        svg.push_str("    <!-- Walls -->\n");
+        for line in lines {
+            svg.push_str(&format!(
+                r#"    <line x1="{:.3}" y1="{:.3}" x2="{:.3}" y2="{:.3}" stroke="black" stroke-width="0.03"/>"#,
+                line.start.x, line.start.y, line.end.x, line.end.y
+            ));
+            svg.push('\n');
+        }
+
+        // Draw edges (light gray)
+        svg.push_str("    <!-- Graph Edges -->\n");
+        for (i, neighbors) in self.edges.iter().enumerate() {
+            for (j, _) in neighbors {
+                if i < *j {
+                    let p1 = self.nodes[i].position;
+                    let p2 = self.nodes[*j].position;
+                    svg.push_str(&format!(
+                        "    <line x1=\"{:.3}\" y1=\"{:.3}\" x2=\"{:.3}\" y2=\"{:.3}\" stroke=\"#cccccc\" stroke-width=\"0.01\"/>\n",
+                        p1.x, p1.y, p2.x, p2.y
+                    ));
+                }
+            }
+        }
+
+        // Draw path if provided (thick green)
+        if let Some(path) = path {
+            svg.push_str("    <!-- Path -->\n");
+            for i in 0..path.points.len().saturating_sub(1) {
+                let p1 = path.points[i];
+                let p2 = path.points[i + 1];
+                svg.push_str(&format!(
+                    r#"    <line x1="{:.3}" y1="{:.3}" x2="{:.3}" y2="{:.3}" stroke="lime" stroke-width="0.05"/>"#,
+                    p1.x, p1.y, p2.x, p2.y
+                ));
+                svg.push('\n');
+            }
+        }
+
+        // Draw nodes
+        svg.push_str("    <!-- Graph Nodes -->\n");
+        for node in &self.nodes {
+            let color = node.svg_color();
+            let radius = if node.is_temporary() { 0.08 } else { 0.05 };
+            svg.push_str(&format!(
+                r#"    <circle cx="{:.3}" cy="{:.3}" r="{}" fill="{}"/>"#,
+                node.position.x, node.position.y, radius, color
+            ));
+            svg.push('\n');
+        }
+
+        svg.push_str("  </g>\n</svg>\n");
+        svg
+    }
+
+    /// Calculate bounds for SVG export.
+    fn calculate_svg_bounds(&self, lines: &[Line2D]) -> (f32, f32, f32, f32) {
+        let mut min_x = f32::INFINITY;
+        let mut min_y = f32::INFINITY;
+        let mut max_x = f32::NEG_INFINITY;
+        let mut max_y = f32::NEG_INFINITY;
+
+        for line in lines {
+            min_x = min_x.min(line.start.x).min(line.end.x);
+            min_y = min_y.min(line.start.y).min(line.end.y);
+            max_x = max_x.max(line.start.x).max(line.end.x);
+            max_y = max_y.max(line.start.y).max(line.end.y);
+        }
+
+        for node in &self.nodes {
+            min_x = min_x.min(node.position.x);
+            min_y = min_y.min(node.position.y);
+            max_x = max_x.max(node.position.x);
+            max_y = max_y.max(node.position.y);
+        }
+
+        if min_x.is_infinite() {
+            (0.0, 0.0, 1.0, 1.0)
+        } else {
+            (min_x, min_y, max_x, max_y)
+        }
+    }
+
+    /// Add a temporary node for the robot's current position.
+    ///
+    /// Returns the index of the added node.
+    pub fn add_robot_node(&mut self, position: Point2D, lines: &[Line2D]) -> usize {
+        let clearance = min_distance_to_walls(position, lines);
+        let node = CBVGNode::robot(position, clearance);
+        self.add_node(node, lines);
+        self.nodes.len() - 1
+    }
+
+    /// Add a temporary node for a goal position.
+    ///
+    /// Returns the index of the added node.
+    pub fn add_goal_node(&mut self, position: Point2D, lines: &[Line2D]) -> usize {
+        let clearance = min_distance_to_walls(position, lines);
+        let node = CBVGNode::goal(position, clearance);
+        self.add_node(node, lines);
+        self.nodes.len() - 1
+    }
+
+    /// Remove all temporary nodes (robot and goal nodes).
+    pub fn remove_temporary_nodes(&mut self) {
+        let mut indices_to_remove: Vec<usize> = self
+            .nodes
+            .iter()
+            .enumerate()
+            .filter(|(_, n)| n.is_temporary())
+            .map(|(i, _)| i)
+            .collect();
+
+        // Remove in reverse order
+        indices_to_remove.sort_by(|a, b| b.cmp(a));
+        for idx in indices_to_remove {
+            self.remove_node(idx);
+        }
+    }
+}
+
+impl Default for ClearanceVisibilityGraph {
+    fn default() -> Self {
+        Self::new(CBVGConfig::default())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_room() -> Vec<Line2D> {
+        vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(4.0, 0.0)), // Bottom
+            Line2D::new(Point2D::new(4.0, 0.0), Point2D::new(4.0, 3.0)), // Right
+            Line2D::new(Point2D::new(4.0, 3.0), Point2D::new(0.0, 3.0)), // Top
+            Line2D::new(Point2D::new(0.0, 3.0), Point2D::new(0.0, 0.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(2.0, 0.0), Point2D::new(2.0, 2.0)));
+        lines
+    }
+
+    #[test]
+    fn test_build_empty() {
+        let mut graph = ClearanceVisibilityGraph::default();
+        graph.build(&[], None);
+        assert!(graph.is_empty());
+    }
+
+    #[test]
+    fn test_build_simple_room() {
+        let walls = make_room();
+        let config = CBVGConfig::default()
+            .with_voronoi_sample_step(0.2)
+            .with_min_clearance(0.3)
+            .with_min_clearance_narrow(0.18);
+
+        let mut graph = ClearanceVisibilityGraph::new(config);
+        graph.build(&walls, None);
+
+        // Should have some nodes
+        assert!(!graph.is_empty());
+
+        // All nodes should have sufficient clearance
+        for node in graph.nodes() {
+            assert!(node.clearance >= 0.18);
+        }
+    }
+
+    #[test]
+    fn test_find_path_direct() {
+        let walls = make_room();
+        let config = CBVGConfig::default()
+            .with_voronoi_sample_step(0.2)
+            .with_min_clearance(0.2)
+            .with_min_clearance_narrow(0.1);
+
+        let mut graph = ClearanceVisibilityGraph::new(config);
+        graph.build(&walls, None);
+
+        // Direct path in open space
+        let path = graph.find_path(Point2D::new(1.0, 1.5), Point2D::new(3.0, 1.5), &walls);
+
+        assert!(path.is_some());
+        let path = path.unwrap();
+        assert!(!path.is_empty());
+        assert!(path.length > 0.0);
+    }
+
+    #[test]
+    fn test_find_path_around_obstacle() {
+        let walls = make_room_with_obstacle();
+        let config = CBVGConfig::default()
+            .with_voronoi_sample_step(0.15)
+            .with_min_clearance(0.2)
+            .with_min_clearance_narrow(0.1);
+
+        let mut graph = ClearanceVisibilityGraph::new(config);
+        graph.build(&walls, None);
+
+        // Path that must go around obstacle
+        let path = graph.find_path(Point2D::new(1.0, 1.0), Point2D::new(3.0, 1.0), &walls);
+
+        // Path should exist and go around
+        if let Some(path) = path {
+            assert!(path.points.len() >= 2);
+            // Path should be longer than direct
+            assert!(path.length > 2.0);
+        }
+    }
+
+    #[test]
+    fn test_add_remove_temporary_nodes() {
+        let walls = make_room();
+        let mut graph = ClearanceVisibilityGraph::default();
+        graph.build(&walls, None);
+
+        let initial_count = graph.node_count();
+
+        // Add robot node
+        let robot_idx = graph.add_robot_node(Point2D::new(1.0, 1.0), &walls);
+        assert_eq!(graph.node_count(), initial_count + 1);
+        assert!(graph.nodes()[robot_idx].is_temporary());
+
+        // Add goal node
+        graph.add_goal_node(Point2D::new(3.0, 2.0), &walls);
+        assert_eq!(graph.node_count(), initial_count + 2);
+
+        // Remove temporary nodes
+        graph.remove_temporary_nodes();
+        assert_eq!(graph.node_count(), initial_count);
+    }
+
+    #[test]
+    fn test_svg_export() {
+        let walls = make_room();
+        let mut graph = ClearanceVisibilityGraph::default();
+        graph.build(&walls, None);
+
+        let svg = graph.to_svg(&walls, None);
+        assert!(svg.contains("<svg"));
+        assert!(svg.contains("</svg>"));
+        assert!(svg.contains("circle")); // Nodes
+        assert!(svg.contains("line")); // Walls/edges
+    }
+
+    #[test]
+    fn test_update_incremental() {
+        let walls = make_room();
+        let config = CBVGConfig::default().with_voronoi_sample_step(0.2);
+
+        let mut graph = ClearanceVisibilityGraph::new(config);
+        graph.build(&walls, None);
+
+        let _initial_count = graph.node_count();
+
+        // Add a new wall
+        let new_wall = vec![Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(2.0, 1.5))];
+
+        let mut all_walls = walls.clone();
+        all_walls.extend(new_wall.iter().cloned());
+
+        graph.update(&all_walls, &new_wall);
+
+        // Graph should be updated (nodes may change)
+        // Just verify it doesn't crash and has nodes
+        assert!(graph.node_count() > 0);
+    }
+
+    #[test]
+    fn test_build_from_visibility() {
+        use crate::core::Pose2D;
+
+        let walls = make_room();
+        let config = CBVGConfig::default()
+            .with_coverage_node_interval(0.3)
+            .with_min_clearance(0.3)
+            .with_min_clearance_narrow(0.18);
+
+        // Create visibility region with scan from center of room
+        let mut visibility = VisibilityRegion::new(0.1);
+        let robot_pose = Pose2D::new(2.0, 1.5, 0.0);
+
+        // Simulate a circular scan
+        let scan_points: Vec<Point2D> = (0..36)
+            .map(|i| {
+                let angle = i as f32 * std::f32::consts::PI / 18.0;
+                let dist = 1.5;
+                robot_pose.transform_point(Point2D::from_polar(angle, dist))
+            })
+            .collect();
+
+        visibility.update_from_scan(robot_pose, &scan_points);
+
+        let mut graph = ClearanceVisibilityGraph::new(config);
+        graph.build_from_visibility(&visibility, &walls);
+
+        // Should have nodes
+        assert!(!graph.is_empty());
+
+        // All nodes should be in visible area
+        for node in graph.nodes() {
+            assert!(
+                visibility.is_visible(node.position),
+                "Node at {:?} is not in visible area",
+                node.position
+            );
+        }
+    }
+
+    #[test]
+    fn test_update_from_visibility() {
+        use crate::core::Pose2D;
+
+        let walls = make_room();
+        let config = CBVGConfig::default()
+            .with_coverage_node_interval(0.3)
+            .with_min_clearance(0.3);
+
+        // Initial visibility from one corner
+        let mut visibility = VisibilityRegion::new(0.1);
+        let pose1 = Pose2D::new(1.0, 1.0, 0.0);
+        let scan1: Vec<Point2D> = (0..36)
+            .map(|i| {
+                let angle = i as f32 * std::f32::consts::PI / 18.0;
+                pose1.transform_point(Point2D::from_polar(angle, 1.0))
+            })
+            .collect();
+        visibility.update_from_scan(pose1, &scan1);
+
+        let mut graph = ClearanceVisibilityGraph::new(config);
+        graph.build_from_visibility(&visibility, &walls);
+        let initial_count = graph.node_count();
+
+        // Add second scan from different location
+        let pose2 = Pose2D::new(3.0, 2.0, 0.0);
+        let scan2: Vec<Point2D> = (0..36)
+            .map(|i| {
+                let angle = i as f32 * std::f32::consts::PI / 18.0;
+                pose2.transform_point(Point2D::from_polar(angle, 1.0))
+            })
+            .collect();
+        visibility.update_from_scan(pose2, &scan2);
+
+        // Update graph with new visibility
+        graph.update_from_visibility(&visibility, &walls);
+
+        // Should have more nodes now (or at least the same)
+        assert!(graph.node_count() >= initial_count);
+    }
+}

vastu-map/src/query/cbvg/mod.rs

@@ -0,0 +1,83 @@
+//! Clearance-Based Visibility Graph (CBVG) path planning.
+//!
+//! This module implements a path planning algorithm that places graph nodes
+//! along the **medial axis** (centerline) of free space, ensuring the robot
+//! navigates through safe, traversable positions rather than wall corners.
+//!
+//! # Key Features
+//!
+//! - **Safe waypoints**: All nodes are at minimum distance from obstacles
+//! - **Medial axis**: Nodes follow the centerline of corridors
+//! - **Adaptive clearance**: Reduced clearance in narrow passages
+//! - **Incremental updates**: Graph updates as map grows
+//! - **SVG visualization**: Export graph for debugging
+//!
+//! # Example
+//!
+//! ```rust,ignore
+//! use vastu_map::query::cbvg::{ClearanceVisibilityGraph, CBVGConfig};
+//! use vastu_map::core::Point2D;
+//!
+//! // Create and configure graph
+//! let config = CBVGConfig::default()
+//!     .with_min_clearance(0.3)
+//!     .with_min_clearance_narrow(0.18);
+//!
+//! let mut graph = ClearanceVisibilityGraph::new(config);
+//!
+//! // Build from wall segments
+//! graph.build(&walls, Some(&bounds));
+//!
+//! // Find path
+//! let start = Point2D::new(1.0, 1.0);
+//! let goal = Point2D::new(5.0, 3.0);
+//!
+//! if let Some(path) = graph.find_path(start, goal, &walls) {
+//!     println!("Path found: {} waypoints, {:.2}m", path.points.len(), path.length);
+//! }
+//!
+//! // Export for visualization
+//! let svg = graph.to_svg(&walls, Some(&path));
+//! std::fs::write("path.svg", svg).unwrap();
+//! ```
+//!
+//! # Algorithm Overview
+//!
+//! 1. **Node generation**: Sample points along the medial axis (Voronoi-based)
+//! 2. **Edge construction**: Connect nodes with clearance-checked edges
+//! 3. **Path planning**: Dijkstra's algorithm on the graph
+//! 4. **Final approach**: Direct navigation from last node to goal
+//!
+//! # Configuration
+//!
+//! Key parameters in [`CBVGConfig`]:
+//!
+//! - `min_clearance`: Minimum distance from walls (default: 0.3m)
+//! - `min_clearance_narrow`: Reduced clearance for tight passages (default: 0.18m)
+//! - `max_edge_length`: Maximum distance between connected nodes (default: 2.0m)
+//! - `voronoi_sample_step`: Density of medial axis sampling (default: 0.1m)
+
+pub mod clearance;
+pub mod config;
+pub mod dijkstra;
+pub mod graph;
+pub mod node;
+pub mod visibility;
+pub mod voronoi;
+
+// Re-export main types
+pub use clearance::{
+    closest_point_on_walls, is_edge_safe, is_path_clear_with_clearance, min_distance_to_walls,
+    nearest_wall, sample_line,
+};
+pub use config::CBVGConfig;
+pub use dijkstra::{
+    DijkstraResult, DijkstraState, dijkstra, dijkstra_distances, dijkstra_with_limit,
+};
+pub use graph::ClearanceVisibilityGraph;
+pub use node::{CBVGNode, NodeType};
+pub use visibility::VisibilityRegion;
+pub use voronoi::{
+    VoronoiEdge, compute_voronoi_edges, generate_medial_axis_nodes, generate_nodes_from_visibility,
+    generate_nodes_hybrid, sample_medial_axis_nodes, sample_wall_points,
+};

vastu-map/src/query/cbvg/node.rs

@@ -0,0 +1,167 @@
+//! Node types for Clearance-Based Visibility Graph.
+
+use crate::core::Point2D;
+
+/// Type of CBVG node for debugging and visualization.
+#[derive(Clone, Copy, Debug, PartialEq, Eq)]
+pub enum NodeType {
+    /// Node on Voronoi edge (medial axis of free space).
+    MedialAxis,
+    /// Node at Voronoi vertex (corridor junction).
+    Junction,
+    /// Temporary node for robot's current position.
+    Robot,
+    /// Temporary node for goal position.
+    Goal,
+}
+
+/// A node in the clearance-based visibility graph.
+///
+/// Each node represents a safe waypoint that the robot can travel through,
+/// with guaranteed minimum clearance from all obstacles.
+#[derive(Clone, Debug)]
+pub struct CBVGNode {
+    /// Position in map coordinates (meters).
+    pub position: Point2D,
+
+    /// Clearance from nearest wall (meters).
+    /// This is the distance to the closest obstacle.
+    pub clearance: f32,
+
+    /// Node type for debugging and visualization.
+    pub node_type: NodeType,
+
+    /// Whether this node is in a narrow passage.
+    /// True if clearance < min_clearance but >= min_clearance_narrow.
+    pub is_narrow_passage: bool,
+}
+
+impl CBVGNode {
+    /// Create a new CBVG node.
+    #[inline]
+    pub fn new(position: Point2D, clearance: f32, node_type: NodeType) -> Self {
+        Self {
+            position,
+            clearance,
+            node_type,
+            is_narrow_passage: false,
+        }
+    }
+
+    /// Create a medial axis node.
+    #[inline]
+    pub fn medial_axis(position: Point2D, clearance: f32, is_narrow: bool) -> Self {
+        Self {
+            position,
+            clearance,
+            node_type: NodeType::MedialAxis,
+            is_narrow_passage: is_narrow,
+        }
+    }
+
+    /// Create a junction node.
+    #[inline]
+    pub fn junction(position: Point2D, clearance: f32) -> Self {
+        Self {
+            position,
+            clearance,
+            node_type: NodeType::Junction,
+            is_narrow_passage: false,
+        }
+    }
+
+    /// Create a robot position node.
+    #[inline]
+    pub fn robot(position: Point2D, clearance: f32) -> Self {
+        Self {
+            position,
+            clearance,
+            node_type: NodeType::Robot,
+            is_narrow_passage: false,
+        }
+    }
+
+    /// Create a goal position node.
+    #[inline]
+    pub fn goal(position: Point2D, clearance: f32) -> Self {
+        Self {
+            position,
+            clearance,
+            node_type: NodeType::Goal,
+            is_narrow_passage: false,
+        }
+    }
+
+    /// Check if this is a temporary node (robot or goal).
+    #[inline]
+    pub fn is_temporary(&self) -> bool {
+        matches!(self.node_type, NodeType::Robot | NodeType::Goal)
+    }
+
+    /// Get the color for SVG visualization.
+    pub fn svg_color(&self) -> &'static str {
+        match self.node_type {
+            NodeType::MedialAxis => {
+                if self.is_narrow_passage {
+                    "orange"
+                } else {
+                    "blue"
+                }
+            }
+            NodeType::Junction => "green",
+            NodeType::Robot => "red",
+            NodeType::Goal => "purple",
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_node_creation() {
+        let node = CBVGNode::new(Point2D::new(1.0, 2.0), 0.5, NodeType::MedialAxis);
+        assert_eq!(node.position.x, 1.0);
+        assert_eq!(node.position.y, 2.0);
+        assert_eq!(node.clearance, 0.5);
+        assert_eq!(node.node_type, NodeType::MedialAxis);
+        assert!(!node.is_narrow_passage);
+    }
+
+    #[test]
+    fn test_medial_axis_node() {
+        let node = CBVGNode::medial_axis(Point2D::new(0.0, 0.0), 0.2, true);
+        assert_eq!(node.node_type, NodeType::MedialAxis);
+        assert!(node.is_narrow_passage);
+    }
+
+    #[test]
+    fn test_temporary_nodes() {
+        let robot = CBVGNode::robot(Point2D::zero(), 0.3);
+        let goal = CBVGNode::goal(Point2D::zero(), 0.3);
+        let axis = CBVGNode::medial_axis(Point2D::zero(), 0.3, false);
+
+        assert!(robot.is_temporary());
+        assert!(goal.is_temporary());
+        assert!(!axis.is_temporary());
+    }
+
+    #[test]
+    fn test_svg_colors() {
+        assert_eq!(
+            CBVGNode::medial_axis(Point2D::zero(), 0.3, false).svg_color(),
+            "blue"
+        );
+        assert_eq!(
+            CBVGNode::medial_axis(Point2D::zero(), 0.2, true).svg_color(),
+            "orange"
+        );
+        assert_eq!(
+            CBVGNode::junction(Point2D::zero(), 0.3).svg_color(),
+            "green"
+        );
+        assert_eq!(CBVGNode::robot(Point2D::zero(), 0.3).svg_color(), "red");
+        assert_eq!(CBVGNode::goal(Point2D::zero(), 0.3).svg_color(), "purple");
+    }
+}

vastu-map/src/query/cbvg/visibility.rs

@@ -0,0 +1,469 @@
+//! Visibility region tracking for scan-based node generation.
+//!
+//! This module tracks areas that have been observed by the lidar scanner,
+//! ensuring CBVG nodes are only placed in regions the robot has actually seen.
+//!
+//! # Algorithm
+//!
+//! For each lidar scan:
+//! 1. For each ray from robot position to scan point
+//! 2. Mark all grid cells along the ray as visible
+//! 3. The endpoint (scan point) marks the boundary
+//!
+//! This creates a conservative estimate of free space - areas the lidar
+//! has actually "seen through".
+
+use crate::core::{Bounds, Point2D, Pose2D};
+
+/// Grid-based visibility tracking.
+///
+/// Tracks which areas of the map have been observed by lidar scans.
+/// Uses a grid representation for efficient lookup.
+#[derive(Clone, Debug)]
+pub struct VisibilityRegion {
+    /// 2D grid of visibility: true = visible, false = unknown.
+    /// Indexed as grid[y_cell][x_cell].
+    grid: Vec<Vec<bool>>,
+
+    /// Resolution of the visibility grid (meters per cell).
+    resolution: f32,
+
+    /// Origin of the grid in world coordinates (min corner).
+    origin: Point2D,
+
+    /// Grid dimensions.
+    width: usize,
+    height: usize,
+
+    /// Current bounds of visible area.
+    bounds: Bounds,
+}
+
+impl VisibilityRegion {
+    /// Create a new visibility region with the given resolution.
+    ///
+    /// # Arguments
+    /// * `resolution` - Grid cell size in meters (default: 0.1)
+    pub fn new(resolution: f32) -> Self {
+        Self {
+            grid: Vec::new(),
+            resolution,
+            origin: Point2D::zero(),
+            width: 0,
+            height: 0,
+            bounds: Bounds::empty(),
+        }
+    }
+
+    /// Create a visibility region with default resolution (0.1m).
+    pub fn default_resolution() -> Self {
+        Self::new(0.1)
+    }
+
+    /// Get the current bounds of the visible region.
+    pub fn visible_bounds(&self) -> Bounds {
+        self.bounds
+    }
+
+    /// Check if the visibility region is empty (no observations yet).
+    pub fn is_empty(&self) -> bool {
+        self.grid.is_empty()
+    }
+
+    /// Check if a point is in a visible (scanned) area.
+    ///
+    /// Returns true if the point has been observed by lidar.
+    pub fn is_visible(&self, point: Point2D) -> bool {
+        if self.grid.is_empty() {
+            return false;
+        }
+
+        // Convert to grid coordinates
+        let (x_cell, y_cell) = self.world_to_grid(point);
+
+        // Check bounds
+        if x_cell < 0 || y_cell < 0 {
+            return false;
+        }
+        let x_cell = x_cell as usize;
+        let y_cell = y_cell as usize;
+
+        if x_cell >= self.width || y_cell >= self.height {
+            return false;
+        }
+
+        self.grid[y_cell][x_cell]
+    }
+
+    /// Update visibility from a lidar scan.
+    ///
+    /// Marks all cells along rays from robot to scan points as visible.
+    ///
+    /// # Arguments
+    /// * `robot_pose` - Current robot pose (position and heading)
+    /// * `scan_points` - Lidar scan points in world coordinates
+    pub fn update_from_scan(&mut self, robot_pose: Pose2D, scan_points: &[Point2D]) {
+        if scan_points.is_empty() {
+            return;
+        }
+
+        let robot_pos = robot_pose.position();
+
+        // First, ensure grid covers all scan points plus robot position
+        self.ensure_coverage(robot_pos, scan_points);
+
+        // Mark rays from robot to each scan point
+        for &scan_point in scan_points {
+            self.mark_ray_visible(robot_pos, scan_point);
+        }
+    }
+
+    /// Update visibility from scan points already in robot-local frame.
+    ///
+    /// # Arguments
+    /// * `robot_pose` - Current robot pose for transforming points
+    /// * `local_points` - Points in robot-local frame
+    pub fn update_from_local_scan(&mut self, robot_pose: Pose2D, local_points: &[Point2D]) {
+        if local_points.is_empty() {
+            return;
+        }
+
+        // Transform points to world frame
+        let world_points: Vec<Point2D> = local_points
+            .iter()
+            .map(|&p| robot_pose.transform_point(p))
+            .collect();
+
+        self.update_from_scan(robot_pose, &world_points);
+    }
+
+    /// Ensure the grid covers all given points.
+    fn ensure_coverage(&mut self, robot_pos: Point2D, scan_points: &[Point2D]) {
+        // Calculate required bounds
+        let mut required_bounds = Bounds::from_point(robot_pos);
+        for &point in scan_points {
+            required_bounds.expand_to_include(point);
+        }
+
+        // Add margin (one cell)
+        required_bounds = required_bounds.expand(self.resolution);
+
+        if self.grid.is_empty() {
+            // Initialize grid
+            self.initialize_grid(required_bounds);
+        } else if !self.bounds_contain(&required_bounds) {
+            // Expand grid
+            self.expand_grid(required_bounds);
+        }
+
+        // Update visible bounds
+        self.bounds = self.bounds.union(&required_bounds);
+    }
+
+    /// Check if current grid bounds contain the required bounds.
+    fn bounds_contain(&self, required: &Bounds) -> bool {
+        let grid_min = self.origin;
+        let grid_max = Point2D::new(
+            self.origin.x + self.width as f32 * self.resolution,
+            self.origin.y + self.height as f32 * self.resolution,
+        );
+
+        required.min.x >= grid_min.x
+            && required.min.y >= grid_min.y
+            && required.max.x <= grid_max.x
+            && required.max.y <= grid_max.y
+    }
+
+    /// Initialize the grid with the given bounds.
+    fn initialize_grid(&mut self, bounds: Bounds) {
+        self.origin = bounds.min;
+        self.width = ((bounds.width() / self.resolution).ceil() as usize).max(1);
+        self.height = ((bounds.height() / self.resolution).ceil() as usize).max(1);
+
+        // Create grid filled with false (not visible)
+        self.grid = vec![vec![false; self.width]; self.height];
+    }
+
+    /// Expand the grid to cover new bounds while preserving existing data.
+    fn expand_grid(&mut self, new_bounds: Bounds) {
+        // Calculate new grid bounds (union of current and required)
+        let current_max = Point2D::new(
+            self.origin.x + self.width as f32 * self.resolution,
+            self.origin.y + self.height as f32 * self.resolution,
+        );
+        let current_bounds = Bounds::new(self.origin, current_max);
+        let expanded = current_bounds.union(&new_bounds);
+
+        let new_origin = expanded.min;
+        let new_width = ((expanded.width() / self.resolution).ceil() as usize).max(1);
+        let new_height = ((expanded.height() / self.resolution).ceil() as usize).max(1);
+
+        // Create new grid
+        let mut new_grid = vec![vec![false; new_width]; new_height];
+
+        // Copy existing data
+        let offset_x = ((self.origin.x - new_origin.x) / self.resolution).round() as isize;
+        let offset_y = ((self.origin.y - new_origin.y) / self.resolution).round() as isize;
+
+        for (old_y, row) in self.grid.iter().enumerate() {
+            let new_y = old_y as isize + offset_y;
+            if new_y >= 0 && (new_y as usize) < new_height {
+                for (old_x, &visible) in row.iter().enumerate() {
+                    let new_x = old_x as isize + offset_x;
+                    if new_x >= 0 && (new_x as usize) < new_width && visible {
+                        new_grid[new_y as usize][new_x as usize] = true;
+                    }
+                }
+            }
+        }
+
+        // Update state
+        self.origin = new_origin;
+        self.width = new_width;
+        self.height = new_height;
+        self.grid = new_grid;
+    }
+
+    /// Mark all cells along a ray from start to end as visible.
+    fn mark_ray_visible(&mut self, start: Point2D, end: Point2D) {
+        // Use Bresenham-like line drawing
+        let (x0, y0) = self.world_to_grid(start);
+        let (x1, y1) = self.world_to_grid(end);
+
+        // Simple DDA line algorithm
+        let dx = (x1 - x0).abs();
+        let dy = (y1 - y0).abs();
+        let steps = dx.max(dy);
+
+        if steps == 0 {
+            self.mark_cell_visible(x0, y0);
+            return;
+        }
+
+        let x_step = (x1 - x0) as f32 / steps as f32;
+        let y_step = (y1 - y0) as f32 / steps as f32;
+
+        let mut x = x0 as f32;
+        let mut y = y0 as f32;
+
+        for _ in 0..=steps {
+            self.mark_cell_visible(x.round() as i32, y.round() as i32);
+            x += x_step;
+            y += y_step;
+        }
+    }
+
+    /// Mark a single cell as visible.
+    fn mark_cell_visible(&mut self, x_cell: i32, y_cell: i32) {
+        if x_cell < 0 || y_cell < 0 {
+            return;
+        }
+        let x_cell = x_cell as usize;
+        let y_cell = y_cell as usize;
+
+        if x_cell < self.width && y_cell < self.height {
+            self.grid[y_cell][x_cell] = true;
+        }
+    }
+
+    /// Convert world coordinates to grid cell indices.
+    fn world_to_grid(&self, point: Point2D) -> (i32, i32) {
+        let x = ((point.x - self.origin.x) / self.resolution).floor() as i32;
+        let y = ((point.y - self.origin.y) / self.resolution).floor() as i32;
+        (x, y)
+    }
+
+    /// Get the grid resolution in meters.
+    pub fn resolution(&self) -> f32 {
+        self.resolution
+    }
+
+    /// Get grid dimensions (width, height) in cells.
+    pub fn grid_size(&self) -> (usize, usize) {
+        (self.width, self.height)
+    }
+
+    /// Count the number of visible cells.
+    pub fn visible_cell_count(&self) -> usize {
+        self.grid
+            .iter()
+            .map(|row| row.iter().filter(|&&v| v).count())
+            .sum()
+    }
+
+    /// Get total cell count.
+    pub fn total_cell_count(&self) -> usize {
+        self.width * self.height
+    }
+
+    /// Get the visibility ratio (visible cells / total cells).
+    pub fn visibility_ratio(&self) -> f32 {
+        if self.total_cell_count() == 0 {
+            return 0.0;
+        }
+        self.visible_cell_count() as f32 / self.total_cell_count() as f32
+    }
+
+    /// Iterator over all visible cell center points.
+    pub fn visible_points(&self) -> impl Iterator<Item = Point2D> + '_ {
+        self.grid.iter().enumerate().flat_map(move |(y, row)| {
+            row.iter()
+                .enumerate()
+                .filter(|&(_, visible)| *visible)
+                .map(move |(x, _)| self.grid_to_world_center(x, y))
+        })
+    }
+
+    /// Convert grid cell to world coordinates (cell center).
+    fn grid_to_world_center(&self, x_cell: usize, y_cell: usize) -> Point2D {
+        Point2D::new(
+            self.origin.x + (x_cell as f32 + 0.5) * self.resolution,
+            self.origin.y + (y_cell as f32 + 0.5) * self.resolution,
+        )
+    }
+}
+
+impl Default for VisibilityRegion {
+    fn default() -> Self {
+        Self::default_resolution()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use std::f32::consts::PI;
+
+    #[test]
+    fn test_new_visibility_region() {
+        let vis = VisibilityRegion::new(0.1);
+        assert!(vis.is_empty());
+        assert_eq!(vis.resolution(), 0.1);
+        assert!(vis.visible_bounds().is_empty());
+    }
+
+    #[test]
+    fn test_empty_scan() {
+        let mut vis = VisibilityRegion::new(0.1);
+        let pose = Pose2D::identity();
+        vis.update_from_scan(pose, &[]);
+        assert!(vis.is_empty());
+    }
+
+    #[test]
+    fn test_single_point_scan() {
+        let mut vis = VisibilityRegion::new(0.1);
+        let pose = Pose2D::identity();
+        let points = vec![Point2D::new(1.0, 0.0)];
+
+        vis.update_from_scan(pose, &points);
+
+        // Robot position should be visible
+        assert!(vis.is_visible(Point2D::new(0.0, 0.0)));
+        // Point along ray should be visible
+        assert!(vis.is_visible(Point2D::new(0.5, 0.0)));
+        // Scan point should be visible
+        assert!(vis.is_visible(Point2D::new(1.0, 0.0)));
+        // Point beyond scan should not be visible
+        assert!(!vis.is_visible(Point2D::new(2.0, 0.0)));
+        // Point off to the side should not be visible
+        assert!(!vis.is_visible(Point2D::new(0.5, 1.0)));
+    }
+
+    #[test]
+    fn test_circular_scan() {
+        let mut vis = VisibilityRegion::new(0.1);
+        let pose = Pose2D::identity();
+
+        // Create a circular scan (8 points at 1m radius)
+        let points: Vec<Point2D> = (0..8)
+            .map(|i| {
+                let angle = i as f32 * PI / 4.0;
+                Point2D::from_polar(angle, 1.0)
+            })
+            .collect();
+
+        vis.update_from_scan(pose, &points);
+
+        // Center should be visible
+        assert!(vis.is_visible(Point2D::zero()));
+        // Points halfway should be visible
+        assert!(vis.is_visible(Point2D::new(0.5, 0.0)));
+        assert!(vis.is_visible(Point2D::new(0.0, 0.5)));
+    }
+
+    #[test]
+    fn test_incremental_scans() {
+        let mut vis = VisibilityRegion::new(0.1);
+
+        // First scan from origin
+        let pose1 = Pose2D::identity();
+        let points1 = vec![Point2D::new(1.0, 0.0)];
+        vis.update_from_scan(pose1, &points1);
+
+        // Second scan from different position
+        let pose2 = Pose2D::new(1.0, 0.0, PI / 2.0);
+        let points2 = vec![Point2D::new(1.0, 1.0)];
+        vis.update_from_scan(pose2, &points2);
+
+        // Both scan areas should be visible
+        assert!(vis.is_visible(Point2D::new(0.5, 0.0)));
+        assert!(vis.is_visible(Point2D::new(1.0, 0.5)));
+    }
+
+    #[test]
+    fn test_visibility_bounds() {
+        let mut vis = VisibilityRegion::new(0.1);
+        let pose = Pose2D::identity();
+        let points = vec![
+            Point2D::new(1.0, 0.0),
+            Point2D::new(-1.0, 0.0),
+            Point2D::new(0.0, 1.0),
+            Point2D::new(0.0, -1.0),
+        ];
+
+        vis.update_from_scan(pose, &points);
+
+        let bounds = vis.visible_bounds();
+        assert!(!bounds.is_empty());
+        assert!(bounds.contains(Point2D::zero()));
+    }
+
+    #[test]
+    fn test_grid_expansion() {
+        let mut vis = VisibilityRegion::new(0.1);
+
+        // First small scan
+        let pose1 = Pose2D::identity();
+        let points1 = vec![Point2D::new(0.5, 0.0)];
+        vis.update_from_scan(pose1, &points1);
+        let initial_size = vis.grid_size();
+
+        // Larger scan requiring expansion
+        let pose2 = Pose2D::identity();
+        let points2 = vec![Point2D::new(5.0, 5.0)];
+        vis.update_from_scan(pose2, &points2);
+        let expanded_size = vis.grid_size();
+
+        // Grid should have grown
+        assert!(expanded_size.0 > initial_size.0);
+        assert!(expanded_size.1 > initial_size.1);
+
+        // Original visibility should be preserved
+        assert!(vis.is_visible(Point2D::new(0.25, 0.0)));
+    }
+
+    #[test]
+    fn test_visibility_statistics() {
+        let mut vis = VisibilityRegion::new(0.1);
+        let pose = Pose2D::identity();
+        let points = vec![Point2D::new(1.0, 0.0)];
+
+        vis.update_from_scan(pose, &points);
+
+        assert!(vis.visible_cell_count() > 0);
+        assert!(vis.total_cell_count() >= vis.visible_cell_count());
+        let ratio = vis.visibility_ratio();
+        assert!(ratio > 0.0 && ratio <= 1.0);
+    }
+}

vastu-map/src/query/cbvg/voronoi.rs

@@ -0,0 +1,781 @@
+//! Voronoi diagram approximation for medial axis computation.
+//!
+//! Uses point sampling along walls and computes Voronoi diagram
+//! to approximate the medial axis of free space.
+
+use crate::core::{Bounds, Point2D};
+use crate::features::Line2D;
+
+use super::clearance::{min_distance_to_walls, sample_line};
+use super::config::CBVGConfig;
+use super::node::{CBVGNode, NodeType};
+use super::visibility::VisibilityRegion;
+
+/// A Voronoi edge connecting two points in free space.
+#[derive(Clone, Debug)]
+pub struct VoronoiEdge {
+    /// Start point of the edge.
+    pub start: Point2D,
+    /// End point of the edge.
+    pub end: Point2D,
+    /// Indices of the two generating wall points.
+    pub generators: (usize, usize),
+}
+
+impl VoronoiEdge {
+    /// Create a new Voronoi edge.
+    pub fn new(start: Point2D, end: Point2D, gen1: usize, gen2: usize) -> Self {
+        Self {
+            start,
+            end,
+            generators: (gen1, gen2),
+        }
+    }
+
+    /// Get the length of this edge.
+    #[inline]
+    pub fn length(&self) -> f32 {
+        self.start.distance(self.end)
+    }
+
+    /// Get a point along the edge at parameter t (0=start, 1=end).
+    #[inline]
+    pub fn point_at(&self, t: f32) -> Point2D {
+        self.start.lerp(self.end, t)
+    }
+}
+
+/// Sample points along all wall segments.
+///
+/// # Arguments
+/// * `lines` - Wall segments
+/// * `sample_step` - Distance between samples
+///
+/// # Returns
+/// Vector of sampled wall points.
+pub fn sample_wall_points(lines: &[Line2D], sample_step: f32) -> Vec<Point2D> {
+    lines
+        .iter()
+        .flat_map(|line| sample_line(line, sample_step))
+        .collect()
+}
+
+/// Compute approximate Voronoi edges using distance field approach.
+///
+/// This is a simplified implementation that:
+/// 1. Samples points along walls
+/// 2. Finds points equidistant from two walls (approximate Voronoi vertices)
+/// 3. Connects them to form Voronoi edges
+///
+/// # Arguments
+/// * `lines` - Wall segments
+/// * `bounds` - Map bounds for grid sampling
+/// * `config` - CBVG configuration
+///
+/// # Returns
+/// Vector of Voronoi edges in free space.
+pub fn compute_voronoi_edges(
+    lines: &[Line2D],
+    bounds: Option<&Bounds>,
+    config: &CBVGConfig,
+) -> Vec<VoronoiEdge> {
+    if lines.is_empty() {
+        return Vec::new();
+    }
+
+    // Get bounds for sampling
+    let bounds = match bounds {
+        Some(b) => *b,
+        None => compute_bounds_from_lines(lines),
+    };
+
+    // Expand bounds slightly to ensure coverage
+    let margin = config.min_clearance * 2.0;
+    let expanded_bounds = Bounds {
+        min: Point2D::new(bounds.min.x - margin, bounds.min.y - margin),
+        max: Point2D::new(bounds.max.x + margin, bounds.max.y + margin),
+    };
+
+    // Use grid-based approach to find medial axis points
+    let step = config.voronoi_sample_step;
+    let mut medial_points: Vec<Point2D> = Vec::new();
+
+    let x_steps = ((expanded_bounds.max.x - expanded_bounds.min.x) / step).ceil() as usize;
+    let y_steps = ((expanded_bounds.max.y - expanded_bounds.min.y) / step).ceil() as usize;
+
+    for ix in 0..=x_steps {
+        for iy in 0..=y_steps {
+            let x = expanded_bounds.min.x + ix as f32 * step;
+            let y = expanded_bounds.min.y + iy as f32 * step;
+            let point = Point2D::new(x, y);
+
+            // Check if this point is on the medial axis
+            if is_near_medial_axis(point, lines, config) {
+                let clearance = min_distance_to_walls(point, lines);
+                if clearance >= config.min_clearance_narrow {
+                    medial_points.push(point);
+                }
+            }
+        }
+    }
+
+    // Connect nearby medial points to form edges
+    connect_medial_points(&medial_points, lines, config)
+}
+
+/// Check if a point is near the medial axis (equidistant from multiple walls).
+///
+/// A point is on the medial axis if it's approximately equidistant from
+/// its two nearest walls.
+fn is_near_medial_axis(point: Point2D, lines: &[Line2D], config: &CBVGConfig) -> bool {
+    if lines.len() < 2 {
+        return false;
+    }
+
+    // Find distances to all walls
+    let mut distances: Vec<(f32, usize)> = lines
+        .iter()
+        .enumerate()
+        .map(|(idx, line)| (line.distance_to_point_segment(point), idx))
+        .collect();
+
+    distances.sort_by(|a, b| a.0.partial_cmp(&b.0).unwrap_or(std::cmp::Ordering::Equal));
+
+    if distances.len() < 2 {
+        return false;
+    }
+
+    let d1 = distances[0].0;
+    let d2 = distances[1].0;
+
+    // Point must have minimum clearance
+    if d1 < config.min_clearance_narrow {
+        return false;
+    }
+
+    // Check if approximately equidistant (within tolerance)
+    let tolerance = config.voronoi_sample_step * 1.5;
+    let diff = (d1 - d2).abs();
+
+    // On medial axis if distances are similar
+    diff < tolerance
+}
+
+/// Connect medial axis points to form edges.
+fn connect_medial_points(
+    points: &[Point2D],
+    lines: &[Line2D],
+    config: &CBVGConfig,
+) -> Vec<VoronoiEdge> {
+    let mut edges = Vec::new();
+    let max_connect_dist = config.voronoi_sample_step * 2.0;
+
+    for i in 0..points.len() {
+        for j in (i + 1)..points.len() {
+            let dist = points[i].distance(points[j]);
+
+            // Only connect nearby points
+            if dist <= max_connect_dist {
+                // Verify the connection is valid (no walls in between)
+                let midpoint = points[i].lerp(points[j], 0.5);
+                let mid_clearance = min_distance_to_walls(midpoint, lines);
+
+                if mid_clearance >= config.min_clearance_narrow {
+                    edges.push(VoronoiEdge::new(points[i], points[j], i, j));
+                }
+            }
+        }
+    }
+
+    edges
+}
+
+/// Sample nodes along Voronoi edges (medial axis).
+///
+/// # Arguments
+/// * `edges` - Voronoi edges
+/// * `lines` - Wall segments (for clearance computation)
+/// * `config` - CBVG configuration
+///
+/// # Returns
+/// Vector of CBVG nodes sampled along the medial axis.
+pub fn sample_medial_axis_nodes(
+    edges: &[VoronoiEdge],
+    lines: &[Line2D],
+    config: &CBVGConfig,
+) -> Vec<CBVGNode> {
+    let mut nodes = Vec::new();
+    let mut seen_positions: Vec<Point2D> = Vec::new();
+
+    for edge in edges {
+        let edge_length = edge.length();
+        let num_samples = (edge_length / config.voronoi_sample_step).ceil() as usize;
+        let num_samples = num_samples.max(1);
+
+        for i in 0..=num_samples {
+            let t = i as f32 / num_samples as f32;
+            let position = edge.point_at(t);
+
+            // Skip if too close to existing node
+            let is_duplicate = seen_positions
+                .iter()
+                .any(|p| p.distance(position) < config.node_merge_distance);
+
+            if is_duplicate {
+                continue;
+            }
+
+            let clearance = min_distance_to_walls(position, lines);
+
+            // Skip if insufficient clearance
+            if clearance < config.min_clearance_narrow {
+                continue;
+            }
+
+            let is_narrow = clearance < config.min_clearance;
+
+            nodes.push(CBVGNode::medial_axis(position, clearance, is_narrow));
+            seen_positions.push(position);
+
+            // Limit total nodes
+            if nodes.len() >= config.max_nodes {
+                return nodes;
+            }
+        }
+    }
+
+    nodes
+}
+
+/// Generate nodes directly from distance field without explicit Voronoi computation.
+///
+/// This is a simpler alternative that samples the medial axis directly.
+///
+/// # Arguments
+/// * `lines` - Wall segments
+/// * `bounds` - Map bounds
+/// * `config` - CBVG configuration
+///
+/// # Returns
+/// Vector of CBVG nodes on the medial axis.
+pub fn generate_medial_axis_nodes(
+    lines: &[Line2D],
+    bounds: Option<&Bounds>,
+    config: &CBVGConfig,
+) -> Vec<CBVGNode> {
+    if lines.is_empty() {
+        return Vec::new();
+    }
+
+    let bounds = match bounds {
+        Some(b) => *b,
+        None => compute_bounds_from_lines(lines),
+    };
+
+    // Expand bounds
+    let margin = config.min_clearance;
+    let expanded_bounds = Bounds {
+        min: Point2D::new(bounds.min.x - margin, bounds.min.y - margin),
+        max: Point2D::new(bounds.max.x + margin, bounds.max.y + margin),
+    };
+
+    let step = config.voronoi_sample_step;
+    let mut nodes: Vec<CBVGNode> = Vec::new();
+
+    let x_steps = ((expanded_bounds.max.x - expanded_bounds.min.x) / step).ceil() as usize;
+    let y_steps = ((expanded_bounds.max.y - expanded_bounds.min.y) / step).ceil() as usize;
+
+    for ix in 0..=x_steps {
+        for iy in 0..=y_steps {
+            let x = expanded_bounds.min.x + ix as f32 * step;
+            let y = expanded_bounds.min.y + iy as f32 * step;
+            let point = Point2D::new(x, y);
+
+            // Check if on medial axis with sufficient clearance
+            if is_near_medial_axis(point, lines, config) {
+                let clearance = min_distance_to_walls(point, lines);
+
+                if clearance >= config.min_clearance_narrow {
+                    let is_narrow = clearance < config.min_clearance;
+
+                    // Check for duplicates
+                    let is_duplicate = nodes
+                        .iter()
+                        .any(|n| n.position.distance(point) < config.node_merge_distance);
+
+                    if !is_duplicate {
+                        nodes.push(CBVGNode::medial_axis(point, clearance, is_narrow));
+
+                        if nodes.len() >= config.max_nodes {
+                            return nodes;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    // Add junction nodes at corridor intersections
+    add_junction_nodes(&mut nodes, lines, config);
+
+    nodes
+}
+
+/// Add junction nodes where multiple corridors meet.
+fn add_junction_nodes(nodes: &mut [CBVGNode], lines: &[Line2D], config: &CBVGConfig) {
+    // Find points where 3+ walls are approximately equidistant
+    // These are corridor junctions
+
+    if lines.len() < 3 {
+        return;
+    }
+
+    // For now, detect junctions as points where medial axis changes direction significantly
+    // This is a simplified approach; a full implementation would use Voronoi vertices
+
+    let mut junction_candidates: Vec<Point2D> = Vec::new();
+
+    for node in nodes.iter() {
+        // Count nearby walls at similar distances
+        let clearance = node.clearance;
+        let tolerance = config.voronoi_sample_step;
+
+        let nearby_walls: usize = lines
+            .iter()
+            .filter(|line| {
+                let dist = line.distance_to_point_segment(node.position);
+                (dist - clearance).abs() < tolerance
+            })
+            .count();
+
+        // Junction if equidistant from 3+ walls
+        if nearby_walls >= 3 {
+            junction_candidates.push(node.position);
+        }
+    }
+
+    // Mark existing nodes as junctions or add new junction nodes
+    for candidate in junction_candidates {
+        // Find if there's an existing node nearby
+        let existing = nodes
+            .iter_mut()
+            .find(|n| n.position.distance(candidate) < config.node_merge_distance);
+
+        if let Some(node) = existing {
+            node.node_type = NodeType::Junction;
+        }
+    }
+}
+
+/// Compute bounds from line segments.
+fn compute_bounds_from_lines(lines: &[Line2D]) -> Bounds {
+    if lines.is_empty() {
+        return Bounds::empty();
+    }
+
+    let mut min = Point2D::new(f32::INFINITY, f32::INFINITY);
+    let mut max = Point2D::new(f32::NEG_INFINITY, f32::NEG_INFINITY);
+
+    for line in lines {
+        min = min.min(line.start).min(line.end);
+        max = max.max(line.start).max(line.end);
+    }
+
+    Bounds { min, max }
+}
+
+/// Generate nodes from lidar visibility region.
+///
+/// Only creates nodes in areas the lidar has actually observed, with proper
+/// wall clearance. Uses a grid-based approach with the coverage_node_interval
+/// to ensure uniform coverage in open areas.
+///
+/// # Arguments
+/// * `visibility` - Visibility region tracking scanned areas
+/// * `lines` - Wall segments for clearance computation
+/// * `config` - CBVG configuration
+///
+/// # Returns
+/// Vector of CBVG nodes in visible areas with safe clearance.
+pub fn generate_nodes_from_visibility(
+    visibility: &VisibilityRegion,
+    lines: &[Line2D],
+    config: &CBVGConfig,
+) -> Vec<CBVGNode> {
+    if visibility.is_empty() || lines.is_empty() {
+        return Vec::new();
+    }
+
+    let bounds = visibility.visible_bounds();
+    if bounds.is_empty() {
+        return Vec::new();
+    }
+
+    let step = config.coverage_node_interval;
+    let mut nodes: Vec<CBVGNode> = Vec::new();
+
+    // Jitter parameters: nodes are offset by ±30% of step size
+    // This prevents systematic gaps near walls due to grid alignment
+    let jitter_range = step * 0.3;
+
+    // Grid sample within visible bounds
+    let x_start = bounds.min.x;
+    let x_end = bounds.max.x;
+    let y_start = bounds.min.y;
+    let y_end = bounds.max.y;
+
+    let x_steps = ((x_end - x_start) / step).ceil() as usize;
+    let y_steps = ((y_end - y_start) / step).ceil() as usize;
+
+    for ix in 0..=x_steps {
+        for iy in 0..=y_steps {
+            // Base grid position
+            let base_x = x_start + ix as f32 * step;
+            let base_y = y_start + iy as f32 * step;
+
+            // Apply deterministic jitter based on grid cell
+            // Uses a simple hash to create pseudo-random but reproducible offsets
+            let (jitter_x, jitter_y) = deterministic_jitter(ix, iy, jitter_range);
+            let x = base_x + jitter_x;
+            let y = base_y + jitter_y;
+            let point = Point2D::new(x, y);
+
+            // Must be in visible area (scanned by lidar)
+            if !visibility.is_visible(point) {
+                continue;
+            }
+
+            // Must have clearance from walls
+            let clearance = min_distance_to_walls(point, lines);
+            if clearance < config.min_clearance_narrow {
+                continue;
+            }
+
+            // Check for duplicates
+            let is_duplicate = nodes
+                .iter()
+                .any(|n| n.position.distance(point) < config.node_merge_distance);
+
+            if is_duplicate {
+                continue;
+            }
+
+            // Determine node type based on clearance
+            let is_narrow = clearance < config.min_clearance;
+            nodes.push(CBVGNode::medial_axis(point, clearance, is_narrow));
+
+            if nodes.len() >= config.max_nodes {
+                return nodes;
+            }
+        }
+    }
+
+    // Add junction nodes at corridor intersections
+    add_junction_nodes(&mut nodes, lines, config);
+
+    nodes
+}
+
+/// Generate deterministic jitter for a grid cell.
+///
+/// Uses a simple hash function to create pseudo-random but reproducible
+/// offsets for each grid cell. This ensures the same grid always produces
+/// the same node positions.
+///
+/// # Arguments
+/// * `ix` - Grid x index
+/// * `iy` - Grid y index
+/// * `range` - Maximum jitter magnitude (±range)
+///
+/// # Returns
+/// Tuple of (x_offset, y_offset) in range [-range, +range]
+fn deterministic_jitter(ix: usize, iy: usize, range: f32) -> (f32, f32) {
+    // Simple hash combining grid indices
+    // Using prime multipliers for better distribution
+    let hash1 = ((ix.wrapping_mul(73856093)) ^ (iy.wrapping_mul(19349663))) as u32;
+    let hash2 = ((ix.wrapping_mul(83492791)) ^ (iy.wrapping_mul(41729867))) as u32;
+
+    // Convert to [-1, 1] range using the hash bits
+    let norm1 = (hash1 % 10000) as f32 / 5000.0 - 1.0;
+    let norm2 = (hash2 % 10000) as f32 / 5000.0 - 1.0;
+
+    (norm1 * range, norm2 * range)
+}
+
+/// Generate nodes combining visibility-aware and medial axis approaches.
+///
+/// This hybrid approach:
+/// 1. Places coverage nodes uniformly in visible areas
+/// 2. Adds medial axis nodes where they don't already exist
+/// 3. Ensures good connectivity in narrow passages
+///
+/// # Arguments
+/// * `visibility` - Visibility region tracking scanned areas
+/// * `lines` - Wall segments
+/// * `config` - CBVG configuration
+///
+/// # Returns
+/// Vector of CBVG nodes optimized for path planning.
+pub fn generate_nodes_hybrid(
+    visibility: &VisibilityRegion,
+    lines: &[Line2D],
+    config: &CBVGConfig,
+) -> Vec<CBVGNode> {
+    // Start with visibility-based coverage nodes
+    let mut nodes = generate_nodes_from_visibility(visibility, lines, config);
+
+    if nodes.len() >= config.max_nodes {
+        return nodes;
+    }
+
+    // Add medial axis nodes for better coverage in corridors
+    // Use smaller step for more precision in narrow areas
+    let corridor_config = CBVGConfig {
+        voronoi_sample_step: config.voronoi_sample_step,
+        ..config.clone()
+    };
+
+    let bounds = visibility.visible_bounds();
+    for ix in 0..((bounds.width() / corridor_config.voronoi_sample_step).ceil() as usize) {
+        for iy in 0..((bounds.height() / corridor_config.voronoi_sample_step).ceil() as usize) {
+            let x = bounds.min.x + ix as f32 * corridor_config.voronoi_sample_step;
+            let y = bounds.min.y + iy as f32 * corridor_config.voronoi_sample_step;
+            let point = Point2D::new(x, y);
+
+            // Only add in visible areas
+            if !visibility.is_visible(point) {
+                continue;
+            }
+
+            // Only add if on medial axis
+            if !is_near_medial_axis(point, lines, &corridor_config) {
+                continue;
+            }
+
+            let clearance = min_distance_to_walls(point, lines);
+            if clearance < config.min_clearance_narrow {
+                continue;
+            }
+
+            // Skip if already covered
+            let is_duplicate = nodes
+                .iter()
+                .any(|n| n.position.distance(point) < config.node_merge_distance);
+
+            if is_duplicate {
+                continue;
+            }
+
+            let is_narrow = clearance < config.min_clearance;
+            nodes.push(CBVGNode::medial_axis(point, clearance, is_narrow));
+
+            if nodes.len() >= config.max_nodes {
+                return nodes;
+            }
+        }
+    }
+
+    nodes
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn make_corridor() -> Vec<Line2D> {
+        // Simple corridor: two parallel walls
+        vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(5.0, 0.0)), // Bottom wall
+            Line2D::new(Point2D::new(0.0, 1.0), Point2D::new(5.0, 1.0)), // Top wall
+        ]
+    }
+
+    fn make_room() -> Vec<Line2D> {
+        // Simple room: 4 walls
+        vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(4.0, 0.0)), // Bottom
+            Line2D::new(Point2D::new(4.0, 0.0), Point2D::new(4.0, 3.0)), // Right
+            Line2D::new(Point2D::new(4.0, 3.0), Point2D::new(0.0, 3.0)), // Top
+            Line2D::new(Point2D::new(0.0, 3.0), Point2D::new(0.0, 0.0)), // Left
+        ]
+    }
+
+    #[test]
+    fn test_deterministic_jitter() {
+        let range = 0.15; // ±0.15m jitter
+
+        // Test that jitter is deterministic (same input = same output)
+        let (j1x, j1y) = deterministic_jitter(5, 10, range);
+        let (j2x, j2y) = deterministic_jitter(5, 10, range);
+        assert_eq!(j1x, j2x);
+        assert_eq!(j1y, j2y);
+
+        // Test that different cells get different jitter
+        let (j3x, j3y) = deterministic_jitter(5, 11, range);
+        assert!(
+            j1x != j3x || j1y != j3y,
+            "Adjacent cells should have different jitter"
+        );
+
+        // Test that jitter is within bounds
+        for ix in 0..20 {
+            for iy in 0..20 {
+                let (jx, jy) = deterministic_jitter(ix, iy, range);
+                assert!(jx.abs() <= range, "X jitter {} exceeds range {}", jx, range);
+                assert!(jy.abs() <= range, "Y jitter {} exceeds range {}", jy, range);
+            }
+        }
+    }
+
+    #[test]
+    fn test_sample_wall_points() {
+        let walls = make_corridor();
+        let points = sample_wall_points(&walls, 1.0);
+
+        // Should have points from both walls
+        assert!(!points.is_empty());
+        assert!(points.len() >= 10); // At least 5 per wall
+    }
+
+    #[test]
+    fn test_is_near_medial_axis() {
+        let walls = make_corridor();
+        let config = CBVGConfig::default().with_voronoi_sample_step(0.1);
+
+        // Point in the middle of corridor should be on medial axis
+        let center = Point2D::new(2.5, 0.5);
+        assert!(is_near_medial_axis(center, &walls, &config));
+
+        // Point close to a wall should not be on medial axis
+        let near_wall = Point2D::new(2.5, 0.1);
+        assert!(!is_near_medial_axis(near_wall, &walls, &config));
+    }
+
+    #[test]
+    fn test_generate_medial_axis_nodes() {
+        let walls = make_room();
+        let config = CBVGConfig::default()
+            .with_voronoi_sample_step(0.2)
+            .with_min_clearance(0.3)
+            .with_min_clearance_narrow(0.18);
+
+        let nodes = generate_medial_axis_nodes(&walls, None, &config);
+
+        // Should have nodes in the room
+        assert!(!nodes.is_empty());
+
+        // All nodes should have sufficient clearance
+        for node in &nodes {
+            assert!(node.clearance >= config.min_clearance_narrow);
+        }
+
+        // Most nodes should be near the center of the room
+        let center = Point2D::new(2.0, 1.5);
+        let near_center: usize = nodes
+            .iter()
+            .filter(|n| n.position.distance(center) < 1.5)
+            .count();
+        assert!(near_center > 0);
+    }
+
+    #[test]
+    fn test_compute_bounds_from_lines() {
+        let walls = make_room();
+        let bounds = compute_bounds_from_lines(&walls);
+
+        assert!((bounds.min.x - 0.0).abs() < 0.01);
+        assert!((bounds.min.y - 0.0).abs() < 0.01);
+        assert!((bounds.max.x - 4.0).abs() < 0.01);
+        assert!((bounds.max.y - 3.0).abs() < 0.01);
+    }
+
+    #[test]
+    fn test_empty_walls() {
+        let config = CBVGConfig::default();
+        let nodes = generate_medial_axis_nodes(&[], None, &config);
+        assert!(nodes.is_empty());
+    }
+
+    #[test]
+    fn test_generate_nodes_from_visibility() {
+        use crate::core::Pose2D;
+
+        let walls = make_room();
+        let config = CBVGConfig::default()
+            .with_coverage_node_interval(0.5)
+            .with_min_clearance(0.3)
+            .with_min_clearance_narrow(0.18);
+
+        // Create visibility region with scan from center of room
+        let mut visibility = VisibilityRegion::new(0.1);
+        let robot_pose = Pose2D::new(2.0, 1.5, 0.0);
+
+        // Simulate a circular scan around the robot
+        let scan_points: Vec<Point2D> = (0..36)
+            .map(|i| {
+                let angle = i as f32 * std::f32::consts::PI / 18.0;
+                let dist = 1.5; // 1.5m range
+                robot_pose.transform_point(Point2D::from_polar(angle, dist))
+            })
+            .collect();
+
+        visibility.update_from_scan(robot_pose, &scan_points);
+
+        let nodes = generate_nodes_from_visibility(&visibility, &walls, &config);
+
+        // Should have nodes
+        assert!(!nodes.is_empty());
+
+        // All nodes should be in visible area
+        for node in &nodes {
+            assert!(visibility.is_visible(node.position));
+        }
+
+        // All nodes should have sufficient clearance
+        for node in &nodes {
+            assert!(node.clearance >= config.min_clearance_narrow);
+        }
+    }
+
+    #[test]
+    fn test_visibility_nodes_only_in_scanned_area() {
+        use crate::core::Pose2D;
+
+        // Create a large room
+        let walls = vec![
+            Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(10.0, 0.0)),
+            Line2D::new(Point2D::new(10.0, 0.0), Point2D::new(10.0, 10.0)),
+            Line2D::new(Point2D::new(10.0, 10.0), Point2D::new(0.0, 10.0)),
+            Line2D::new(Point2D::new(0.0, 10.0), Point2D::new(0.0, 0.0)),
+        ];
+
+        let config = CBVGConfig::default()
+            .with_coverage_node_interval(0.5)
+            .with_min_clearance(0.3);
+
+        // Scan only the bottom-left corner
+        let mut visibility = VisibilityRegion::new(0.1);
+        let robot_pose = Pose2D::new(2.0, 2.0, 0.0);
+
+        let scan_points: Vec<Point2D> = (0..36)
+            .map(|i| {
+                let angle = i as f32 * std::f32::consts::PI / 18.0;
+                let dist = 1.5;
+                robot_pose.transform_point(Point2D::from_polar(angle, dist))
+            })
+            .collect();
+
+        visibility.update_from_scan(robot_pose, &scan_points);
+
+        let nodes = generate_nodes_from_visibility(&visibility, &walls, &config);
+
+        // All nodes should be near the robot (not in unexplored far corner)
+        for node in &nodes {
+            assert!(
+                node.position.distance(robot_pose.position()) < 3.0,
+                "Node at {:?} is too far from scanned area",
+                node.position
+            );
+        }
+    }
+}

vastu-map/src/query/frontier.rs

@@ -229,12 +229,15 @@ impl FrontierDetector {
             // Find the closest unconnected endpoint that forms a valid gap
             let mut best_pair: Option<(usize, f32)> = None;
 
-            for j in (i + 1)..self.unconnected.len() {
-                if used_endpoints[j] {
+            for (j, (used, ep2)) in used_endpoints
+                .iter()
+                .zip(self.unconnected.iter())
+                .enumerate()
+                .skip(i + 1)
+            {
+                if *used {
                     continue;
                 }
-
-                let ep2 = &self.unconnected[j];
                 let distance = ep1.point.distance(ep2.point);
 
                 // Check if this pair forms a valid gap

vastu-map/src/query/mod.rs

@@ -4,12 +4,13 @@
 //! - **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
+//! - **CBVG**: Clearance-based visibility graph for safe path planning
 //!
 //! # Example
 //!
 //! ```rust,ignore
-//! use vastu_map::query::{raycast, query_occupancy, detect_frontiers, PathPlanner};
+//! use vastu_map::query::{raycast, query_occupancy, detect_frontiers};
+//! use vastu_map::query::{ClearanceVisibilityGraph, CBVGConfig};
 //! use vastu_map::core::Point2D;
 //!
 //! // Raycast to find wall distance
@@ -23,11 +24,14 @@
 //! // 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);
+//! // Plan a path using CBVG
+//! let config = CBVGConfig::default();
+//! let mut graph = ClearanceVisibilityGraph::new(config);
+//! graph.build(&lines, None);
+//! let path = graph.find_path(start, goal, &lines);
 //! ```
 
+pub mod cbvg;
 pub mod frontier;
 pub mod occupancy;
 pub mod path_planning;
@@ -42,8 +46,11 @@ 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 path_planning::is_straight_path_clear;
 pub use raycast::{
     RaycastResult, raycast, raycast_360, raycast_batch, raycast_detailed, raycast_indexed,
     raycast_sweep,
 };
+
+// Re-export CBVG types
+pub use cbvg::{CBVGConfig, CBVGNode, ClearanceVisibilityGraph, NodeType};

vastu-map/src/query/path_planning.rs

@@ -1,529 +1,38 @@
-//! Vector-based path planning using Visibility Graph.
+//! Path clearance checking utilities.
 //!
-//! 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,
-//! );
-//! ```
+//! Provides functions to check if a straight-line path is clear of obstacles.
 
-use serde::{Deserialize, Serialize};
-
-use crate::core::{Bounds, Point2D};
+use crate::core::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, Serialize, Deserialize)]
-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.
+/// Check if a straight-line path between two points is clear of obstacles.
 ///
-/// 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
-    }
-
-    /// Get the adjacency list of edges.
-    ///
-    /// Returns a slice of edge lists, where each inner list contains
-    /// (neighbor_index, distance) pairs for the corresponding node.
-    pub fn edges(&self) -> &[Vec<(usize, f32)>] {
-        &self.edges
-    }
-
-    /// Get all unique edges as (from_node, to_node, distance) tuples.
-    ///
-    /// Each edge is returned only once (not duplicated for both directions).
-    pub fn edge_list(&self) -> Vec<(usize, usize, f32)> {
-        let mut edges = Vec::new();
-        for (from_idx, neighbors) in self.edges.iter().enumerate() {
-            for &(to_idx, dist) in neighbors {
-                // Only include each edge once (from < to)
-                if from_idx < to_idx {
-                    edges.push((from_idx, to_idx, dist));
-                }
-            }
-        }
-        edges
-    }
-}
-
-/// 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;
-            }
-        }
-
-        // Try direct path first - much faster than building full visibility graph
-        if is_straight_path_clear(from, to, lines, self.config.robot_radius) {
-            return Some(Path {
-                points: vec![from, to],
-                length: from.distance(to),
-            });
-        }
-
-        // 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
-    }
-
-    /// Build a visibility graph for visualization.
-    ///
-    /// This builds the full visibility graph without planning a path,
-    /// useful for debugging and visualization purposes.
-    ///
-    /// # Arguments
-    ///
-    /// * `from` - Start position
-    /// * `to` - Goal position
-    /// * `lines` - Map lines (obstacles)
-    ///
-    /// # Returns
-    ///
-    /// The visibility graph with all nodes and edges.
-    pub fn build_graph(&self, from: Point2D, to: Point2D, lines: &[Line2D]) -> VisibilityGraph {
-        VisibilityGraph::build(
-            lines,
-            from,
-            to,
-            self.config.robot_radius,
-            self.config.max_nodes,
-        )
-    }
-}
-
-/// 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.
+/// This performs collision checking by:
+/// 1. Checking for direct line-line intersections
+/// 2. Sampling points along the path and checking clearance
 ///
 /// # 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 (no obstacles within robot_radius).
 ///
-/// True if the straight path is clear.
+/// # Example
+///
+/// ```rust,ignore
+/// use vastu_map::query::is_straight_path_clear;
+/// use vastu_map::core::Point2D;
+///
+/// let clear = is_straight_path_clear(
+///     Point2D::new(0.0, 0.0),
+///     Point2D::new(1.0, 1.0),
+///     &walls,
+///     0.15, // robot radius
+/// );
+/// ```
 pub fn is_straight_path_clear(
     from: Point2D,
     to: Point2D,
@@ -568,228 +77,62 @@ pub fn is_straight_path_clear(
 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,
+            0.1
         ));
     }
 
     #[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,
+            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
+        // Path goes close to wall but doesn't intersect
+        let lines = vec![Line2D::new(Point2D::new(0.5, 0.15), Point2D::new(0.5, 1.0))];
+        // With radius 0.2, should be blocked (too close)
         assert!(!is_straight_path_clear(
-            Point2D::new(0.0, 0.5),
-            Point2D::new(1.0, 0.5),
+            Point2D::new(0.0, 0.0),
+            Point2D::new(1.0, 0.0),
             &lines,
-            0.2, // Robot would clip the wall
+            0.2
         ));
     }
 
     #[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
+        // Wall is far enough away
+        let lines = vec![Line2D::new(Point2D::new(0.5, 0.5), Point2D::new(0.5, 1.0))];
         assert!(is_straight_path_clear(
-            Point2D::new(0.0, 2.0),
-            Point2D::new(1.0, 2.0),
+            Point2D::new(0.0, 0.0),
+            Point2D::new(1.0, 0.0),
             &lines,
-            0.2,
+            0.1
         ));
     }
 
     #[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(
+    fn test_zero_length_path() {
+        let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))];
+        // Zero-length path is always clear
+        assert!(is_straight_path_clear(
             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);
+            0.1
+        ));
     }
 }

vastu-map/src/vector_map/config.rs

@@ -6,7 +6,8 @@ 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, PathPlanningConfig};
+use crate::query::cbvg::CBVGConfig;
+use crate::query::{FrontierConfig, OccupancyConfig};
 
 // ============================================================================
 // Configuration Validation
@@ -118,8 +119,9 @@ pub struct VectorMapConfig {
     /// Configuration for occupancy queries.
     pub occupancy: OccupancyConfig,
 
-    /// Configuration for path planning.
-    pub path_planning: PathPlanningConfig,
+    /// Configuration for clearance-based visibility graph (CBVG).
+    #[serde(default)]
+    pub cbvg: CBVGConfig,
 
     /// Configuration for loop closure detection.
     pub loop_closure: LoopClosureConfig,
@@ -151,7 +153,7 @@ impl Default for VectorMapConfig {
             merger: MergerConfig::default(),
             frontier: FrontierConfig::default(),
             occupancy: OccupancyConfig::default(),
-            path_planning: PathPlanningConfig::default(),
+            cbvg: CBVGConfig::default(),
             loop_closure: LoopClosureConfig::default(),
             min_match_confidence: 0.3,
             mapping_enabled: true,
@@ -306,17 +308,20 @@ impl VectorMapConfig {
         self
     }
 
-    /// Builder-style setter for robot radius (path planning).
+    /// Builder-style setter for robot radius.
     ///
-    /// Sets the robot radius used for collision checking in path planning.
+    /// Sets the robot radius used for collision checking in path planning
+    /// and frontier detection.
     pub fn with_robot_radius(mut self, radius: f32) -> Self {
-        self.path_planning.robot_radius = radius;
+        self.frontier.robot_radius = radius;
+        // Update CBVG narrow clearance to match (robot_radius + small margin)
+        self.cbvg.min_clearance_narrow = radius + 0.02;
         self
     }
 
-    /// Builder-style setter for path planning configuration.
-    pub fn with_path_planning(mut self, config: PathPlanningConfig) -> Self {
-        self.path_planning = config;
+    /// Builder-style setter for CBVG configuration.
+    pub fn with_cbvg(mut self, config: CBVGConfig) -> Self {
+        self.cbvg = config;
         self
     }
 

vastu-map/src/vector_map/map.rs

@@ -13,9 +13,8 @@ use crate::integration::{
 };
 use crate::loop_closure::{LoopClosure, LoopClosureDetector};
 use crate::matching::PointToLineIcp;
-use crate::query::{
-    PathPlanner, detect_frontiers, is_straight_path_clear, query_occupancy, raycast,
-};
+use crate::query::cbvg::{ClearanceVisibilityGraph, VisibilityRegion};
+use crate::query::{detect_frontiers, is_straight_path_clear, query_occupancy, raycast};
 use crate::{Frontier, Map, ObserveResult, Occupancy, Path, TimingBreakdown};
 
 use super::config::VectorMapConfig;
@@ -168,6 +167,17 @@ pub struct VectorMap {
 
     /// Exploration configuration (None if exploration mode disabled).
     exploration_config: Option<ExplorationConfig>,
+
+    /// Clearance-based visibility graph for path planning.
+    /// Automatically updated when lines are added to the map.
+    cbvg: ClearanceVisibilityGraph,
+
+    /// Whether the CBVG needs rebuilding.
+    cbvg_dirty: bool,
+
+    /// Visibility region tracking lidar-scanned areas.
+    /// Used to ensure CBVG nodes are only placed in observed areas.
+    visibility: VisibilityRegion,
 }
 
 impl VectorMap {
@@ -175,6 +185,9 @@ impl VectorMap {
     pub fn new(config: VectorMapConfig) -> Self {
         let loop_detector = LoopClosureDetector::new(config.loop_closure.clone());
         let icp_matcher = PointToLineIcp::with_config(config.matching.clone());
+        let cbvg = ClearanceVisibilityGraph::new(config.cbvg.clone());
+        // Use CBVG coverage_node_interval as visibility resolution for consistency
+        let visibility = VisibilityRegion::new(config.cbvg.coverage_node_interval * 0.5);
         Self {
             config,
             line_store: LineStore::new(),
@@ -187,6 +200,9 @@ impl VectorMap {
             observe_scratch: ObserveScratchSpace::default_capacity(),
             scan_store: None,
             exploration_config: None,
+            cbvg,
+            cbvg_dirty: true,
+            visibility,
         }
     }
 
@@ -242,6 +258,47 @@ impl VectorMap {
         self.loop_closures.clear();
     }
 
+    // ─────────────────────────────────────────────────────────────────────────
+    // Visibility and Path Planning
+    // ─────────────────────────────────────────────────────────────────────────
+
+    /// Get the visibility region (for visualization/debugging).
+    pub fn visibility(&self) -> &VisibilityRegion {
+        &self.visibility
+    }
+
+    /// Build and return a CBVG for visualization.
+    ///
+    /// Uses visibility-based node generation if visibility data is available.
+    pub fn build_cbvg(&self) -> ClearanceVisibilityGraph {
+        let mut cbvg = ClearanceVisibilityGraph::new(self.config.cbvg.clone());
+
+        if !self.visibility.is_empty() {
+            cbvg.build_from_visibility(&self.visibility, self.line_store.lines());
+        } else {
+            cbvg.build(self.line_store.lines(), self.line_store.bounds());
+        }
+
+        cbvg
+    }
+
+    /// Find the nearest CBVG node to a target point.
+    ///
+    /// This is useful for exploration to find reachable navigation targets
+    /// near frontiers. CBVG nodes are guaranteed to be in visible, reachable
+    /// areas with proper wall clearance.
+    ///
+    /// # Arguments
+    /// * `target` - The point to find the nearest node to
+    /// * `max_distance` - Maximum distance to search (nodes farther are ignored)
+    ///
+    /// # Returns
+    /// The position of the nearest CBVG node, or None if no node is within range.
+    pub fn nearest_cbvg_node(&self, target: Point2D, max_distance: f32) -> Option<Point2D> {
+        let cbvg = self.build_cbvg();
+        cbvg.nearest_node(target, max_distance)
+    }
+
     // ─────────────────────────────────────────────────────────────────────────
     // Exploration Mode
     // ─────────────────────────────────────────────────────────────────────────
@@ -417,6 +474,19 @@ impl VectorMap {
 
         self.current_pose = final_pose;
 
+        // ─── Visibility Update ───
+        // Update visibility region with scan points in world frame
+        // robot_frame_points are already in scratch buffer
+        let world_scan_points: Vec<Point2D> = self
+            .observe_scratch
+            .robot_frame_points()
+            .iter()
+            .map(|&p| final_pose.transform_point(p))
+            .collect();
+        self.visibility
+            .update_from_scan(final_pose, &world_scan_points);
+        self.cbvg_dirty = true;
+
         // ─── Scan Storage (Exploration Mode) ───
         if let Some(ref mut store) = self.scan_store {
             store.add_scan(
@@ -499,6 +569,18 @@ impl VectorMap {
         // Fill scratch with robot frame points (used by feature extraction and loop closure)
         self.observe_scratch.fill_robot_frame_points(scan);
 
+        // ─── Visibility Update ───
+        // Update visibility region with scan points in world frame
+        let world_scan_points: Vec<Point2D> = self
+            .observe_scratch
+            .robot_frame_points()
+            .iter()
+            .map(|&p| odometry.transform_point(p))
+            .collect();
+        self.visibility
+            .update_from_scan(odometry, &world_scan_points);
+        self.cbvg_dirty = true;
+
         // ─── Scan Storage (Exploration Mode) ───
         if let Some(ref mut store) = self.scan_store {
             store.add_scan(
@@ -823,14 +905,23 @@ impl Map for VectorMap {
     }
 
     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,
-        )
+        // Use CBVG for path planning with visibility-aware node generation
+        // This ensures nodes are only placed in lidar-scanned areas
+        let mut cbvg = ClearanceVisibilityGraph::new(self.config.cbvg.clone());
+
+        // Use visibility-based build if we have visibility data
+        if !self.visibility.is_empty() {
+            cbvg.build_from_visibility(&self.visibility, self.line_store.lines());
+        } else {
+            // Fallback to wall-based build for legacy/testing
+            cbvg.build(self.line_store.lines(), self.line_store.bounds());
+        }
+
+        cbvg.find_path(from, to, self.line_store.lines())
+    }
+
+    fn nearest_cbvg_node(&self, target: Point2D, max_distance: f32) -> Option<Point2D> {
+        self.nearest_cbvg_node(target, max_distance)
     }
 
     fn is_path_clear(&self, from: Point2D, to: Point2D) -> bool {
@@ -838,7 +929,7 @@ impl Map for VectorMap {
             from,
             to,
             self.line_store.lines(),
-            self.config.path_planning.robot_radius,
+            self.config.frontier.robot_radius,
         )
     }
 
@@ -857,6 +948,10 @@ impl Map for VectorMap {
         self.loop_detector.clear();
         self.loop_closures.clear();
         self.icp_matcher.update_last_confidence(1.0);
+        self.cbvg.clear();
+        self.cbvg_dirty = true;
+        // Reset visibility to empty (create new instance with same resolution)
+        self.visibility = VisibilityRegion::new(self.config.cbvg.coverage_node_interval * 0.5);
     }
 }
 

vastu-map/src/vector_map/yaml.rs

@@ -260,10 +260,6 @@ frontier:
 occupancy:
   obstacle_distance: 0.1
   known_distance: 2.0
-path_planning:
-  robot_radius: 0.15
-  max_nodes: 1000
-  conservative: true
 loop_closure:
   min_travel_distance: 5.0
   max_descriptor_distance: 0.5
@@ -383,10 +379,6 @@ frontier:
 occupancy:
   obstacle_distance: 0.1
   known_distance: 2.0
-path_planning:
-  robot_radius: 0.15
-  max_nodes: 1000
-  conservative: true
 loop_closure:
   min_travel_distance: 5.0
   max_descriptor_distance: 0.5

vastu-map/tests/integration/harness.rs

@@ -32,7 +32,7 @@ use vastu_map::core::Pose2D;
 use vastu_map::core::math::normalize_angle;
 use vastu_map::integration::{ScanStore, ScanStoreConfig};
 use vastu_map::odometry::WheelOdometry;
-use vastu_map::query::{PathPlanningConfig, VisibilityGraph};
+use vastu_map::query::ClearanceVisibilityGraph;
 use vastu_map::{Map, Path as PlannedPath, VectorMap, VectorMapConfig};
 
 use crate::adapters::lidar_to_point_cloud;
@@ -186,22 +186,16 @@ impl HarnessConfig {
     }
 
     /// Create config for the medium_room map (8x8m with obstacles).
-    ///
-    /// Uses non-conservative path planning to allow paths to frontiers
-    /// (which are at map edges and may be in "unknown" space).
     pub fn medium_room() -> Self {
         let map_file = concat!(
             env!("CARGO_MANIFEST_DIR"),
             "/tests/integration/scenarios/maps/medium_room.yaml"
         );
-        // Load base config from YAML, then override path planning for exploration
-        let slam = load_slam_config()
-            .with_path_planning(PathPlanningConfig::default().with_conservative(false));
         Self {
             map_file: map_file.to_string(),
             start_x: 4.0,
             start_y: 4.0,
-            slam,
+            slam: load_slam_config(),
             ..Default::default()
         }
     }
@@ -631,17 +625,17 @@ impl TestHarness {
         }
     }
 
-    /// Finalize the test with visibility graph visualization.
+    /// Finalize the test with CBVG visualization.
     ///
-    /// Extended version of `finalize` that also renders the visibility graph
-    /// and planned path in the SVG output.
+    /// Extended version of `finalize` that also renders the clearance-based
+    /// visibility graph and planned path in the SVG output.
     ///
     /// # Arguments
-    /// * `graph` - Optional visibility graph to visualize
+    /// * `graph` - Optional CBVG to visualize
     /// * `path` - Optional planned path to visualize
     pub fn finalize_with_graph(
         &mut self,
-        graph: Option<&VisibilityGraph>,
+        graph: Option<&ClearanceVisibilityGraph>,
         path: Option<&PlannedPath>,
     ) -> TestResult {
         let final_pose = self.slam.current_pose();

vastu-map/tests/integration/scenarios/exploration_test.rs

@@ -34,7 +34,7 @@ use vastu_map::exploration::{
     VelocityCommand,
 };
 use vastu_map::integration::CoplanarMergeConfig;
-use vastu_map::query::{PathPlanner, PathPlanningConfig, VisibilityGraph};
+use vastu_map::query::cbvg::ClearanceVisibilityGraph;
 
 /// Maximum simulation time in seconds.
 const MAX_TIME: f32 = 200.0; // 3.3 minutes
@@ -173,8 +173,8 @@ mod tests {
         let mut collisions = 0;
         let mut virtual_walls = 0;
 
-        // Variables to capture the last visibility graph and path for visualization
-        let mut last_visibility_graph: Option<VisibilityGraph> = None;
+        // Variables to capture the last CBVG and path for visualization
+        let mut last_cbvg: Option<ClearanceVisibilityGraph> = None;
         let mut last_planned_path: Option<PlannedPath> = None;
 
         println!("Starting exploration with region-based TSP planning...");
@@ -254,16 +254,9 @@ mod tests {
                             path.points.len()
                         );
 
-                        // Capture visibility graph for this path for visualization
-                        let path_config = PathPlanningConfig::default()
-                            .with_robot_radius(config.robot_radius)
-                            .with_conservative(false);
-                        let planner = PathPlanner::new(path_config);
-                        last_visibility_graph = Some(planner.build_graph(
-                            robot_pos,
-                            *target_frontier,
-                            harness.slam().lines(),
-                        ));
+                        // Capture CBVG for visualization using visibility-based building
+                        // This ensures nodes are only in lidar-scanned areas
+                        last_cbvg = Some(harness.slam().build_cbvg());
                         last_planned_path = Some(path.clone());
                     }
 
@@ -362,9 +355,9 @@ mod tests {
             lines_merged
         );
 
-        // Finalize to generate visualization with visibility graph
+        // Finalize to generate visualization with CBVG
         let _test_result =
-            harness.finalize_with_graph(last_visibility_graph.as_ref(), last_planned_path.as_ref());
+            harness.finalize_with_graph(last_cbvg.as_ref(), last_planned_path.as_ref());
 
         ExplorationTestResult {
             completed: harness.slam().frontiers().is_empty(),

vastu-map/tests/integration/visualization.rs

@@ -20,7 +20,7 @@ use vastu_map::Path as PlannedPath;
 use vastu_map::VectorMap;
 use vastu_map::core::{Point2D, Pose2D};
 use vastu_map::integration::ScanStore;
-use vastu_map::query::VisibilityGraph;
+use vastu_map::query::{ClearanceVisibilityGraph, NodeType};
 
 use crate::harness::TrajectoryHistory;
 use crate::metrics::TestMetrics;
@@ -655,41 +655,44 @@ impl Visualizer {
         Group::new().add(body).add(direction)
     }
 
-    /// Render a visibility graph showing nodes and edges.
+    /// Render a clearance-based visibility graph showing nodes and edges.
     ///
     /// # Arguments
-    /// * `graph` - The visibility graph to render
+    /// * `graph` - The CBVG to render
     /// * `svg_height` - SVG canvas height for coordinate transformation
     /// * `show_edges` - Whether to show edges (can be noisy for large graphs)
-    pub fn render_visibility_graph(
+    pub fn render_cbvg(
         &self,
-        graph: &VisibilityGraph,
+        graph: &ClearanceVisibilityGraph,
         svg_height: f32,
         show_edges: bool,
     ) -> Group {
-        let mut group = Group::new().set("id", "visibility_graph");
+        let mut group = Group::new().set("id", "cbvg");
 
         let nodes = graph.nodes();
-        let start_idx = graph.start_idx();
-        let goal_idx = graph.goal_idx();
 
         // Render edges first (so nodes appear on top)
         if show_edges {
-            let edge_group = self.render_graph_edges(graph, svg_height);
+            let edge_group = self.render_cbvg_edges(graph, svg_height);
             group = group.add(edge_group);
         }
 
         // Render nodes
-        for (idx, &node) in nodes.iter().enumerate() {
-            let (sx, sy) = self.transform_point(node, 0.0, 0.0, svg_height);
+        for node in nodes {
+            let (sx, sy) = self.transform_point(node.position, 0.0, 0.0, svg_height);
 
-            // Choose color based on node type
-            let (color, radius, opacity) = if idx == start_idx {
-                (colors::START_NODE, 6.0, 0.9)
-            } else if idx == goal_idx {
-                (colors::GOAL_NODE, 6.0, 0.9)
-            } else {
-                (colors::GRAPH_NODE, 4.0, 0.7)
+            // Choose color and size based on node type
+            let (color, radius, opacity) = match node.node_type {
+                NodeType::Robot => (colors::START_NODE, 6.0, 0.9),
+                NodeType::Goal => (colors::GOAL_NODE, 6.0, 0.9),
+                NodeType::Junction => ("#9966FF", 5.0, 0.8), // Purple for junctions
+                NodeType::MedialAxis => {
+                    if node.is_narrow_passage {
+                        ("#FF9900", 4.0, 0.7) // Orange for narrow passage nodes
+                    } else {
+                        (colors::GRAPH_NODE, 4.0, 0.7) // Default color for medial axis
+                    }
+                }
             };
 
             let circle = Circle::new()
@@ -705,29 +708,34 @@ impl Visualizer {
         group
     }
 
-    /// Render visibility graph edges.
-    fn render_graph_edges(&self, graph: &VisibilityGraph, svg_height: f32) -> Group {
-        let mut group = Group::new().set("id", "graph_edges");
+    /// Render CBVG edges.
+    fn render_cbvg_edges(&self, graph: &ClearanceVisibilityGraph, svg_height: f32) -> Group {
+        let mut group = Group::new().set("id", "cbvg_edges");
 
         let nodes = graph.nodes();
-        let edge_list = graph.edge_list();
+        let edges = graph.edges();
 
-        for (from_idx, to_idx, _dist) in edge_list {
-            let from_node = nodes[from_idx];
-            let to_node = nodes[to_idx];
+        for (from_idx, neighbors) in edges.iter().enumerate() {
+            for (to_idx, _dist) in neighbors {
+                // Only draw each edge once (when from_idx < to_idx)
+                if from_idx < *to_idx {
+                    let from_node = nodes[from_idx].position;
+                    let to_node = nodes[*to_idx].position;
 
-            let line = self.create_line(
-                from_node,
-                to_node,
-                0.0,
-                0.0,
-                svg_height,
-                colors::GRAPH_EDGE,
-                0.5,
-                Some("2,2"), // Dashed line for edges
-            );
+                    let line = self.create_line(
+                        from_node,
+                        to_node,
+                        0.0,
+                        0.0,
+                        svg_height,
+                        colors::GRAPH_EDGE,
+                        0.5,
+                        Some("2,2"), // Dashed line for edges
+                    );
 
-            group = group.add(line.set("opacity", 0.3));
+                    group = group.add(line.set("opacity", 0.3));
+                }
+            }
         }
 
         group
@@ -791,9 +799,9 @@ impl Visualizer {
         group
     }
 
-    /// Render full visualization with visibility graph.
+    /// Render full visualization with clearance-based visibility graph.
     ///
-    /// Extended version of `render_full` that also shows the visibility graph
+    /// Extended version of `render_full` that also shows the CBVG
     /// and planned path for path planning debugging.
     #[allow(clippy::too_many_arguments)]
     pub fn render_with_path_planning(
@@ -805,7 +813,7 @@ impl Visualizer {
         scan_store: &ScanStore,
         trajectory: &TrajectoryHistory,
         metrics: &TestMetrics,
-        graph: Option<&VisibilityGraph>,
+        graph: Option<&ClearanceVisibilityGraph>,
         path: Option<&PlannedPath>,
     ) {
         // Compute bounds from map
@@ -850,9 +858,9 @@ impl Visualizer {
         // Layer 4: Detected walls (teal)
         doc = doc.add(self.render_detected(slam, height as f32));
 
-        // Layer 5: Visibility graph (if provided)
+        // Layer 5: Clearance-based visibility graph (if provided)
         if let Some(g) = graph {
-            doc = doc.add(self.render_visibility_graph(g, height as f32, true));
+            doc = doc.add(self.render_cbvg(g, height as f32, true));
         }
 
         // Layer 6: Planned path (if provided)
@@ -884,15 +892,15 @@ impl Visualizer {
         svg::save(&self.output_path, &doc).expect("Failed to save SVG");
     }
 
-    /// Render standalone visibility graph visualization.
+    /// Render standalone CBVG visualization.
     ///
-    /// Creates a simple SVG showing just the visibility graph, detected walls,
+    /// Creates a simple SVG showing just the CBVG, detected walls,
     /// and optionally a planned path. Useful for path planning debugging.
     #[allow(dead_code)]
     pub fn render_graph_only(
         &self,
         slam: &VectorMap,
-        graph: &VisibilityGraph,
+        graph: &ClearanceVisibilityGraph,
         path: Option<&PlannedPath>,
         bounds: (f32, f32, f32, f32), // (min_x, min_y, max_x, max_y)
     ) {
@@ -921,8 +929,8 @@ impl Visualizer {
         // Detected walls
         doc = doc.add(self.render_detected(slam, height as f32));
 
-        // Visibility graph
-        doc = doc.add(self.render_visibility_graph(graph, height as f32, true));
+        // CBVG
+        doc = doc.add(self.render_cbvg(graph, height as f32, true));
 
         // Planned path (if provided)
         if let Some(p) = path {