condor-pathfinding-grid 0.4.0

Grid pathfinding, preprocessing, replanning, and multi-agent algorithms for Condor.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113

//! Maximal open horizontal runs on grid vertex rows for interval Anya.
//!
//! [`RowRunIndex`] materializes, per horizontal vertex row `0..=height`, the
//! contiguous open spans that interval states project onto. Built once per
//! inspect-mode search; not retained across public oracle-supervised searches.

use crate::{
    Grid,
    any_angle::geometry::{is_endpoint_valid, sampling_segment_is_legal},
};
use condor_core::Point2;

/// One maximal open interval on a horizontal grid vertex row.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct RowRun {
    /// Inclusive left endpoint in continuous x (grid vertex coordinate).
    pub left: f64,
    /// Inclusive right endpoint in continuous x (grid vertex coordinate).
    pub right: f64,
}

/// Dense row-major index of maximal open horizontal runs.
#[derive(Debug, Clone, PartialEq)]
pub struct RowRunIndex {
    runs_by_row: Vec<Vec<RowRun>>,
    width: usize,
    height: usize,
}

impl RowRunIndex {
    /// Indexes maximal open runs on every horizontal vertex row `0..=height`.
    #[must_use]
    pub fn build(grid: &Grid) -> Self {
        let width = grid.width();
        let height = grid.height();
        let mut runs_by_row = Vec::with_capacity(height + 1);
        for row in 0..=height {
            runs_by_row.push(build_row_runs(grid, row));
        }
        Self {
            runs_by_row,
            width,
            height,
        }
    }

    /// Open runs on vertex row `row`, or empty when out of range.
    #[must_use]
    pub fn runs_on_row(&self, row: i32) -> &[RowRun] {
        if row < 0 || row as usize >= self.runs_by_row.len() {
            return &[];
        }
        &self.runs_by_row[row as usize]
    }

    /// Maximal open run containing `point` on its rounded vertex row, if any.
    #[must_use]
    pub fn run_containing(&self, point: Point2) -> Option<RowRun> {
        let row = point.y.round() as i32;
        let x = point.x;
        self.runs_on_row(row)
            .iter()
            .copied()
            .find(|run| x + 1e-12 >= run.left && x <= run.right + 1e-12)
    }

    /// Grid height in cells (run rows are `height + 1` vertex lines).
    #[must_use]
    pub const fn height(&self) -> usize {
        self.height
    }

    /// Approximate retained bytes for diagnostics (run storage only).
    #[must_use]
    pub fn bytes(&self) -> usize {
        self.runs_by_row.len() * std::mem::size_of::<Vec<RowRun>>()
            + self
                .runs_by_row
                .iter()
                .map(|row| row.len() * std::mem::size_of::<RowRun>())
                .sum::<usize>()
    }
}

fn build_row_runs(grid: &Grid, row: usize) -> Vec<RowRun> {
    let y = row as f64;
    let width = grid.width();
    let mut runs = Vec::new();
    let mut x = 0usize;
    while x <= width {
        let start = Point2::new(x as f64, y);
        if !is_endpoint_valid(grid, start) {
            x += 1;
            continue;
        }
        let mut end = x;
        while end < width {
            let next = Point2::new((end + 1) as f64, y);
            if !is_endpoint_valid(grid, next)
                || !sampling_segment_is_legal(grid, Point2::new(end as f64, y), next)
            {
                break;
            }
            end += 1;
        }
        runs.push(RowRun {
            left: x as f64,
            right: end as f64,
        });
        x = end + 1;
    }
    runs
}