condor-pathfinding-grid 0.4.0

//! Interpolated-grid [`InterpolatedGridReplanner`]:
//! Field D* surface.
//!
//! It owns a grid snapshot and fractional request after `initialize`; updates mutate
//! that state and `replan` solves again. Cost uses
//! [`InterpolatedTraversalCostModel::CellLengthWeightedV0`]
//! over cell-center Dijkstra and Euclidean endpoint segments. Outcomes can be found,
//! fallback, or partial when connectivity is lost. Prefer
//! [`crate::algorithms::d_star_lite::DStarLite`] for changing integer-cell maps;
//! this lane is for fractional requests.
//!
//! # Examples
//!
//! ```
//! use condor_grid::{
//!     algorithms::field_d_star::FieldDStar, Grid, InterpolatedGridReplanner,
//!     InterpolatedSearchRequest, InterpolatedSearchResult, Point2,
//! };
//!
//! let mut replanner = FieldDStar::new();
//! let grid = Grid::new(3, 3).expect("grid dimensions are valid");
//! let request = InterpolatedSearchRequest::new(
//!     Point2::new(0.5, 0.5),
//!     Point2::new(2.5, 2.5),
//! );
//! let result = replanner.initialize(&grid, request).expect("request is valid");
//! assert!(result.expected_kind() != condor_grid::InterpolatedExpectedKind::InvalidStart);
//! ```
use crate::{
    grid::{Cell, Grid, GridEditError},
    point::Point,
    replanning::{
        InterpolatedGridReplanner, InterpolatedMovingGoalReplanner, InterpolatedPath,
        InterpolatedQueryResult, InterpolatedSearchRequest, InterpolatedSearchResult,
        InterpolatedTraversalCostModel, best_fallback_interpolated_path,
        best_partial_interpolated_path, interpolated_segment_cost, query_interpolated_grid,
    },
};
use condor_core::Point2;

const EPSILON: f64 = 1e-9;

/// Interpolated [`InterpolatedGridReplanner`] (Field D* name).
///
/// Fractional start/goal; cost model
/// [`InterpolatedTraversalCostModel::CellLengthWeightedV0`]
/// via cell-center Dijkstra with continuous segment costs. Also implements
/// [`InterpolatedMovingGoalReplanner`].
/// Returns partial/fallback paths when discrete connectivity is lost. Prefer for
/// continuous endpoints on a discrete weighted grid; not exact Field D* literature form.
pub struct FieldDStar {
    grid: Option<Grid>,
    request: Option<InterpolatedSearchRequest>,
    cost_model: InterpolatedTraversalCostModel,
}

impl Default for FieldDStar {
    fn default() -> Self {
        Self::new()
    }
}

impl FieldDStar {
    /// Creates an uninitialized interpolating replanner; call `initialize` before replan.
    #[must_use]
    pub fn new() -> Self {
        Self {
            grid: None,
            request: None,
            cost_model: InterpolatedTraversalCostModel::CellLengthWeightedV0,
        }
    }

    fn solve(&self, grid: &Grid, request: InterpolatedSearchRequest) -> InterpolatedSearchResult {
        match query_interpolated_grid(grid, request) {
            InterpolatedQueryResult::InvalidStart => {
                Err(crate::InterpolatedSearchError::InvalidStart {
                    point: request.start,
                })
            }
            InterpolatedQueryResult::InvalidGoal => {
                Err(crate::InterpolatedSearchError::InvalidGoal {
                    point: request.goal,
                })
            }
            InterpolatedQueryResult::NoPath { .. } => {
                if let Some(path) = best_fallback_interpolated_path(grid, request, self.cost_model)?
                {
                    return crate::replanning::interpolated_fallback(path, 0);
                }
                match best_partial_interpolated_path(grid, request, self.cost_model)? {
                    Some(path) => crate::replanning::interpolated_partial(path, 0),
                    None => crate::replanning::interpolated_not_found(0),
                }
            }
            InterpolatedQueryResult::Connected { .. } => {
                let Some((path, visited_nodes)) =
                    shortest_interpolated_path(grid, request, self.cost_model)
                else {
                    return crate::replanning::interpolated_not_found(0);
                };
                crate::replanning::interpolated_found(path, visited_nodes)
            }
        }
    }
}

impl InterpolatedGridReplanner for FieldDStar {
    fn name(&self) -> &'static str {
        "field-d-star"
    }

    fn initialize(
        &mut self,
        grid: &Grid,
        request: InterpolatedSearchRequest,
    ) -> InterpolatedSearchResult {
        self.grid = Some(grid.clone());
        self.request = Some(request);
        self.solve(grid, request)
    }

    fn update_cell(&mut self, point: Point, cell: Cell) {
        if let Some(grid) = &mut self.grid {
            let _ = grid.set_cell(point, cell);
        }
    }

    fn update_cost(&mut self, point: Point, cost: usize) -> Result<(), GridEditError> {
        if let Some(grid) = &mut self.grid {
            return grid.set_traversal_cost(point, cost);
        }
        Ok(())
    }

    fn replan(&mut self) -> InterpolatedSearchResult {
        match (&self.grid, self.request) {
            (Some(grid), Some(request)) => self.solve(grid, request),
            _ => Err(crate::InterpolatedSearchError::NotInitialized),
        }
    }
}

impl InterpolatedMovingGoalReplanner for FieldDStar {
    fn update_goal(&mut self, goal: Point2) {
        if let Some(request) = &mut self.request {
            request.goal = goal;
        }
    }
}

fn shortest_interpolated_path(
    grid: &Grid,
    request: InterpolatedSearchRequest,
    cost_model: InterpolatedTraversalCostModel,
) -> Option<(InterpolatedPath, usize)> {
    let nodes = candidate_nodes(grid, request);
    let goal_index = 1;

    let mut distances = vec![f64::INFINITY; nodes.len()];
    let mut parents = vec![None; nodes.len()];
    let mut visited = vec![false; nodes.len()];
    let mut visited_nodes = 0;

    distances[0] = 0.0;

    loop {
        let current = next_unvisited_node(&distances, &visited)?;
        if !distances[current].is_finite() {
            break;
        }

        visited[current] = true;
        visited_nodes += 1;

        if current == goal_index {
            break;
        }

        for neighbor in 0..nodes.len() {
            if neighbor == current || visited[neighbor] {
                continue;
            }

            let Some(step_cost) = candidate_step_cost(grid, &nodes, current, neighbor, cost_model)
            else {
                continue;
            };

            let candidate_cost = distances[current] + step_cost;
            if candidate_cost + EPSILON < distances[neighbor] {
                distances[neighbor] = candidate_cost;
                parents[neighbor] = Some(current);
            }
        }
    }

    if !distances[goal_index].is_finite() {
        return None;
    }

    let mut path_points = vec![nodes[goal_index]];
    let mut cursor = goal_index;
    while let Some(parent) = parents[cursor] {
        cursor = parent;
        path_points.push(nodes[cursor]);
    }
    path_points.reverse();

    let path = match InterpolatedPath::from_points_on_grid(grid, path_points, cost_model) {
        Ok(path) => path,
        Err(_) => return None,
    };
    Some((path, visited_nodes))
}

fn candidate_step_cost(
    grid: &Grid,
    nodes: &[Point2],
    current: usize,
    neighbor: usize,
    cost_model: InterpolatedTraversalCostModel,
) -> Option<f64> {
    if !candidate_edge_allowed(nodes, current, neighbor) {
        return None;
    }

    interpolated_segment_cost(grid, nodes[current], nodes[neighbor], cost_model)
}

fn candidate_edge_allowed(nodes: &[Point2], current: usize, neighbor: usize) -> bool {
    let direct_start_to_goal = (current == 0 && neighbor == 1) || (current == 1 && neighbor == 0);
    if direct_start_to_goal || axis_aligned(nodes[current], nodes[neighbor]) {
        return true;
    }

    const START_INDEX: usize = 0;
    const GOAL_INDEX: usize = 1;
    edge_anchors_endpoint_to_its_cell(nodes, current, neighbor, START_INDEX)
        || edge_anchors_endpoint_to_its_cell(nodes, current, neighbor, GOAL_INDEX)
}

fn edge_anchors_endpoint_to_its_cell(
    nodes: &[Point2],
    current: usize,
    neighbor: usize,
    endpoint: usize,
) -> bool {
    let other = if current == endpoint {
        neighbor
    } else if neighbor == endpoint {
        current
    } else {
        return false;
    };

    same_point(nodes[other], containing_cell_center(nodes[endpoint]))
}

fn containing_cell_center(point: Point2) -> Point2 {
    Point2::new(point.x.floor() + 0.5, point.y.floor() + 0.5)
}

fn next_unvisited_node(distances: &[f64], visited: &[bool]) -> Option<usize> {
    let mut best_index = None;
    let mut best_cost = f64::INFINITY;

    for (index, cost) in distances.iter().copied().enumerate() {
        if visited[index] || cost + EPSILON >= best_cost {
            continue;
        }
        best_cost = cost;
        best_index = Some(index);
    }

    best_index
}

fn candidate_nodes(grid: &Grid, request: InterpolatedSearchRequest) -> Vec<Point2> {
    let mut nodes = vec![request.start, request.goal];

    for index in 0..grid.cell_count() {
        let point = grid.point_from_index(index);
        if !grid.is_walkable(point) {
            continue;
        }

        let center = cell_center(point);
        if same_point(center, request.start) || same_point(center, request.goal) {
            continue;
        }

        nodes.push(center);
    }

    nodes
}

fn same_point(left: Point2, right: Point2) -> bool {
    (left.x - right.x).abs() <= EPSILON && (left.y - right.y).abs() <= EPSILON
}

fn axis_aligned(left: Point2, right: Point2) -> bool {
    (left.x - right.x).abs() <= EPSILON || (left.y - right.y).abs() <= EPSILON
}

fn cell_center(point: Point) -> Point2 {
    Point2::new(point.x as f64 + 0.5, point.y as f64 + 0.5)
}