rust_robotics_planning/
anya.rs

1//! Anya-inspired optimal any-angle pathfinding
2//!
3//! This module implements optimal any-angle pathfinding using a visibility
4//! graph approach on grid corner points. Instead of expanding intervals
5//! (as in the original Anya algorithm), we build a visibility graph over
6//! obstacle corner points and run Dijkstra's algorithm to find the
7//! shortest euclidean path.
8//!
9//! The result is the same optimal any-angle path that Anya would produce,
10//! since both methods find the shortest path through free space whose
11//! waypoints lie on obstacle corners.
12//!
13//! Reference: Harabor, Grastien, Oz, Aksakalli (2016)
14//! "Optimal Any-Angle Pathfinding In Practice", Journal of AI Research.
15
16use std::cmp::Ordering;
17use std::collections::BinaryHeap;
18
19use crate::grid::GridMap;
20use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
21
22/// Configuration for Anya planner
23#[derive(Debug, Clone)]
24pub struct AnyaConfig {
25    pub resolution: f64,
26    pub robot_radius: f64,
27    pub heuristic_weight: f64,
28}
29
30impl Default for AnyaConfig {
31    fn default() -> Self {
32        Self {
33            resolution: 1.0,
34            robot_radius: 0.5,
35            heuristic_weight: 1.0,
36        }
37    }
38}
39
40impl AnyaConfig {
41    pub fn validate(&self) -> RoboticsResult<()> {
42        if !self.resolution.is_finite() || self.resolution <= 0.0 {
43            return Err(RoboticsError::InvalidParameter(format!(
44                "resolution must be positive and finite, got {}",
45                self.resolution
46            )));
47        }
48        if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
49            return Err(RoboticsError::InvalidParameter(format!(
50                "robot_radius must be non-negative and finite, got {}",
51                self.robot_radius
52            )));
53        }
54        if !self.heuristic_weight.is_finite() || self.heuristic_weight <= 0.0 {
55            return Err(RoboticsError::InvalidParameter(format!(
56                "heuristic_weight must be positive and finite, got {}",
57                self.heuristic_weight
58            )));
59        }
60        Ok(())
61    }
62}
63
64#[derive(Debug)]
65struct DijkstraNode {
66    node_id: usize,
67    cost: f64,
68}
69
70impl Eq for DijkstraNode {}
71impl PartialEq for DijkstraNode {
72    fn eq(&self, other: &Self) -> bool {
73        self.cost == other.cost
74    }
75}
76impl Ord for DijkstraNode {
77    fn cmp(&self, other: &Self) -> Ordering {
78        other
79            .cost
80            .partial_cmp(&self.cost)
81            .unwrap_or(Ordering::Equal)
82    }
83}
84impl PartialOrd for DijkstraNode {
85    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
86        Some(self.cmp(other))
87    }
88}
89
90pub struct AnyaPlanner {
91    grid_map: GridMap,
92    #[allow(dead_code)]
93    config: AnyaConfig,
94}
95
96impl AnyaPlanner {
97    pub fn new(ox: &[f64], oy: &[f64], config: AnyaConfig) -> Self {
98        Self::try_new(ox, oy, config).expect(
99            "invalid Anya planner input: obstacle list must be non-empty and valid, and config values must be positive/finite",
100        )
101    }
102
103    pub fn try_new(ox: &[f64], oy: &[f64], config: AnyaConfig) -> RoboticsResult<Self> {
104        config.validate()?;
105        let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
106        Ok(AnyaPlanner { grid_map, config })
107    }
108
109    pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
110        let config = AnyaConfig {
111            resolution,
112            robot_radius,
113            ..Default::default()
114        };
115        Self::new(ox, oy, config)
116    }
117
118    pub fn from_obstacle_points(obstacles: &Obstacles, config: AnyaConfig) -> RoboticsResult<Self> {
119        config.validate()?;
120        let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
121        Ok(AnyaPlanner { grid_map, config })
122    }
123
124    #[deprecated(note = "use plan() or plan_xy() instead")]
125    pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
126        match self.plan_xy(sx, sy, gx, gy) {
127            Ok(path) => Some((path.x_coords(), path.y_coords())),
128            Err(_) => None,
129        }
130    }
131
132    pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
133        self.plan_impl(start, goal)
134    }
135
136    pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
137        self.plan_impl(Point2D::new(sx, sy), Point2D::new(gx, gy))
138    }
139
140    pub fn grid_map(&self) -> &GridMap {
141        &self.grid_map
142    }
143
144    /// Line-of-sight check using DDA ray marching (same algorithm as theta_star).
145    fn line_of_sight(&self, x0: i32, y0: i32, x1: i32, y1: i32) -> bool {
146        if !self.grid_map.is_valid(x0, y0) || !self.grid_map.is_valid(x1, y1) {
147            return false;
148        }
149
150        if x0 == x1 && y0 == y1 {
151            return true;
152        }
153
154        let dx = x1 - x0;
155        let dy = y1 - y0;
156        let step_x = dx.signum();
157        let step_y = dy.signum();
158        let abs_dx = dx.abs() as f64;
159        let abs_dy = dy.abs() as f64;
160
161        let mut x = x0;
162        let mut y = y0;
163        let mut t_max_x = if step_x != 0 {
164            0.5 / abs_dx
165        } else {
166            f64::INFINITY
167        };
168        let mut t_max_y = if step_y != 0 {
169            0.5 / abs_dy
170        } else {
171            f64::INFINITY
172        };
173        let t_delta_x = if step_x != 0 {
174            1.0 / abs_dx
175        } else {
176            f64::INFINITY
177        };
178        let t_delta_y = if step_y != 0 {
179            1.0 / abs_dy
180        } else {
181            f64::INFINITY
182        };
183
184        while x != x1 || y != y1 {
185            let advance_x = t_max_x <= t_max_y;
186            let advance_y = t_max_y <= t_max_x;
187            let next_x = if advance_x { x + step_x } else { x };
188            let next_y = if advance_y { y + step_y } else { y };
189
190            if !self.grid_map.is_valid_step(x, y, next_x, next_y) {
191                return false;
192            }
193
194            x = next_x;
195            y = next_y;
196
197            if advance_x {
198                t_max_x += t_delta_x;
199            }
200            if advance_y {
201                t_max_y += t_delta_y;
202            }
203        }
204
205        true
206    }
207
208    /// Find all grid corner points adjacent to at least one obstacle and
209    /// at least one free cell. These are the candidate waypoints for the
210    /// optimal any-angle path.
211    /// Collect ALL free cells as visibility-graph nodes.
212    ///
213    /// For a grid with no-corner-cutting semantics, the optimal any-angle
214    /// path may turn at any free cell, not just obstacle-adjacent corners.
215    /// Using all free cells guarantees the visibility graph produces the
216    /// true optimal any-angle path (at the cost of O(V^2) LOS checks).
217    fn find_all_free_cells(&self) -> Vec<(i32, i32)> {
218        let mut cells = Vec::new();
219        for ix in 0..self.grid_map.x_width {
220            for iy in 0..self.grid_map.y_width {
221                if self.grid_map.is_valid(ix, iy) {
222                    cells.push((ix, iy));
223                }
224            }
225        }
226        cells
227    }
228
229    fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
230        if self.grid_map.is_valid(x, y) {
231            return Ok(());
232        }
233        Err(RoboticsError::PlanningError(format!(
234            "{} position is invalid",
235            label
236        )))
237    }
238
239    fn plan_impl(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
240        let start_x = self.grid_map.calc_x_index(start.x);
241        let start_y = self.grid_map.calc_y_index(start.y);
242        let goal_x = self.grid_map.calc_x_index(goal.x);
243        let goal_y = self.grid_map.calc_y_index(goal.y);
244
245        self.ensure_query_is_valid(start_x, start_y, "Start")?;
246        self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;
247
248        // Direct line of sight => straight line path
249        if self.line_of_sight(start_x, start_y, goal_x, goal_y) {
250            return Ok(Path2D::from_points(vec![
251                Point2D::new(
252                    self.grid_map.calc_x_position(start_x),
253                    self.grid_map.calc_y_position(start_y),
254                ),
255                Point2D::new(
256                    self.grid_map.calc_x_position(goal_x),
257                    self.grid_map.calc_y_position(goal_y),
258                ),
259            ]));
260        }
261
262        // Build visibility graph nodes: ALL free cells (guarantees optimality)
263        let mut nodes: Vec<(i32, i32)> = self.find_all_free_cells();
264
265        // Add start and goal if not already present
266        let start_pos = (start_x, start_y);
267        let goal_pos = (goal_x, goal_y);
268        if !nodes.contains(&start_pos) {
269            nodes.push(start_pos);
270        }
271        if !nodes.contains(&goal_pos) {
272            nodes.push(goal_pos);
273        }
274
275        let n = nodes.len();
276        let start_id = nodes.iter().position(|&p| p == start_pos).unwrap();
277        let goal_id = nodes.iter().position(|&p| p == goal_pos).unwrap();
278
279        // Build adjacency list: for each pair, check LOS and store distance
280        let mut adj: Vec<Vec<(usize, f64)>> = vec![Vec::new(); n];
281        for i in 0..n {
282            for j in (i + 1)..n {
283                let (x0, y0) = nodes[i];
284                let (x1, y1) = nodes[j];
285                if self.line_of_sight(x0, y0, x1, y1) {
286                    let dist = (((x0 - x1).pow(2) + (y0 - y1).pow(2)) as f64).sqrt();
287                    adj[i].push((j, dist));
288                    adj[j].push((i, dist));
289                }
290            }
291        }
292
293        // Dijkstra
294        let mut dist = vec![f64::INFINITY; n];
295        let mut prev: Vec<Option<usize>> = vec![None; n];
296        let mut visited = vec![false; n];
297        dist[start_id] = 0.0;
298
299        let mut heap = BinaryHeap::new();
300        heap.push(DijkstraNode {
301            node_id: start_id,
302            cost: 0.0,
303        });
304
305        while let Some(current) = heap.pop() {
306            if current.node_id == goal_id {
307                break;
308            }
309            if visited[current.node_id] {
310                continue;
311            }
312            visited[current.node_id] = true;
313
314            for &(neighbor, weight) in &adj[current.node_id] {
315                let new_dist = dist[current.node_id] + weight;
316                if new_dist < dist[neighbor] {
317                    dist[neighbor] = new_dist;
318                    prev[neighbor] = Some(current.node_id);
319                    heap.push(DijkstraNode {
320                        node_id: neighbor,
321                        cost: new_dist,
322                    });
323                }
324            }
325        }
326
327        if dist[goal_id].is_infinite() {
328            return Err(RoboticsError::PlanningError("No path found".to_string()));
329        }
330
331        // Reconstruct path
332        let mut path_indices = Vec::new();
333        let mut current = goal_id;
334        loop {
335            path_indices.push(current);
336            match prev[current] {
337                Some(p) => current = p,
338                None => break,
339            }
340        }
341        path_indices.reverse();
342
343        let points: Vec<Point2D> = path_indices
344            .iter()
345            .map(|&id| {
346                let (ix, iy) = nodes[id];
347                Point2D::new(
348                    self.grid_map.calc_x_position(ix),
349                    self.grid_map.calc_y_position(iy),
350                )
351            })
352            .collect();
353
354        Ok(Path2D::from_points(points))
355    }
356}
357
358impl PathPlanner for AnyaPlanner {
359    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
360        self.plan_impl(start, goal)
361    }
362}
363
364#[cfg(test)]
365mod tests {
366    use super::*;
367    use rust_robotics_core::Obstacles;
368
369    fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
370        let mut ox = Vec::new();
371        let mut oy = Vec::new();
372        for i in 0..21 {
373            ox.push(i as f64);
374            oy.push(0.0);
375            ox.push(i as f64);
376            oy.push(20.0);
377            ox.push(0.0);
378            oy.push(i as f64);
379            ox.push(20.0);
380            oy.push(i as f64);
381        }
382        for i in 5..15 {
383            ox.push(10.0);
384            oy.push(i as f64);
385        }
386        (ox, oy)
387    }
388
389    #[test]
390    fn test_anya_finds_path() {
391        let (ox, oy) = create_simple_obstacles();
392        let planner = AnyaPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
393        let result = planner.plan(Point2D::new(2.0, 10.0), Point2D::new(18.0, 10.0));
394        assert!(result.is_ok());
395        let path = result.unwrap();
396        assert!(!path.is_empty());
397        // Path should start near start and end near goal
398        let xs = path.x_coords();
399        let ys = path.y_coords();
400        assert!((xs[0] - 2.0).abs() < 1.5);
401        assert!((ys[0] - 10.0).abs() < 1.5);
402        assert!((*xs.last().unwrap() - 18.0).abs() < 1.5);
403        assert!((*ys.last().unwrap() - 10.0).abs() < 1.5);
404    }
405
406    #[test]
407    fn test_anya_shorter_than_a_star() {
408        let (ox, oy) = create_simple_obstacles();
409        let anya_planner = AnyaPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
410        let a_star_planner = crate::a_star::AStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
411        let start = Point2D::new(2.0, 2.0);
412        let goal = Point2D::new(18.0, 18.0);
413        let anya_path = anya_planner.plan(start, goal).unwrap();
414        let a_star_path = a_star_planner.plan(start, goal).unwrap();
415        let anya_length = anya_path.total_length();
416        let a_star_length = a_star_path.total_length();
417        assert!(
418            anya_length <= a_star_length + 0.1,
419            "Anya path ({}) should not be longer than A* path ({})",
420            anya_length,
421            a_star_length
422        );
423    }
424
425    #[test]
426    fn test_anya_optimal_straight_line() {
427        // Open grid with only boundary obstacles, diagonal path should be
428        // close to euclidean distance.
429        let mut ox = Vec::new();
430        let mut oy = Vec::new();
431        for i in 0..21 {
432            ox.push(i as f64);
433            oy.push(0.0);
434            ox.push(i as f64);
435            oy.push(20.0);
436            ox.push(0.0);
437            oy.push(i as f64);
438            ox.push(20.0);
439            oy.push(i as f64);
440        }
441        let planner = AnyaPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
442        let start = Point2D::new(2.0, 2.0);
443        let goal = Point2D::new(18.0, 18.0);
444        let path = planner.plan(start, goal).unwrap();
445        let path_length = path.total_length();
446        let euclidean = ((18.0 - 2.0_f64).powi(2) + (18.0 - 2.0_f64).powi(2)).sqrt();
447        assert!(
448            (path_length - euclidean).abs() < 1.0,
449            "Straight-line path length ({}) should be close to euclidean distance ({})",
450            path_length,
451            euclidean
452        );
453    }
454
455    #[test]
456    fn test_anya_from_obstacle_points() {
457        let (ox, oy) = create_simple_obstacles();
458        let obstacles = Obstacles::try_from_xy(&ox, &oy).unwrap();
459        let planner = AnyaPlanner::from_obstacle_points(&obstacles, AnyaConfig::default()).unwrap();
460        let path = planner.plan_xy(2.0, 10.0, 18.0, 10.0).unwrap();
461        assert!(!path.is_empty());
462    }
463}
rust_robotics_planning/anya.rs

rust_robotics_planning/
anya.rs
