condor_grid/algorithms/
field_d_star.rs

1//! Interpolated-grid [`InterpolatedGridReplanner`]:
2//! Field D* surface.
3//!
4//! It owns a grid snapshot and fractional request after `initialize`; updates mutate
5//! that state and `replan` solves again. Cost uses
6//! [`InterpolatedTraversalCostModel::CellLengthWeightedV0`]
7//! over cell-center Dijkstra and Euclidean endpoint segments. Outcomes can be found,
8//! fallback, or partial when connectivity is lost. Prefer
9//! [`crate::algorithms::d_star_lite::DStarLite`] for changing integer-cell maps;
10//! this lane is for fractional requests.
11//!
12//! # Examples
13//!
14//! ```
15//! use condor_grid::{
16//!     algorithms::field_d_star::FieldDStar, Grid, InterpolatedGridReplanner,
17//!     InterpolatedSearchRequest, InterpolatedSearchResult, Point2,
18//! };
19//!
20//! let mut replanner = FieldDStar::new();
21//! let grid = Grid::new(3, 3).expect("grid dimensions are valid");
22//! let request = InterpolatedSearchRequest::new(
23//!     Point2::new(0.5, 0.5),
24//!     Point2::new(2.5, 2.5),
25//! );
26//! let result = replanner.initialize(&grid, request).expect("request is valid");
27//! assert!(result.expected_kind() != condor_grid::InterpolatedExpectedKind::InvalidStart);
28//! ```
29use crate::{
30    grid::{Cell, Grid, GridEditError},
31    point::Point,
32    replanning::{
33        InterpolatedGridReplanner, InterpolatedMovingGoalReplanner, InterpolatedPath,
34        InterpolatedQueryResult, InterpolatedSearchRequest, InterpolatedSearchResult,
35        InterpolatedTraversalCostModel, best_fallback_interpolated_path,
36        best_partial_interpolated_path, interpolated_segment_cost, query_interpolated_grid,
37    },
38};
39use condor_core::Point2;
40
41const EPSILON: f64 = 1e-9;
42
43/// Interpolated [`InterpolatedGridReplanner`] (Field D* name).
44///
45/// Fractional start/goal; cost model
46/// [`InterpolatedTraversalCostModel::CellLengthWeightedV0`]
47/// via cell-center Dijkstra with continuous segment costs. Also implements
48/// [`InterpolatedMovingGoalReplanner`].
49/// Returns partial/fallback paths when discrete connectivity is lost. Prefer for
50/// continuous endpoints on a discrete weighted grid; not exact Field D* literature form.
51pub struct FieldDStar {
52    grid: Option<Grid>,
53    request: Option<InterpolatedSearchRequest>,
54    cost_model: InterpolatedTraversalCostModel,
55}
56
57impl Default for FieldDStar {
58    fn default() -> Self {
59        Self::new()
60    }
61}
62
63impl FieldDStar {
64    /// Creates an uninitialized interpolating replanner; call `initialize` before replan.
65    #[must_use]
66    pub fn new() -> Self {
67        Self {
68            grid: None,
69            request: None,
70            cost_model: InterpolatedTraversalCostModel::CellLengthWeightedV0,
71        }
72    }
73
74    fn solve(&self, grid: &Grid, request: InterpolatedSearchRequest) -> InterpolatedSearchResult {
75        match query_interpolated_grid(grid, request) {
76            InterpolatedQueryResult::InvalidStart => {
77                Err(crate::InterpolatedSearchError::InvalidStart {
78                    point: request.start,
79                })
80            }
81            InterpolatedQueryResult::InvalidGoal => {
82                Err(crate::InterpolatedSearchError::InvalidGoal {
83                    point: request.goal,
84                })
85            }
86            InterpolatedQueryResult::NoPath { .. } => {
87                if let Some(path) = best_fallback_interpolated_path(grid, request, self.cost_model)?
88                {
89                    return crate::replanning::interpolated_fallback(path, 0);
90                }
91                match best_partial_interpolated_path(grid, request, self.cost_model)? {
92                    Some(path) => crate::replanning::interpolated_partial(path, 0),
93                    None => crate::replanning::interpolated_not_found(0),
94                }
95            }
96            InterpolatedQueryResult::Connected { .. } => {
97                let Some((path, visited_nodes)) =
98                    shortest_interpolated_path(grid, request, self.cost_model)
99                else {
100                    return crate::replanning::interpolated_not_found(0);
101                };
102                crate::replanning::interpolated_found(path, visited_nodes)
103            }
104        }
105    }
106}
107
108impl InterpolatedGridReplanner for FieldDStar {
109    fn name(&self) -> &'static str {
110        "field-d-star"
111    }
112
113    fn initialize(
114        &mut self,
115        grid: &Grid,
116        request: InterpolatedSearchRequest,
117    ) -> InterpolatedSearchResult {
118        self.grid = Some(grid.clone());
119        self.request = Some(request);
120        self.solve(grid, request)
121    }
122
123    fn update_cell(&mut self, point: Point, cell: Cell) {
124        if let Some(grid) = &mut self.grid {
125            let _ = grid.set_cell(point, cell);
126        }
127    }
128
129    fn update_cost(&mut self, point: Point, cost: usize) -> Result<(), GridEditError> {
130        if let Some(grid) = &mut self.grid {
131            return grid.set_traversal_cost(point, cost);
132        }
133        Ok(())
134    }
135
136    fn replan(&mut self) -> InterpolatedSearchResult {
137        match (&self.grid, self.request) {
138            (Some(grid), Some(request)) => self.solve(grid, request),
139            _ => Err(crate::InterpolatedSearchError::NotInitialized),
140        }
141    }
142}
143
144impl InterpolatedMovingGoalReplanner for FieldDStar {
145    fn update_goal(&mut self, goal: Point2) {
146        if let Some(request) = &mut self.request {
147            request.goal = goal;
148        }
149    }
150}
151
152fn shortest_interpolated_path(
153    grid: &Grid,
154    request: InterpolatedSearchRequest,
155    cost_model: InterpolatedTraversalCostModel,
156) -> Option<(InterpolatedPath, usize)> {
157    let nodes = candidate_nodes(grid, request);
158    let goal_index = 1;
159
160    let mut distances = vec![f64::INFINITY; nodes.len()];
161    let mut parents = vec![None; nodes.len()];
162    let mut visited = vec![false; nodes.len()];
163    let mut visited_nodes = 0;
164
165    distances[0] = 0.0;
166
167    loop {
168        let current = next_unvisited_node(&distances, &visited)?;
169        if !distances[current].is_finite() {
170            break;
171        }
172
173        visited[current] = true;
174        visited_nodes += 1;
175
176        if current == goal_index {
177            break;
178        }
179
180        for neighbor in 0..nodes.len() {
181            if neighbor == current || visited[neighbor] {
182                continue;
183            }
184
185            let Some(step_cost) = candidate_step_cost(grid, &nodes, current, neighbor, cost_model)
186            else {
187                continue;
188            };
189
190            let candidate_cost = distances[current] + step_cost;
191            if candidate_cost + EPSILON < distances[neighbor] {
192                distances[neighbor] = candidate_cost;
193                parents[neighbor] = Some(current);
194            }
195        }
196    }
197
198    if !distances[goal_index].is_finite() {
199        return None;
200    }
201
202    let mut path_points = vec![nodes[goal_index]];
203    let mut cursor = goal_index;
204    while let Some(parent) = parents[cursor] {
205        cursor = parent;
206        path_points.push(nodes[cursor]);
207    }
208    path_points.reverse();
209
210    let path = match InterpolatedPath::from_points_on_grid(grid, path_points, cost_model) {
211        Ok(path) => path,
212        Err(_) => return None,
213    };
214    Some((path, visited_nodes))
215}
216
217fn candidate_step_cost(
218    grid: &Grid,
219    nodes: &[Point2],
220    current: usize,
221    neighbor: usize,
222    cost_model: InterpolatedTraversalCostModel,
223) -> Option<f64> {
224    if !candidate_edge_allowed(nodes, current, neighbor) {
225        return None;
226    }
227
228    interpolated_segment_cost(grid, nodes[current], nodes[neighbor], cost_model)
229}
230
231fn candidate_edge_allowed(nodes: &[Point2], current: usize, neighbor: usize) -> bool {
232    let direct_start_to_goal = (current == 0 && neighbor == 1) || (current == 1 && neighbor == 0);
233    if direct_start_to_goal || axis_aligned(nodes[current], nodes[neighbor]) {
234        return true;
235    }
236
237    const START_INDEX: usize = 0;
238    const GOAL_INDEX: usize = 1;
239    edge_anchors_endpoint_to_its_cell(nodes, current, neighbor, START_INDEX)
240        || edge_anchors_endpoint_to_its_cell(nodes, current, neighbor, GOAL_INDEX)
241}
242
243fn edge_anchors_endpoint_to_its_cell(
244    nodes: &[Point2],
245    current: usize,
246    neighbor: usize,
247    endpoint: usize,
248) -> bool {
249    let other = if current == endpoint {
250        neighbor
251    } else if neighbor == endpoint {
252        current
253    } else {
254        return false;
255    };
256
257    same_point(nodes[other], containing_cell_center(nodes[endpoint]))
258}
259
260fn containing_cell_center(point: Point2) -> Point2 {
261    Point2::new(point.x.floor() + 0.5, point.y.floor() + 0.5)
262}
263
264fn next_unvisited_node(distances: &[f64], visited: &[bool]) -> Option<usize> {
265    let mut best_index = None;
266    let mut best_cost = f64::INFINITY;
267
268    for (index, cost) in distances.iter().copied().enumerate() {
269        if visited[index] || cost + EPSILON >= best_cost {
270            continue;
271        }
272        best_cost = cost;
273        best_index = Some(index);
274    }
275
276    best_index
277}
278
279fn candidate_nodes(grid: &Grid, request: InterpolatedSearchRequest) -> Vec<Point2> {
280    let mut nodes = vec![request.start, request.goal];
281
282    for index in 0..grid.cell_count() {
283        let point = grid.point_from_index(index);
284        if !grid.is_walkable(point) {
285            continue;
286        }
287
288        let center = cell_center(point);
289        if same_point(center, request.start) || same_point(center, request.goal) {
290            continue;
291        }
292
293        nodes.push(center);
294    }
295
296    nodes
297}
298
299fn same_point(left: Point2, right: Point2) -> bool {
300    (left.x - right.x).abs() <= EPSILON && (left.y - right.y).abs() <= EPSILON
301}
302
303fn axis_aligned(left: Point2, right: Point2) -> bool {
304    (left.x - right.x).abs() <= EPSILON || (left.y - right.y).abs() <= EPSILON
305}
306
307fn cell_center(point: Point) -> Point2 {
308    Point2::new(point.x as f64 + 0.5, point.y as f64 + 0.5)
309}
condor_grid/algorithms/field_d_star.rs

condor_grid/algorithms/
field_d_star.rs
