rust_robotics_planning/
lazy_theta_star.rs

1//! Lazy Theta* path planning algorithm
2//!
3//! Lazy Theta* defers line-of-sight checks until node expansion, reducing
4//! the number of expensive visibility tests. When a node is generated as a
5//! neighbor, it optimistically assumes line-of-sight to the grandparent exists.
6//! When the node is later popped for expansion, the line-of-sight is verified;
7//! if it fails, the node's parent is corrected to the grid neighbor with the
8//! lowest g-value.
9//!
10//! Reference: Nash, A., Koenig, S., & Tovey, C. (2010).
11//! "Lazy Theta*: Any-Angle Path Planning and Path Length Analysis in 3D"
12
13use std::cmp::Ordering;
14use std::collections::{BinaryHeap, HashMap};
15
16use crate::grid::{GridMap, Node};
17use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
18
19/// Configuration for Lazy Theta* planner
20#[derive(Debug, Clone)]
21pub struct LazyThetaStarConfig {
22    pub resolution: f64,
23    pub robot_radius: f64,
24    pub heuristic_weight: f64,
25}
26
27impl Default for LazyThetaStarConfig {
28    fn default() -> Self {
29        Self {
30            resolution: 1.0,
31            robot_radius: 0.5,
32            heuristic_weight: 1.0,
33        }
34    }
35}
36
37impl LazyThetaStarConfig {
38    pub fn validate(&self) -> RoboticsResult<()> {
39        if !self.resolution.is_finite() || self.resolution <= 0.0 {
40            return Err(RoboticsError::InvalidParameter(format!(
41                "resolution must be positive and finite, got {}",
42                self.resolution
43            )));
44        }
45        if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
46            return Err(RoboticsError::InvalidParameter(format!(
47                "robot_radius must be non-negative and finite, got {}",
48                self.robot_radius
49            )));
50        }
51        if !self.heuristic_weight.is_finite() || self.heuristic_weight <= 0.0 {
52            return Err(RoboticsError::InvalidParameter(format!(
53                "heuristic_weight must be positive and finite, got {}",
54                self.heuristic_weight
55            )));
56        }
57        Ok(())
58    }
59}
60
61#[derive(Debug)]
62struct PriorityNode {
63    x: i32,
64    y: i32,
65    #[allow(dead_code)]
66    cost: f64,
67    priority: f64,
68    index: usize,
69}
70impl Eq for PriorityNode {}
71impl PartialEq for PriorityNode {
72    fn eq(&self, other: &Self) -> bool {
73        self.priority == other.priority
74    }
75}
76impl Ord for PriorityNode {
77    fn cmp(&self, other: &Self) -> Ordering {
78        other
79            .priority
80            .partial_cmp(&self.priority)
81            .unwrap_or(Ordering::Equal)
82    }
83}
84impl PartialOrd for PriorityNode {
85    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
86        Some(self.cmp(other))
87    }
88}
89
90pub struct LazyThetaStarPlanner {
91    grid_map: GridMap,
92    config: LazyThetaStarConfig,
93    motion: Vec<(i32, i32, f64)>,
94}
95
96impl LazyThetaStarPlanner {
97    pub fn new(ox: &[f64], oy: &[f64], config: LazyThetaStarConfig) -> Self {
98        Self::try_new(ox, oy, config).expect(
99            "invalid Lazy Theta* 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: LazyThetaStarConfig) -> RoboticsResult<Self> {
104        config.validate()?;
105        let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
106        let motion = Self::get_motion_model();
107        Ok(LazyThetaStarPlanner {
108            grid_map,
109            config,
110            motion,
111        })
112    }
113
114    pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
115        let config = LazyThetaStarConfig {
116            resolution,
117            robot_radius,
118            ..Default::default()
119        };
120        Self::new(ox, oy, config)
121    }
122
123    pub fn from_obstacle_points(
124        obstacles: &Obstacles,
125        config: LazyThetaStarConfig,
126    ) -> RoboticsResult<Self> {
127        config.validate()?;
128        let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
129        let motion = Self::get_motion_model();
130        Ok(LazyThetaStarPlanner {
131            grid_map,
132            config,
133            motion,
134        })
135    }
136
137    #[deprecated(note = "use plan() or plan_xy() instead")]
138    pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
139        match self.plan_xy(sx, sy, gx, gy) {
140            Ok(path) => Some((path.x_coords(), path.y_coords())),
141            Err(_) => None,
142        }
143    }
144
145    pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
146        self.plan_impl(start, goal)
147    }
148
149    pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
150        self.plan_impl(Point2D::new(sx, sy), Point2D::new(gx, gy))
151    }
152
153    pub fn grid_map(&self) -> &GridMap {
154        &self.grid_map
155    }
156
157    fn calc_heuristic(&self, n1_x: i32, n1_y: i32, n2_x: i32, n2_y: i32) -> f64 {
158        self.config.heuristic_weight * (((n1_x - n2_x).pow(2) + (n1_y - n2_y).pow(2)) as f64).sqrt()
159    }
160
161    fn get_motion_model() -> Vec<(i32, i32, f64)> {
162        vec![
163            (1, 0, 1.0),
164            (0, 1, 1.0),
165            (-1, 0, 1.0),
166            (0, -1, 1.0),
167            (-1, -1, std::f64::consts::SQRT_2),
168            (-1, 1, std::f64::consts::SQRT_2),
169            (1, -1, std::f64::consts::SQRT_2),
170            (1, 1, std::f64::consts::SQRT_2),
171        ]
172    }
173
174    fn line_of_sight(&self, x0: i32, y0: i32, x1: i32, y1: i32) -> bool {
175        if !self.grid_map.is_valid(x0, y0) || !self.grid_map.is_valid(x1, y1) {
176            return false;
177        }
178
179        if x0 == x1 && y0 == y1 {
180            return true;
181        }
182
183        let dx = x1 - x0;
184        let dy = y1 - y0;
185        let step_x = dx.signum();
186        let step_y = dy.signum();
187        let abs_dx = dx.abs() as f64;
188        let abs_dy = dy.abs() as f64;
189
190        let mut x = x0;
191        let mut y = y0;
192        let mut t_max_x = if step_x != 0 {
193            0.5 / abs_dx
194        } else {
195            f64::INFINITY
196        };
197        let mut t_max_y = if step_y != 0 {
198            0.5 / abs_dy
199        } else {
200            f64::INFINITY
201        };
202        let t_delta_x = if step_x != 0 {
203            1.0 / abs_dx
204        } else {
205            f64::INFINITY
206        };
207        let t_delta_y = if step_y != 0 {
208            1.0 / abs_dy
209        } else {
210            f64::INFINITY
211        };
212
213        while x != x1 || y != y1 {
214            let advance_x = t_max_x <= t_max_y;
215            let advance_y = t_max_y <= t_max_x;
216            let next_x = if advance_x { x + step_x } else { x };
217            let next_y = if advance_y { y + step_y } else { y };
218
219            if !self.grid_map.is_valid_step(x, y, next_x, next_y) {
220                return false;
221            }
222
223            x = next_x;
224            y = next_y;
225
226            if advance_x {
227                t_max_x += t_delta_x;
228            }
229            if advance_y {
230                t_max_y += t_delta_y;
231            }
232        }
233
234        true
235    }
236
237    fn euclidean_distance(&self, x1: i32, y1: i32, x2: i32, y2: i32) -> f64 {
238        (((x1 - x2).pow(2) + (y1 - y2).pow(2)) as f64).sqrt()
239    }
240
241    fn build_path(&self, goal_index: usize, node_storage: &[Node]) -> Path2D {
242        let mut points = Vec::new();
243        let mut current_index = Some(goal_index);
244        while let Some(index) = current_index {
245            let node = &node_storage[index];
246            points.push(Point2D::new(
247                self.grid_map.calc_x_position(node.x),
248                self.grid_map.calc_y_position(node.y),
249            ));
250            current_index = node.parent_index;
251        }
252        points.reverse();
253        Path2D::from_points(points)
254    }
255
256    fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
257        if self.grid_map.is_valid(x, y) {
258            return Ok(());
259        }
260        Err(RoboticsError::PlanningError(format!(
261            "{} position is invalid",
262            label
263        )))
264    }
265
266    /// Find the best closed-set neighbor of (x, y) to use as parent when
267    /// the optimistic line-of-sight assumption fails.
268    fn best_grid_neighbor_parent(
269        &self,
270        x: i32,
271        y: i32,
272        closed_set: &HashMap<i32, usize>,
273        node_storage: &[Node],
274    ) -> Option<(usize, f64)> {
275        let mut best: Option<(usize, f64)> = None;
276        for &(dx, dy, move_cost) in &self.motion {
277            let nx = x + dx;
278            let ny = y + dy;
279            if !self.grid_map.is_valid_offset(x, y, dx, dy) {
280                continue;
281            }
282            let neighbor_grid_index = self.grid_map.calc_index(nx, ny);
283            if let Some(&neighbor_storage_index) = closed_set.get(&neighbor_grid_index) {
284                let g_via_neighbor = node_storage[neighbor_storage_index].cost + move_cost;
285                if best.map_or(true, |(_, best_g)| g_via_neighbor < best_g) {
286                    best = Some((neighbor_storage_index, g_via_neighbor));
287                }
288            }
289        }
290        best
291    }
292
293    fn plan_impl(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
294        let start_x = self.grid_map.calc_x_index(start.x);
295        let start_y = self.grid_map.calc_y_index(start.y);
296        let goal_x = self.grid_map.calc_x_index(goal.x);
297        let goal_y = self.grid_map.calc_y_index(goal.y);
298
299        self.ensure_query_is_valid(start_x, start_y, "Start")?;
300        self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;
301
302        let mut open_set = BinaryHeap::new();
303        let mut closed_set: HashMap<i32, usize> = HashMap::new();
304        let mut node_storage: Vec<Node> = Vec::new();
305        let mut g_values: HashMap<i32, f64> = HashMap::new();
306        // Maps grid_index -> latest storage index for that cell
307        let mut best_index: HashMap<i32, usize> = HashMap::new();
308
309        node_storage.push(Node::new(start_x, start_y, 0.0, None));
310        let start_index = 0;
311        let start_grid_index = self.grid_map.calc_index(start_x, start_y);
312        g_values.insert(start_grid_index, 0.0);
313        best_index.insert(start_grid_index, start_index);
314
315        open_set.push(PriorityNode {
316            x: start_x,
317            y: start_y,
318            cost: 0.0,
319            priority: self.calc_heuristic(start_x, start_y, goal_x, goal_y),
320            index: start_index,
321        });
322
323        while let Some(current) = open_set.pop() {
324            let current_grid_index = self.grid_map.calc_index(current.x, current.y);
325
326            if closed_set.contains_key(&current_grid_index) {
327                continue;
328            }
329
330            // --- LAZY EVALUATION ---
331            // When this node was added to the open set, we optimistically set its
332            // parent to the grandparent (parent's parent). Now we verify line-of-sight.
333            // If the line-of-sight fails, we fall back to the best closed-set
334            // grid neighbor as parent.
335            let mut corrected_index = current.index;
336            let current_node = &node_storage[current.index];
337            if let Some(parent_idx) = current_node.parent_index {
338                let parent_node = &node_storage[parent_idx];
339                // Verify line-of-sight to the actual parent
340                let los_ok = self.line_of_sight(parent_node.x, parent_node.y, current.x, current.y);
341
342                if !los_ok {
343                    // Line-of-sight to parent failed: find best grid neighbor in closed set
344                    if let Some((best_parent_idx, best_g)) = self.best_grid_neighbor_parent(
345                        current.x,
346                        current.y,
347                        &closed_set,
348                        &node_storage,
349                    ) {
350                        node_storage.push(Node::new(
351                            current.x,
352                            current.y,
353                            best_g,
354                            Some(best_parent_idx),
355                        ));
356                        corrected_index = node_storage.len() - 1;
357                        g_values.insert(current_grid_index, best_g);
358                        best_index.insert(current_grid_index, corrected_index);
359                    }
360                }
361            }
362
363            if current.x == goal_x && current.y == goal_y {
364                return Ok(self.build_path(corrected_index, &node_storage));
365            }
366
367            closed_set.insert(current_grid_index, corrected_index);
368
369            let current_cost = node_storage[corrected_index].cost;
370            let current_parent = node_storage[corrected_index].parent_index;
371
372            for &(dx, dy, _) in &self.motion {
373                let new_x = current.x + dx;
374                let new_y = current.y + dy;
375                let new_grid_index = self.grid_map.calc_index(new_x, new_y);
376                if !self.grid_map.is_valid_offset(current.x, current.y, dx, dy) {
377                    continue;
378                }
379                if closed_set.contains_key(&new_grid_index) {
380                    continue;
381                }
382
383                // Lazy Theta*: optimistically try to set parent to current's parent
384                // (grandparent path). The line-of-sight check is deferred.
385                let (new_cost, new_parent_index) = if let Some(p_idx) = current_parent {
386                    let parent_node = &node_storage[p_idx];
387                    let dist = self.euclidean_distance(parent_node.x, parent_node.y, new_x, new_y);
388                    let cost_via_parent = parent_node.cost + dist;
389
390                    // Also compute cost via current node (grid path)
391                    let dist_via_current =
392                        self.euclidean_distance(current.x, current.y, new_x, new_y);
393                    let cost_via_current = current_cost + dist_via_current;
394
395                    // Optimistically choose the cheaper option
396                    if cost_via_parent < cost_via_current {
397                        (cost_via_parent, Some(p_idx))
398                    } else {
399                        (cost_via_current, Some(corrected_index))
400                    }
401                } else {
402                    let dist = self.euclidean_distance(current.x, current.y, new_x, new_y);
403                    (current_cost + dist, Some(corrected_index))
404                };
405
406                let existing_g = g_values
407                    .get(&new_grid_index)
408                    .copied()
409                    .unwrap_or(f64::INFINITY);
410                if new_cost < existing_g {
411                    g_values.insert(new_grid_index, new_cost);
412                    node_storage.push(Node::new(new_x, new_y, new_cost, new_parent_index));
413                    let new_index = node_storage.len() - 1;
414                    best_index.insert(new_grid_index, new_index);
415                    let priority = new_cost + self.calc_heuristic(new_x, new_y, goal_x, goal_y);
416                    open_set.push(PriorityNode {
417                        x: new_x,
418                        y: new_y,
419                        cost: new_cost,
420                        priority,
421                        index: new_index,
422                    });
423                }
424            }
425        }
426
427        Err(RoboticsError::PlanningError("No path found".to_string()))
428    }
429}
430
431impl PathPlanner for LazyThetaStarPlanner {
432    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
433        self.plan_impl(start, goal)
434    }
435}
436
437#[cfg(test)]
438mod tests {
439    use super::*;
440    use rust_robotics_core::Obstacles;
441
442    fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
443        let mut ox = Vec::new();
444        let mut oy = Vec::new();
445        for i in 0..21 {
446            ox.push(i as f64);
447            oy.push(0.0);
448            ox.push(i as f64);
449            oy.push(20.0);
450            ox.push(0.0);
451            oy.push(i as f64);
452            ox.push(20.0);
453            oy.push(i as f64);
454        }
455        for i in 5..15 {
456            ox.push(10.0);
457            oy.push(i as f64);
458        }
459        (ox, oy)
460    }
461
462    #[test]
463    fn test_lazy_theta_star_finds_path() {
464        let (ox, oy) = create_simple_obstacles();
465        let planner = LazyThetaStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
466        let result = planner.plan(Point2D::new(2.0, 10.0), Point2D::new(18.0, 10.0));
467        assert!(result.is_ok());
468        assert!(!result.unwrap().is_empty());
469    }
470
471    #[test]
472    #[allow(deprecated)]
473    fn test_lazy_theta_star_legacy_interface() {
474        let (ox, oy) = create_simple_obstacles();
475        let planner = LazyThetaStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
476        let result = planner.planning(2.0, 10.0, 18.0, 10.0);
477        assert!(result.is_some());
478        let (rx, ry) = result.unwrap();
479        assert!(!rx.is_empty());
480        assert_eq!(rx.len(), ry.len());
481    }
482
483    #[test]
484    fn test_lazy_theta_star_shorter_than_a_star() {
485        let (ox, oy) = create_simple_obstacles();
486        let lazy_planner = LazyThetaStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
487        let a_star_planner = crate::a_star::AStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
488        let start = Point2D::new(2.0, 2.0);
489        let goal = Point2D::new(18.0, 18.0);
490        let lazy_path = lazy_planner.plan(start, goal).unwrap();
491        let a_star_path = a_star_planner.plan(start, goal).unwrap();
492        let lazy_length = lazy_path.total_length();
493        let a_star_length = a_star_path.total_length();
494        assert!(
495            lazy_length <= a_star_length + 0.1,
496            "Lazy Theta* path ({}) should not be significantly longer than A* path ({})",
497            lazy_length,
498            a_star_length
499        );
500    }
501
502    #[test]
503    fn test_lazy_theta_star_similar_to_theta_star() {
504        let (ox, oy) = create_simple_obstacles();
505        let lazy_planner = LazyThetaStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
506        let theta_planner = crate::theta_star::ThetaStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
507        let start = Point2D::new(2.0, 10.0);
508        let goal = Point2D::new(18.0, 10.0);
509        let lazy_path = lazy_planner.plan(start, goal).unwrap();
510        let theta_path = theta_planner.plan(start, goal).unwrap();
511        let lazy_length = lazy_path.total_length();
512        let theta_length = theta_path.total_length();
513        // Lazy Theta* may produce slightly longer paths than Theta* but should be close
514        let ratio = lazy_length / theta_length;
515        assert!(
516            ratio < 1.05,
517            "Lazy Theta* path ({:.2}) should be within 5% of Theta* path ({:.2}), ratio = {:.4}",
518            lazy_length,
519            theta_length,
520            ratio
521        );
522    }
523
524    #[test]
525    fn test_lazy_theta_star_from_obstacle_points() {
526        let (ox, oy) = create_simple_obstacles();
527        let obstacles = Obstacles::try_from_xy(&ox, &oy).unwrap();
528        let planner =
529            LazyThetaStarPlanner::from_obstacle_points(&obstacles, LazyThetaStarConfig::default())
530                .unwrap();
531        let path = planner.plan_xy(2.0, 10.0, 18.0, 10.0).unwrap();
532        assert!(!path.is_empty());
533    }
534
535    #[test]
536    fn test_lazy_theta_star_try_new_rejects_invalid_config() {
537        let (ox, oy) = create_simple_obstacles();
538        let config = LazyThetaStarConfig {
539            heuristic_weight: 0.0,
540            ..Default::default()
541        };
542        let err = match LazyThetaStarPlanner::try_new(&ox, &oy, config) {
543            Ok(_) => panic!("expected invalid config to be rejected"),
544            Err(err) => err,
545        };
546        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
547    }
548}
rust_robotics_planning/lazy_theta_star.rs

rust_robotics_planning/
lazy_theta_star.rs
