cell-map 0.5.3

Many-layered 2D cellular generic map
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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187

//! Provides the [`CellMapMetadata`] struct which describes metadata about a [`CellMap`], such as
//! its location and size.
//!
//! [`CellMap`]: crate::CellMap

// ------------------------------------------------------------------------------------------------
// IMPORTS
// ------------------------------------------------------------------------------------------------

use nalgebra::{Affine2, Isometry2, Matrix3, Point2, Vector2};
use serde::{Deserialize, Serialize};

use crate::{cell_map::Bounds, CellMapParams};

// ------------------------------------------------------------------------------------------------
// STRUCTS
// ------------------------------------------------------------------------------------------------

/// Provides metadata about a [`CellMap`], such as size and location.
///
/// The data in this struct is constructed from the [`CellMapParams`] provided by the user at
/// construction of the map.
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
pub(crate) struct CellMapMetadata {
    /// The size (resolution) of each cell in the map, in both the `x` and `y` directions.
    pub cell_size: Vector2<f64>,

    /// The bounds of the map
    pub cell_bounds: Bounds,

    /// The number of cells in the map
    pub num_cells: Vector2<usize>,

    /// The precision to use when determining cell boundaries.
    ///
    /// This precision factor allows us to account for times when a cell position should fit into a
    /// particular cell index, but due to floating point rounding does not. For example take a map
    /// with a `cell_size = [0.1, 0.1]`, the cell index of the position `[0.7, 0.1]` should be `[7,
    /// 1`], however the positions floating point index would be calculated as `[6.999999999999998,
    /// 0.9999999999999999]`, which if `floor()`ed to fit into a `usize` would give the incorrect
    /// index `[6, 0]`.
    ///
    /// When calculating cell index we therefore `floor` the floating point index unless it is
    /// within `cell_size * cell_boundary_precision`, in which case we round up to the next cell.
    /// Mutliplying by `cell_size` allows this value to be independent of the scale of the map.
    ///
    /// This value defaults to `1e-10`.
    pub cell_boundary_precision: f64,

    /// The transform between the map's frame and the parent frame. This is the transform that will
    /// be applied when going from a cell index to a parent-frame position.
    pub to_parent: Affine2<f64>,
}

// ------------------------------------------------------------------------------------------------
// IMPLS
// ------------------------------------------------------------------------------------------------

impl CellMapMetadata {
    /// Returns whether or not the given index is inside the map.
    pub fn is_in_map(&self, index: Point2<usize>) -> bool {
        index.x < self.num_cells.x && index.y < self.num_cells.y
    }

    /// Returns the position in the parent frame of the centre of the given cell index.
    ///
    /// Returns `None` if the given `index` is not inside the map.
    pub fn position(&self, index: Point2<usize>) -> Option<Point2<f64>> {
        if self.is_in_map(index) {
            Some(self.position_unchecked(index))
        } else {
            None
        }
    }

    /// Returns the position in the parent frame of the centre of the given cell index, without
    /// checking that the `index` is inside the map.
    ///
    /// # Safety
    ///
    /// This method won't panic if `index` is outside the map, but it's result can't be guaranteed
    /// to be a position in the map.
    pub fn position_unchecked(&self, index: Point2<usize>) -> Point2<f64> {
        // Get the centre of the cell, which is + 0.5 cells in the x and y direction, also account
        // for the bounds by adding the lower bound
        let index_centre = index.cast()
            + Vector2::new(
                self.cell_bounds.x.0 as f64 + 0.5,
                self.cell_bounds.y.0 as f64 + 0.5,
            );
        self.to_parent.transform_point(&index_centre)
    }

    /// Get the cell index of the given poisition.
    ///
    /// Returns `None` if the given `position` is not inside the map.
    pub fn index(&self, position: Point2<f64>) -> Option<Point2<usize>> {
        let index = unsafe { self.index_unchecked(position) };

        if index.x < 0 || index.y < 0 {
            return None;
        }

        let index = index.map(|v| v as usize);

        if self.is_in_map(index) {
            Some(index)
        } else {
            None
        }
    }

    /// Get the cell index of the given poisition, without checking that the position is inside the
    /// map.
    ///
    /// # Safety
    ///
    /// This function will not panic if `position` is outside the map, but use of the result to
    /// index into the map is not guaranteed to be safe. It is possible for this function to return
    /// a negative index value, which would indicate that the cell is outside the map.
    pub unsafe fn index_unchecked(&self, position: Point2<f64>) -> Point2<isize> {
        let cell = self.get_cell(position);

        // What we have now is a "point", i.e. a map-frame point relative to the map origin. But if
        // we want the index we have to account for the bounds of the map, so we must pass this
        // through the bounds
        self.cell_bounds.get_index_unchecked(cell)
    }

    /// Gets the map-origin relative cell location of the given position.
    pub fn get_cell(&self, position: Point2<f64>) -> Point2<isize> {
        let els: Vec<isize> = self
            .to_parent
            .inverse_transform_point(&position)
            .iter()
            .zip(self.cell_size.iter())
            .map(|(&v, &s)| {
                let v_floor = v as isize;
                let v_next_floor = (s * self.cell_boundary_precision + v) as isize;

                if v_floor != v_next_floor {
                    v_next_floor
                } else {
                    v_floor
                }
            })
            .collect();

        Point2::new(els[0], els[1])
    }

    pub(crate) fn calc_to_parent(
        position: Vector2<f64>,
        rotation_rad: f64,
        cell_size: Vector2<f64>,
    ) -> Affine2<f64> {
        // First build isometry to convert from the parent to map
        let isom_from_parent = Isometry2::new(position, rotation_rad);

        // Scale transformation matrix, based on cell size.
        let scale = Matrix3::new(cell_size.x, 0.0, 0.0, 0.0, cell_size.y, 0.0, 0.0, 0.0, 1.0);

        // Build the affine by multiplying isom and scale, which will take the translation and
        // rotation of isom and scale it by the cell size. Scale must come first so that the isom,
        // which is in parent coordinates, is not scaled itself. Get the inverse of
        // isom_from_parent to get the to_parent
        Affine2::from_matrix_unchecked(isom_from_parent.to_matrix() * scale)
    }
}

impl From<CellMapParams> for CellMapMetadata {
    fn from(params: CellMapParams) -> Self {
        let to_parent = Self::calc_to_parent(
            params.position_in_parent,
            params.rotation_in_parent_rad,
            params.cell_size,
        );

        Self {
            cell_size: params.cell_size,
            cell_bounds: params.cell_bounds,
            num_cells: params.cell_bounds.get_num_cells(),
            cell_boundary_precision: params.cell_boundary_precision,
            to_parent,
        }
    }
}