rust_robotics_planning/
enhanced_lazy_theta_star.rs

1//! Enhanced Lazy Theta* path planning algorithm
2//!
3//! Extends Lazy Theta* with improved fallback parent selection. When the
4//! optimistic line-of-sight check fails at expansion time, standard Lazy
5//! Theta* falls back to the best 8-connected grid neighbor. Enhanced Lazy
6//! Theta* additionally:
7//!
8//! 1. Checks line-of-sight to the fallback parent's ancestors (2-hop LOS),
9//!    potentially skipping intermediate nodes for shorter paths.
10//! 2. On every expansion (not just fallback), scans closed-set neighbors
11//!    for any-angle shortcuts via line-of-sight, similar to Theta* but
12//!    only at expansion time (still lazy).
13//!
14//! This produces paths that are shorter than both Theta* and Lazy Theta*
15//! because the search explores a wider set of parent candidates at each
16//! node expansion.
17//!
18//! Reference: Builds on Nash et al. (2010) "Lazy Theta*: Any-Angle Path
19//! Planning and Path Length Analysis in 3D"
20
21use std::cmp::Ordering;
22use std::collections::{BinaryHeap, HashMap};
23
24use crate::grid::{GridMap, Node};
25use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
26
27/// Configuration for Enhanced Lazy Theta* planner
28#[derive(Debug, Clone)]
29pub struct EnhancedLazyThetaStarConfig {
30    pub resolution: f64,
31    pub robot_radius: f64,
32    pub heuristic_weight: f64,
33}
34
35impl Default for EnhancedLazyThetaStarConfig {
36    fn default() -> Self {
37        Self {
38            resolution: 1.0,
39            robot_radius: 0.5,
40            heuristic_weight: 1.0,
41        }
42    }
43}
44
45impl EnhancedLazyThetaStarConfig {
46    pub fn validate(&self) -> RoboticsResult<()> {
47        if !self.resolution.is_finite() || self.resolution <= 0.0 {
48            return Err(RoboticsError::InvalidParameter(format!(
49                "resolution must be positive and finite, got {}",
50                self.resolution
51            )));
52        }
53        if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
54            return Err(RoboticsError::InvalidParameter(format!(
55                "robot_radius must be non-negative and finite, got {}",
56                self.robot_radius
57            )));
58        }
59        if !self.heuristic_weight.is_finite() || self.heuristic_weight <= 0.0 {
60            return Err(RoboticsError::InvalidParameter(format!(
61                "heuristic_weight must be positive and finite, got {}",
62                self.heuristic_weight
63            )));
64        }
65        Ok(())
66    }
67}
68
69#[derive(Debug)]
70struct PriorityNode {
71    x: i32,
72    y: i32,
73    #[allow(dead_code)]
74    cost: f64,
75    priority: f64,
76    index: usize,
77}
78impl Eq for PriorityNode {}
79impl PartialEq for PriorityNode {
80    fn eq(&self, other: &Self) -> bool {
81        self.priority == other.priority
82    }
83}
84impl Ord for PriorityNode {
85    fn cmp(&self, other: &Self) -> Ordering {
86        other
87            .priority
88            .partial_cmp(&self.priority)
89            .unwrap_or(Ordering::Equal)
90    }
91}
92impl PartialOrd for PriorityNode {
93    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
94        Some(self.cmp(other))
95    }
96}
97
98pub struct EnhancedLazyThetaStarPlanner {
99    grid_map: GridMap,
100    config: EnhancedLazyThetaStarConfig,
101    motion: Vec<(i32, i32, f64)>,
102}
103
104impl EnhancedLazyThetaStarPlanner {
105    pub fn new(ox: &[f64], oy: &[f64], config: EnhancedLazyThetaStarConfig) -> Self {
106        Self::try_new(ox, oy, config).expect("invalid Enhanced Lazy Theta* planner input")
107    }
108
109    pub fn try_new(
110        ox: &[f64],
111        oy: &[f64],
112        config: EnhancedLazyThetaStarConfig,
113    ) -> RoboticsResult<Self> {
114        config.validate()?;
115        let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
116        let motion = Self::get_motion_model();
117        Ok(Self {
118            grid_map,
119            config,
120            motion,
121        })
122    }
123
124    pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
125        let config = EnhancedLazyThetaStarConfig {
126            resolution,
127            robot_radius,
128            ..Default::default()
129        };
130        Self::new(ox, oy, config)
131    }
132
133    pub fn from_obstacle_points(
134        obstacles: &Obstacles,
135        config: EnhancedLazyThetaStarConfig,
136    ) -> RoboticsResult<Self> {
137        config.validate()?;
138        let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
139        let motion = Self::get_motion_model();
140        Ok(Self {
141            grid_map,
142            config,
143            motion,
144        })
145    }
146
147    #[deprecated(note = "use plan() or plan_xy() instead")]
148    pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
149        match self.plan_xy(sx, sy, gx, gy) {
150            Ok(path) => Some((path.x_coords(), path.y_coords())),
151            Err(_) => None,
152        }
153    }
154
155    pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
156        self.plan_impl(start, goal)
157    }
158
159    pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
160        self.plan_impl(Point2D::new(sx, sy), Point2D::new(gx, gy))
161    }
162
163    pub fn grid_map(&self) -> &GridMap {
164        &self.grid_map
165    }
166
167    fn calc_heuristic(&self, n1_x: i32, n1_y: i32, n2_x: i32, n2_y: i32) -> f64 {
168        self.config.heuristic_weight * (((n1_x - n2_x).pow(2) + (n1_y - n2_y).pow(2)) as f64).sqrt()
169    }
170
171    fn get_motion_model() -> Vec<(i32, i32, f64)> {
172        vec![
173            (1, 0, 1.0),
174            (0, 1, 1.0),
175            (-1, 0, 1.0),
176            (0, -1, 1.0),
177            (-1, -1, std::f64::consts::SQRT_2),
178            (-1, 1, std::f64::consts::SQRT_2),
179            (1, -1, std::f64::consts::SQRT_2),
180            (1, 1, std::f64::consts::SQRT_2),
181        ]
182    }
183
184    fn line_of_sight(&self, x0: i32, y0: i32, x1: i32, y1: i32) -> bool {
185        if !self.grid_map.is_valid(x0, y0) || !self.grid_map.is_valid(x1, y1) {
186            return false;
187        }
188        if x0 == x1 && y0 == y1 {
189            return true;
190        }
191        let dx = x1 - x0;
192        let dy = y1 - y0;
193        let step_x = dx.signum();
194        let step_y = dy.signum();
195        let abs_dx = dx.abs() as f64;
196        let abs_dy = dy.abs() as f64;
197        let mut x = x0;
198        let mut y = y0;
199        let mut t_max_x = if step_x != 0 {
200            0.5 / abs_dx
201        } else {
202            f64::INFINITY
203        };
204        let mut t_max_y = if step_y != 0 {
205            0.5 / abs_dy
206        } else {
207            f64::INFINITY
208        };
209        let t_delta_x = if step_x != 0 {
210            1.0 / abs_dx
211        } else {
212            f64::INFINITY
213        };
214        let t_delta_y = if step_y != 0 {
215            1.0 / abs_dy
216        } else {
217            f64::INFINITY
218        };
219        while x != x1 || y != y1 {
220            let advance_x = t_max_x <= t_max_y;
221            let advance_y = t_max_y <= t_max_x;
222            let next_x = if advance_x { x + step_x } else { x };
223            let next_y = if advance_y { y + step_y } else { y };
224            if !self.grid_map.is_valid_step(x, y, next_x, next_y) {
225                return false;
226            }
227            x = next_x;
228            y = next_y;
229            if advance_x {
230                t_max_x += t_delta_x;
231            }
232            if advance_y {
233                t_max_y += t_delta_y;
234            }
235        }
236        true
237    }
238
239    fn euclidean_distance(&self, x1: i32, y1: i32, x2: i32, y2: i32) -> f64 {
240        (((x1 - x2).pow(2) + (y1 - y2).pow(2)) as f64).sqrt()
241    }
242
243    fn build_path(&self, goal_index: usize, node_storage: &[Node]) -> Path2D {
244        let mut points = Vec::new();
245        let mut current_index = Some(goal_index);
246        while let Some(index) = current_index {
247            let node = &node_storage[index];
248            points.push(Point2D::new(
249                self.grid_map.calc_x_position(node.x),
250                self.grid_map.calc_y_position(node.y),
251            ));
252            current_index = node.parent_index;
253        }
254        points.reverse();
255        Path2D::from_points(points)
256    }
257
258    fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
259        if self.grid_map.is_valid(x, y) {
260            return Ok(());
261        }
262        Err(RoboticsError::PlanningError(format!(
263            "{} position is invalid",
264            label
265        )))
266    }
267
268    /// Enhanced fallback: find the best parent among closed-set neighbors
269    /// in a 2-ring neighborhood, then walk ancestor chains for LOS shortcuts.
270    fn enhanced_best_parent(
271        &self,
272        x: i32,
273        y: i32,
274        closed_set: &HashMap<i32, usize>,
275        node_storage: &[Node],
276    ) -> Option<(usize, f64)> {
277        let mut best: Option<(usize, f64)> = None;
278        const MAX_ANCESTOR_HOPS: usize = 6;
279
280        // Collect candidate closed-set nodes from 2-ring neighborhood
281        // Ring 1: 8-connected (distance 1 or sqrt(2))
282        // Ring 2: distance 2, sqrt(5), 2*sqrt(2)
283        let offsets: [(i32, i32); 24] = [
284            // Ring 1 (8)
285            (1, 0),
286            (0, 1),
287            (-1, 0),
288            (0, -1),
289            (1, 1),
290            (1, -1),
291            (-1, 1),
292            (-1, -1),
293            // Ring 2 (16)
294            (2, 0),
295            (0, 2),
296            (-2, 0),
297            (0, -2),
298            (2, 1),
299            (2, -1),
300            (-2, 1),
301            (-2, -1),
302            (1, 2),
303            (1, -2),
304            (-1, 2),
305            (-1, -2),
306            (2, 2),
307            (2, -2),
308            (-2, 2),
309            (-2, -2),
310        ];
311
312        for &(dx, dy) in &offsets {
313            let nx = x + dx;
314            let ny = y + dy;
315            if !self.grid_map.is_valid(nx, ny) {
316                continue;
317            }
318            let neighbor_grid_index = self.grid_map.calc_index(nx, ny);
319            if let Some(&neighbor_storage_index) = closed_set.get(&neighbor_grid_index) {
320                let neighbor = &node_storage[neighbor_storage_index];
321
322                // Direct LOS check to this closed neighbor
323                if self.line_of_sight(neighbor.x, neighbor.y, x, y) {
324                    let dist = self.euclidean_distance(neighbor.x, neighbor.y, x, y);
325                    let g_via = neighbor.cost + dist;
326                    if best.map_or(true, |(_, best_g)| g_via < best_g) {
327                        best = Some((neighbor_storage_index, g_via));
328                    }
329                }
330
331                // Walk ancestor chain of this neighbor
332                let mut ancestor_idx = neighbor.parent_index;
333                let mut hops = 0;
334                while let Some(a_idx) = ancestor_idx {
335                    if hops >= MAX_ANCESTOR_HOPS {
336                        break;
337                    }
338                    let ancestor = &node_storage[a_idx];
339                    if self.line_of_sight(ancestor.x, ancestor.y, x, y) {
340                        let dist = self.euclidean_distance(ancestor.x, ancestor.y, x, y);
341                        let g_via = ancestor.cost + dist;
342                        if best.map_or(true, |(_, best_g)| g_via < best_g) {
343                            best = Some((a_idx, g_via));
344                        }
345                    }
346                    ancestor_idx = ancestor.parent_index;
347                    hops += 1;
348                }
349            }
350        }
351
352        best
353    }
354
355    fn plan_impl(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
356        let start_x = self.grid_map.calc_x_index(start.x);
357        let start_y = self.grid_map.calc_y_index(start.y);
358        let goal_x = self.grid_map.calc_x_index(goal.x);
359        let goal_y = self.grid_map.calc_y_index(goal.y);
360
361        self.ensure_query_is_valid(start_x, start_y, "Start")?;
362        self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;
363
364        let mut open_set = BinaryHeap::new();
365        let mut closed_set: HashMap<i32, usize> = HashMap::new();
366        let mut node_storage: Vec<Node> = Vec::new();
367        let mut g_values: HashMap<i32, f64> = HashMap::new();
368        let mut best_index: HashMap<i32, usize> = HashMap::new();
369
370        node_storage.push(Node::new(start_x, start_y, 0.0, None));
371        let start_index = 0;
372        let start_grid_index = self.grid_map.calc_index(start_x, start_y);
373        g_values.insert(start_grid_index, 0.0);
374        best_index.insert(start_grid_index, start_index);
375
376        open_set.push(PriorityNode {
377            x: start_x,
378            y: start_y,
379            cost: 0.0,
380            priority: self.calc_heuristic(start_x, start_y, goal_x, goal_y),
381            index: start_index,
382        });
383
384        while let Some(current) = open_set.pop() {
385            let current_grid_index = self.grid_map.calc_index(current.x, current.y);
386
387            if closed_set.contains_key(&current_grid_index) {
388                continue;
389            }
390
391            // --- ENHANCED LAZY EVALUATION ---
392            // Verify LOS to optimistic parent. On failure, use enhanced
393            // fallback that searches ancestors and cross-branch parents.
394            let mut corrected_index = current.index;
395            let current_node = &node_storage[current.index];
396            if let Some(parent_idx) = current_node.parent_index {
397                let parent_node = &node_storage[parent_idx];
398                // Verify direct LOS to parent
399                let needs_correction =
400                    !self.line_of_sight(parent_node.x, parent_node.y, current.x, current.y);
401
402                if needs_correction {
403                    // Enhanced fallback: search wider parent candidates
404                    if let Some((best_parent_idx, best_g)) =
405                        self.enhanced_best_parent(current.x, current.y, &closed_set, &node_storage)
406                    {
407                        node_storage.push(Node::new(
408                            current.x,
409                            current.y,
410                            best_g,
411                            Some(best_parent_idx),
412                        ));
413                        corrected_index = node_storage.len() - 1;
414                        g_values.insert(current_grid_index, best_g);
415                        best_index.insert(current_grid_index, corrected_index);
416                    }
417                } else {
418                    // LOS succeeded — still try any-angle improvement via closed neighbors
419                    // This is the key enhancement: even when the optimistic parent is valid,
420                    // there might be a better parent among closed-set ancestors.
421                    if let Some((better_idx, better_g)) =
422                        self.enhanced_best_parent(current.x, current.y, &closed_set, &node_storage)
423                    {
424                        if better_g < current_node.cost {
425                            node_storage.push(Node::new(
426                                current.x,
427                                current.y,
428                                better_g,
429                                Some(better_idx),
430                            ));
431                            corrected_index = node_storage.len() - 1;
432                            g_values.insert(current_grid_index, better_g);
433                            best_index.insert(current_grid_index, corrected_index);
434                        }
435                    }
436                }
437            }
438
439            if current.x == goal_x && current.y == goal_y {
440                return Ok(self.build_path(corrected_index, &node_storage));
441            }
442
443            closed_set.insert(current_grid_index, corrected_index);
444
445            let current_cost = node_storage[corrected_index].cost;
446            let current_parent = node_storage[corrected_index].parent_index;
447
448            for &(dx, dy, _) in &self.motion {
449                let new_x = current.x + dx;
450                let new_y = current.y + dy;
451                let new_grid_index = self.grid_map.calc_index(new_x, new_y);
452                if !self.grid_map.is_valid_offset(current.x, current.y, dx, dy) {
453                    continue;
454                }
455                if closed_set.contains_key(&new_grid_index) {
456                    continue;
457                }
458
459                // Optimistic parent assignment (same as Lazy Theta*)
460                let (new_cost, new_parent_index) = if let Some(p_idx) = current_parent {
461                    let parent_node = &node_storage[p_idx];
462                    let dist = self.euclidean_distance(parent_node.x, parent_node.y, new_x, new_y);
463                    let cost_via_parent = parent_node.cost + dist;
464
465                    let dist_via_current =
466                        self.euclidean_distance(current.x, current.y, new_x, new_y);
467                    let cost_via_current = current_cost + dist_via_current;
468
469                    if cost_via_parent < cost_via_current {
470                        (cost_via_parent, Some(p_idx))
471                    } else {
472                        (cost_via_current, Some(corrected_index))
473                    }
474                } else {
475                    let dist = self.euclidean_distance(current.x, current.y, new_x, new_y);
476                    (current_cost + dist, Some(corrected_index))
477                };
478
479                let existing_g = g_values
480                    .get(&new_grid_index)
481                    .copied()
482                    .unwrap_or(f64::INFINITY);
483                if new_cost < existing_g {
484                    g_values.insert(new_grid_index, new_cost);
485                    node_storage.push(Node::new(new_x, new_y, new_cost, new_parent_index));
486                    let new_index = node_storage.len() - 1;
487                    best_index.insert(new_grid_index, new_index);
488                    let priority = new_cost + self.calc_heuristic(new_x, new_y, goal_x, goal_y);
489                    open_set.push(PriorityNode {
490                        x: new_x,
491                        y: new_y,
492                        cost: new_cost,
493                        priority,
494                        index: new_index,
495                    });
496                }
497            }
498        }
499
500        Err(RoboticsError::PlanningError("No path found".to_string()))
501    }
502}
503
504impl PathPlanner for EnhancedLazyThetaStarPlanner {
505    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
506        self.plan_impl(start, goal)
507    }
508}
509
510#[cfg(test)]
511mod tests {
512    use super::*;
513    use rust_robotics_core::Obstacles;
514
515    fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
516        let mut ox = Vec::new();
517        let mut oy = Vec::new();
518        for i in 0..21 {
519            ox.push(i as f64);
520            oy.push(0.0);
521            ox.push(i as f64);
522            oy.push(20.0);
523            ox.push(0.0);
524            oy.push(i as f64);
525            ox.push(20.0);
526            oy.push(i as f64);
527        }
528        for i in 5..15 {
529            ox.push(10.0);
530            oy.push(i as f64);
531        }
532        (ox, oy)
533    }
534
535    #[test]
536    fn test_enhanced_finds_path() {
537        let (ox, oy) = create_simple_obstacles();
538        let planner = EnhancedLazyThetaStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
539        let result = planner.plan(Point2D::new(2.0, 10.0), Point2D::new(18.0, 10.0));
540        assert!(result.is_ok());
541        assert!(!result.unwrap().is_empty());
542    }
543
544    #[test]
545    fn test_enhanced_shorter_than_a_star() {
546        let (ox, oy) = create_simple_obstacles();
547        let planner = EnhancedLazyThetaStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
548        let a_star = crate::a_star::AStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
549        let start = Point2D::new(2.0, 2.0);
550        let goal = Point2D::new(18.0, 18.0);
551        let enhanced_path = planner.plan(start, goal).unwrap();
552        let a_star_path = a_star.plan(start, goal).unwrap();
553        assert!(
554            enhanced_path.total_length() <= a_star_path.total_length() + 0.1,
555            "Enhanced ({:.2}) should not be longer than A* ({:.2})",
556            enhanced_path.total_length(),
557            a_star_path.total_length()
558        );
559    }
560
561    #[test]
562    fn test_enhanced_at_least_as_good_as_lazy_theta() {
563        let (ox, oy) = create_simple_obstacles();
564        let enhanced = EnhancedLazyThetaStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
565        let lazy = crate::lazy_theta_star::LazyThetaStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
566        let theta = crate::theta_star::ThetaStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
567
568        let start = Point2D::new(2.0, 10.0);
569        let goal = Point2D::new(18.0, 10.0);
570
571        let enhanced_len = enhanced.plan(start, goal).unwrap().total_length();
572        let lazy_len = lazy.plan(start, goal).unwrap().total_length();
573        let theta_len = theta.plan(start, goal).unwrap().total_length();
574
575        // Enhanced should be at least as good as the better of Lazy and Theta
576        let best_existing = lazy_len.min(theta_len);
577        assert!(
578            enhanced_len <= best_existing + 0.5,
579            "Enhanced ({:.2}) should be at least as good as best existing ({:.2}, lazy={:.2}, theta={:.2})",
580            enhanced_len,
581            best_existing,
582            lazy_len,
583            theta_len
584        );
585    }
586
587    #[test]
588    fn test_enhanced_from_obstacle_points() {
589        let (ox, oy) = create_simple_obstacles();
590        let obstacles = Obstacles::try_from_xy(&ox, &oy).unwrap();
591        let planner = EnhancedLazyThetaStarPlanner::from_obstacle_points(
592            &obstacles,
593            EnhancedLazyThetaStarConfig::default(),
594        )
595        .unwrap();
596        let path = planner.plan_xy(2.0, 10.0, 18.0, 10.0).unwrap();
597        assert!(!path.is_empty());
598    }
599
600    #[test]
601    fn test_enhanced_rejects_invalid_config() {
602        let (ox, oy) = create_simple_obstacles();
603        let config = EnhancedLazyThetaStarConfig {
604            heuristic_weight: 0.0,
605            ..Default::default()
606        };
607        assert!(EnhancedLazyThetaStarPlanner::try_new(&ox, &oy, config).is_err());
608    }
609}
rust_robotics_planning/enhanced_lazy_theta_star.rs

rust_robotics_planning/
enhanced_lazy_theta_star.rs
