rust_robotics_planning/
path_smoothing.rs

1//! Path smoothing via line-of-sight shortcutting
2//!
3//! Removes redundant waypoints from a grid-based path by checking direct
4//! visibility between non-adjacent points. Works with any grid planner output
5//! but is especially useful for DFS, which produces long, winding paths.
6
7use crate::grid::GridMap;
8use rust_robotics_core::{Path2D, Point2D};
9
10/// Smooth a path by removing intermediate waypoints that can be skipped
11/// via direct line-of-sight.
12///
13/// Uses a greedy forward scan: from the current anchor point, find the
14/// farthest reachable point with line-of-sight, advance the anchor there,
15/// and repeat.
16pub fn smooth_path(path: &Path2D, grid_map: &GridMap) -> Path2D {
17    if path.len() <= 2 {
18        return path.clone();
19    }
20
21    let points = &path.points;
22    let mut smoothed = Vec::new();
23    smoothed.push(points[0]);
24
25    let mut anchor = 0;
26    while anchor < points.len() - 1 {
27        // Find the farthest point visible from anchor
28        let mut farthest = anchor + 1;
29        for candidate in (anchor + 2)..points.len() {
30            if line_of_sight_world(grid_map, &points[anchor], &points[candidate]) {
31                farthest = candidate;
32            }
33        }
34        smoothed.push(points[farthest]);
35        anchor = farthest;
36    }
37
38    Path2D::from_points(smoothed)
39}
40
41/// Check line-of-sight between two world-coordinate points on a grid.
42/// Uses DDA ray marching identical to Theta*'s line_of_sight.
43fn line_of_sight_world(grid_map: &GridMap, from: &Point2D, to: &Point2D) -> bool {
44    let x0 = grid_map.calc_x_index(from.x);
45    let y0 = grid_map.calc_y_index(from.y);
46    let x1 = grid_map.calc_x_index(to.x);
47    let y1 = grid_map.calc_y_index(to.y);
48
49    if !grid_map.is_valid(x0, y0) || !grid_map.is_valid(x1, y1) {
50        return false;
51    }
52
53    if x0 == x1 && y0 == y1 {
54        return true;
55    }
56
57    let dx = x1 - x0;
58    let dy = y1 - y0;
59    let step_x = dx.signum();
60    let step_y = dy.signum();
61    let abs_dx = dx.abs() as f64;
62    let abs_dy = dy.abs() as f64;
63
64    let mut x = x0;
65    let mut y = y0;
66    let mut t_max_x = if step_x != 0 {
67        0.5 / abs_dx
68    } else {
69        f64::INFINITY
70    };
71    let mut t_max_y = if step_y != 0 {
72        0.5 / abs_dy
73    } else {
74        f64::INFINITY
75    };
76    let t_delta_x = if step_x != 0 {
77        1.0 / abs_dx
78    } else {
79        f64::INFINITY
80    };
81    let t_delta_y = if step_y != 0 {
82        1.0 / abs_dy
83    } else {
84        f64::INFINITY
85    };
86
87    while x != x1 || y != y1 {
88        let advance_x = t_max_x <= t_max_y;
89        let advance_y = t_max_y <= t_max_x;
90        let next_x = if advance_x { x + step_x } else { x };
91        let next_y = if advance_y { y + step_y } else { y };
92
93        if !grid_map.is_valid_step(x, y, next_x, next_y) {
94            return false;
95        }
96
97        x = next_x;
98        y = next_y;
99
100        if advance_x {
101            t_max_x += t_delta_x;
102        }
103        if advance_y {
104            t_max_y += t_delta_y;
105        }
106    }
107
108    true
109}
110
111/// Full pipeline: shortcutting + waypoint relaxation.
112///
113/// 1. Greedy LOS shortcutting (removes redundant waypoints)
114/// 2. Iterative waypoint perturbation (moves each interior waypoint to
115///    minimize total path length while maintaining LOS constraints)
116/// 3. Final shortcutting pass (removes any waypoints made redundant by relaxation)
117pub fn optimize_path(path: &Path2D, grid_map: &GridMap, max_iterations: usize) -> Path2D {
118    let mut current = smooth_path(path, grid_map);
119    if current.len() <= 2 {
120        return current;
121    }
122
123    for _ in 0..max_iterations {
124        let improved = relax_waypoints(&current, grid_map);
125        let improved = smooth_path(&improved, grid_map);
126        if (improved.total_length() - current.total_length()).abs() < 1e-6 {
127            break;
128        }
129        current = improved;
130    }
131
132    current
133}
134
135/// Move each interior waypoint toward the line between its neighbors,
136/// minimizing local path length while maintaining LOS.
137fn relax_waypoints(path: &Path2D, grid_map: &GridMap) -> Path2D {
138    if path.len() <= 2 {
139        return path.clone();
140    }
141
142    let mut points = path.points.clone();
143    let resolution = grid_map.resolution;
144
145    for i in 1..points.len() - 1 {
146        let prev = points[i - 1];
147        let next = points[i + 1];
148
149        // Target: midpoint of the line between prev and next
150        let mid_x = (prev.x + next.x) / 2.0;
151        let mid_y = (prev.y + next.y) / 2.0;
152
153        // Try to move waypoint toward the midpoint in steps
154        let current = points[i];
155        let dx = mid_x - current.x;
156        let dy = mid_y - current.y;
157
158        // Try progressively smaller steps toward midpoint
159        for &step_frac in &[1.0, 0.5, 0.25, 0.125] {
160            let candidate = Point2D::new(current.x + dx * step_frac, current.y + dy * step_frac);
161
162            // Snap to grid resolution
163            let snapped = Point2D::new(
164                (candidate.x / resolution).round() * resolution,
165                (candidate.y / resolution).round() * resolution,
166            );
167
168            // Check grid validity
169            let gx = grid_map.calc_x_index(snapped.x);
170            let gy = grid_map.calc_y_index(snapped.y);
171            if !grid_map.is_valid(gx, gy) {
172                continue;
173            }
174
175            // Check LOS to both neighbors
176            if !line_of_sight_world(grid_map, &prev, &snapped) {
177                continue;
178            }
179            if !line_of_sight_world(grid_map, &snapped, &next) {
180                continue;
181            }
182
183            // Check if this improves total local length
184            let old_len = dist(&prev, &current) + dist(&current, &next);
185            let new_len = dist(&prev, &snapped) + dist(&snapped, &next);
186            if new_len < old_len - 1e-10 {
187                points[i] = snapped;
188                break;
189            }
190        }
191    }
192
193    Path2D::from_points(points)
194}
195
196fn dist(a: &Point2D, b: &Point2D) -> f64 {
197    ((a.x - b.x).powi(2) + (a.y - b.y).powi(2)).sqrt()
198}
199
200#[cfg(test)]
201mod tests {
202    use super::*;
203    use rust_robotics_core::Obstacles;
204
205    fn create_simple_obstacles() -> (Obstacles, GridMap) {
206        let mut ox = Vec::new();
207        let mut oy = Vec::new();
208        for i in 0..21 {
209            ox.push(i as f64);
210            oy.push(0.0);
211            ox.push(i as f64);
212            oy.push(20.0);
213            ox.push(0.0);
214            oy.push(i as f64);
215            ox.push(20.0);
216            oy.push(i as f64);
217        }
218        for i in 5..15 {
219            ox.push(10.0);
220            oy.push(i as f64);
221        }
222        let obstacles = Obstacles::try_from_xy(&ox, &oy).unwrap();
223        let grid_map = GridMap::try_new(&ox, &oy, 1.0, 0.5).unwrap();
224        (obstacles, grid_map)
225    }
226
227    #[test]
228    fn test_smooth_path_reduces_waypoints() {
229        let (_obstacles, grid_map) = create_simple_obstacles();
230        // Simulate a DFS-like zigzag path in an open area
231        let zigzag = Path2D::from_points(vec![
232            Point2D::new(2.0, 2.0),
233            Point2D::new(3.0, 2.0),
234            Point2D::new(4.0, 2.0),
235            Point2D::new(5.0, 2.0),
236            Point2D::new(6.0, 2.0),
237            Point2D::new(7.0, 2.0),
238            Point2D::new(8.0, 2.0),
239        ]);
240        let smoothed = smooth_path(&zigzag, &grid_map);
241        assert!(
242            smoothed.len() <= 2,
243            "straight line should collapse to 2 points, got {}",
244            smoothed.len()
245        );
246        assert!((smoothed.points.first().unwrap().x - 2.0).abs() < 1e-6);
247        assert!((smoothed.points.last().unwrap().x - 8.0).abs() < 1e-6);
248    }
249
250    #[test]
251    fn test_smooth_path_preserves_endpoints() {
252        let (_obstacles, grid_map) = create_simple_obstacles();
253        let path = Path2D::from_points(vec![
254            Point2D::new(2.0, 2.0),
255            Point2D::new(3.0, 3.0),
256            Point2D::new(4.0, 4.0),
257        ]);
258        let smoothed = smooth_path(&path, &grid_map);
259        let first = smoothed.points.first().unwrap();
260        let last = smoothed.points.last().unwrap();
261        assert!((first.x - 2.0).abs() < 1e-6);
262        assert!((first.y - 2.0).abs() < 1e-6);
263        assert!((last.x - 4.0).abs() < 1e-6);
264        assert!((last.y - 4.0).abs() < 1e-6);
265    }
266
267    #[test]
268    fn test_smooth_path_respects_obstacles() {
269        let (_obstacles, grid_map) = create_simple_obstacles();
270        // Path that goes around the wall at x=10, y=5..15
271        let path = Path2D::from_points(vec![
272            Point2D::new(8.0, 10.0),
273            Point2D::new(8.0, 16.0),
274            Point2D::new(12.0, 16.0),
275            Point2D::new(12.0, 10.0),
276        ]);
277        let smoothed = smooth_path(&path, &grid_map);
278        // Should not shortcut through the wall
279        assert!(
280            smoothed.len() >= 3,
281            "should not shortcut through wall, got {} waypoints",
282            smoothed.len()
283        );
284    }
285
286    #[test]
287    fn test_smooth_path_with_dfs() {
288        let (obstacles, _grid_map) = create_simple_obstacles();
289        let dfs_planner = crate::depth_first_search::DFSPlanner::from_obstacle_points(
290            &obstacles,
291            crate::depth_first_search::DFSConfig::default(),
292        )
293        .unwrap();
294        let a_star_planner = crate::a_star::AStarPlanner::from_obstacle_points(
295            &obstacles,
296            crate::a_star::AStarConfig::default(),
297        )
298        .unwrap();
299
300        let start = Point2D::new(2.0, 2.0);
301        let goal = Point2D::new(18.0, 18.0);
302        let dfs_path = dfs_planner.plan(start, goal).unwrap();
303        let a_star_path = a_star_planner.plan(start, goal).unwrap();
304        let smoothed = smooth_path(&dfs_path, dfs_planner.grid_map());
305
306        // Smoothed DFS should have far fewer waypoints than raw DFS
307        assert!(
308            smoothed.len() < dfs_path.len(),
309            "smoothed ({}) should be shorter than raw DFS ({})",
310            smoothed.len(),
311            dfs_path.len()
312        );
313        // Smoothed DFS path length should be reasonable (within 20% of A*)
314        let ratio = smoothed.total_length() / a_star_path.total_length();
315        assert!(
316            ratio < 1.2,
317            "smoothed DFS length ({:.2}) should be within 20% of A* ({:.2}), ratio = {:.4}",
318            smoothed.total_length(),
319            a_star_path.total_length(),
320            ratio
321        );
322    }
323
324    #[test]
325    fn test_smooth_empty_and_single_point() {
326        let (_obstacles, grid_map) = create_simple_obstacles();
327        let empty = Path2D::from_points(vec![]);
328        assert!(smooth_path(&empty, &grid_map).is_empty());
329
330        let single = Path2D::from_points(vec![Point2D::new(5.0, 5.0)]);
331        assert_eq!(smooth_path(&single, &grid_map).len(), 1);
332    }
333}
rust_robotics_planning/path_smoothing.rs

rust_robotics_planning/
path_smoothing.rs
