1use nalgebra::DMatrix;
6use std::collections::HashMap;
7use std::f64::consts::PI;
8use std::fs::File;
9use std::io::Write;
10
11pub const EXTEND_AREA: f64 = 10.0;
13pub const XY_RESOLUTION: f64 = 0.25;
15pub const YAW_RESOLUTION: f64 = PI / 18.0;
17
18#[derive(Clone)]
20struct CellInfo {
21 ix: usize,
22 iy: usize,
23 distance: f64,
24}
25
26struct PrecastDB {
28 cells_by_angle: HashMap<i32, Vec<CellInfo>>,
30}
31
32pub struct RayCastingGridMap {
34 pub data: DMatrix<f64>,
35 pub min_x: f64,
36 pub min_y: f64,
37 pub max_x: f64,
38 pub max_y: f64,
39 pub resolution: f64,
40 pub x_width: usize,
41 pub y_width: usize,
42 precast_db: PrecastDB,
43 origin_ix: usize,
44 origin_iy: usize,
45}
46
47impl RayCastingGridMap {
48 pub fn new(ox: &[f64], oy: &[f64], resolution: f64, yaw_resolution: f64) -> Self {
50 let min_x = ox.iter().cloned().fold(f64::INFINITY, f64::min) - EXTEND_AREA;
51 let min_y = oy.iter().cloned().fold(f64::INFINITY, f64::min) - EXTEND_AREA;
52 let max_x = ox.iter().cloned().fold(f64::NEG_INFINITY, f64::max) + EXTEND_AREA;
53 let max_y = oy.iter().cloned().fold(f64::NEG_INFINITY, f64::max) + EXTEND_AREA;
54
55 let x_width = ((max_x - min_x) / resolution).round() as usize;
56 let y_width = ((max_y - min_y) / resolution).round() as usize;
57
58 let origin_x = (min_x + max_x) / 2.0;
59 let origin_y = (min_y + max_y) / 2.0;
60 let origin_ix = ((origin_x - min_x) / resolution).round() as usize;
61 let origin_iy = ((origin_y - min_y) / resolution).round() as usize;
62
63 let data = DMatrix::from_element(x_width, y_width, 0.0);
64
65 let precast_db = Self::precompute_db(
66 x_width,
67 y_width,
68 origin_ix,
69 origin_iy,
70 resolution,
71 yaw_resolution,
72 );
73
74 let mut grid_map = RayCastingGridMap {
75 data,
76 min_x,
77 min_y,
78 max_x,
79 max_y,
80 resolution,
81 x_width,
82 y_width,
83 precast_db,
84 origin_ix,
85 origin_iy,
86 };
87
88 for (obs_x, obs_y) in ox.iter().zip(oy.iter()) {
89 grid_map.ray_casting(*obs_x, *obs_y, yaw_resolution);
90 }
91
92 grid_map
93 }
94
95 fn precompute_db(
97 x_width: usize,
98 y_width: usize,
99 origin_ix: usize,
100 origin_iy: usize,
101 resolution: f64,
102 yaw_resolution: f64,
103 ) -> PrecastDB {
104 let mut cells_by_angle: HashMap<i32, Vec<CellInfo>> = HashMap::new();
105
106 for ix in 0..x_width {
107 for iy in 0..y_width {
108 let dx = ix as f64 - origin_ix as f64;
109 let dy = iy as f64 - origin_iy as f64;
110
111 let distance = (dx * dx + dy * dy).sqrt() * resolution;
112 let angle = atan_zero_to_twopi(dy, dx);
113 let angle_id = (angle / yaw_resolution).floor() as i32;
114
115 let cell_info = CellInfo { ix, iy, distance };
116
117 cells_by_angle.entry(angle_id).or_default().push(cell_info);
118 }
119 }
120
121 for cells in cells_by_angle.values_mut() {
122 cells.sort_by(|a, b| a.distance.partial_cmp(&b.distance).unwrap());
123 }
124
125 PrecastDB { cells_by_angle }
126 }
127
128 fn ray_casting(&mut self, obs_x: f64, obs_y: f64, yaw_resolution: f64) {
130 let obs_ix = ((obs_x - self.min_x) / self.resolution).round() as i32;
131 let obs_iy = ((obs_y - self.min_y) / self.resolution).round() as i32;
132
133 if obs_ix < 0
134 || obs_ix >= self.x_width as i32
135 || obs_iy < 0
136 || obs_iy >= self.y_width as i32
137 {
138 return;
139 }
140
141 let dx = obs_ix as f64 - self.origin_ix as f64;
142 let dy = obs_iy as f64 - self.origin_iy as f64;
143 let obs_angle = atan_zero_to_twopi(dy, dx);
144 let obs_distance = (dx * dx + dy * dy).sqrt() * self.resolution;
145 let angle_id = (obs_angle / yaw_resolution).floor() as i32;
146
147 if let Some(cells) = self.precast_db.cells_by_angle.get(&angle_id) {
148 for cell in cells {
149 if cell.distance < obs_distance {
150 self.data[(cell.ix, cell.iy)] = 0.5;
151 } else if (cell.distance - obs_distance).abs() < self.resolution {
152 self.data[(cell.ix, cell.iy)] = 1.0;
153 break;
154 }
155 }
156 }
157 }
158
159 pub fn get_index(&self, x: f64, y: f64) -> Option<(usize, usize)> {
161 let ix = ((x - self.min_x) / self.resolution).floor() as i32;
162 let iy = ((y - self.min_y) / self.resolution).floor() as i32;
163
164 if ix >= 0 && ix < self.x_width as i32 && iy >= 0 && iy < self.y_width as i32 {
165 Some((ix as usize, iy as usize))
166 } else {
167 None
168 }
169 }
170
171 pub fn get_occupancy(&self, x: f64, y: f64) -> Option<f64> {
173 self.get_index(x, y).map(|(ix, iy)| self.data[(ix, iy)])
174 }
175}
176
177fn atan_zero_to_twopi(y: f64, x: f64) -> f64 {
179 let angle = y.atan2(x);
180 if angle < 0.0 {
181 angle + 2.0 * PI
182 } else {
183 angle
184 }
185}
186
187fn occupancy_style(value: f64) -> (&'static str, f64) {
188 if value >= 0.99 {
189 ("#b91c1c", 0.95)
190 } else if value >= 0.49 {
191 ("#bfdbfe", 0.92)
192 } else {
193 ("#f8fafc", 0.75)
194 }
195}
196
197pub fn build_ray_casting_grid_map_svg(
199 grid_map: &RayCastingGridMap,
200 ox: &[f64],
201 oy: &[f64],
202) -> String {
203 let width = 920.0;
204 let height = 700.0;
205 let margin = 72.0;
206 let side_panel_width = 220.0;
207 let available_plot_width = width - 2.0 * margin - side_panel_width;
208 let available_plot_height = height - 2.0 * margin;
209 let x_range = grid_map.max_x - grid_map.min_x;
210 let y_range = grid_map.max_y - grid_map.min_y;
211 let scale = (available_plot_width / x_range)
212 .min(available_plot_height / y_range)
213 .max(1.0);
214 let plot_width = x_range * scale;
215 let plot_height = y_range * scale;
216 let plot_left = margin;
217 let plot_top = margin + (available_plot_height - plot_height) / 2.0;
218 let legend_x = plot_left + plot_width + 36.0;
219 let legend_y = plot_top + 24.0;
220
221 let to_svg_x = |x: f64| plot_left + (x - grid_map.min_x) * scale;
222 let to_svg_y = |y: f64| plot_top + plot_height - (y - grid_map.min_y) * scale;
223 let cell_size = grid_map.resolution * scale;
224 let origin_x = grid_map.min_x + grid_map.origin_ix as f64 * grid_map.resolution;
225 let origin_y = grid_map.min_y + grid_map.origin_iy as f64 * grid_map.resolution;
226
227 let mut svg = String::new();
228 svg.push_str(&format!(
229 "<?xml version='1.0' encoding='UTF-8'?>\n<svg xmlns='http://www.w3.org/2000/svg' width='{width}' height='{height}' viewBox='0 0 {width} {height}'>\n"
230 ));
231 svg.push_str("<rect width='100%' height='100%' fill='white'/>\n");
232 svg.push_str(&format!(
233 "<text x='{:.1}' y='38' text-anchor='middle' font-size='24' font-family='sans-serif' font-weight='700' fill='#111827'>Ray Casting Grid Map</text>\n",
234 width / 2.0
235 ));
236 svg.push_str(&format!(
237 "<rect x='{plot_left:.1}' y='{plot_top:.1}' width='{plot_width:.1}' height='{plot_height:.1}' fill='#fffdf7' stroke='#d6d3d1' stroke-width='2'/>\n"
238 ));
239
240 let x_tick_start = (grid_map.min_x / 10.0).ceil() as i32;
241 let x_tick_end = (grid_map.max_x / 10.0).floor() as i32;
242 for tick in x_tick_start..=x_tick_end {
243 let x = tick as f64 * 10.0;
244 let sx = to_svg_x(x);
245 svg.push_str(&format!(
246 "<line x1='{sx:.2}' y1='{plot_top:.2}' x2='{sx:.2}' y2='{:.2}' stroke='#ece7dc' stroke-width='1'/>\n",
247 plot_top + plot_height
248 ));
249 svg.push_str(&format!(
250 "<text x='{sx:.2}' y='{:.2}' text-anchor='middle' font-size='14' font-family='sans-serif' fill='#374151'>{x:.0}</text>\n",
251 plot_top + plot_height + 24.0
252 ));
253 }
254
255 let y_tick_start = (grid_map.min_y / 10.0).ceil() as i32;
256 let y_tick_end = (grid_map.max_y / 10.0).floor() as i32;
257 for tick in y_tick_start..=y_tick_end {
258 let y = tick as f64 * 10.0;
259 let sy = to_svg_y(y);
260 svg.push_str(&format!(
261 "<line x1='{plot_left:.2}' y1='{sy:.2}' x2='{:.2}' y2='{sy:.2}' stroke='#ece7dc' stroke-width='1'/>\n",
262 plot_left + plot_width
263 ));
264 svg.push_str(&format!(
265 "<text x='{:.2}' y='{:.2}' text-anchor='end' font-size='14' font-family='sans-serif' fill='#374151'>{y:.0}</text>\n",
266 plot_left - 10.0,
267 sy + 5.0
268 ));
269 }
270
271 for iy in 0..grid_map.y_width {
272 for ix in 0..grid_map.x_width {
273 let value = grid_map.data[(ix, iy)];
274 let x = grid_map.min_x + ix as f64 * grid_map.resolution;
275 let y = grid_map.min_y + iy as f64 * grid_map.resolution;
276 let sx = to_svg_x(x);
277 let sy = to_svg_y(y + grid_map.resolution);
278 let (fill, opacity) = occupancy_style(value);
279 svg.push_str(&format!(
280 "<rect x='{sx:.2}' y='{sy:.2}' width='{cell_size:.2}' height='{cell_size:.2}' fill='{fill}' fill-opacity='{opacity:.3}' stroke='none'/>\n"
281 ));
282 }
283 }
284
285 for (x, y) in ox.iter().zip(oy.iter()) {
286 svg.push_str(&format!(
287 "<circle cx='{:.2}' cy='{:.2}' r='3.8' fill='#111827' stroke='white' stroke-width='1.2'/>\n",
288 to_svg_x(*x),
289 to_svg_y(*y)
290 ));
291 }
292 svg.push_str(&format!(
293 "<circle cx='{:.2}' cy='{:.2}' r='6' fill='#2563eb' stroke='white' stroke-width='2'/>\n",
294 to_svg_x(origin_x),
295 to_svg_y(origin_y)
296 ));
297
298 svg.push_str(&format!(
299 "<rect x='{legend_x:.1}' y='{legend_y:.1}' width='182' height='156' rx='18' fill='white' stroke='#d6d3d1'/>\n"
300 ));
301 let legend_rows = [
302 ("#b91c1c", "Occupied cell"),
303 ("#bfdbfe", "Free space"),
304 ("#f8fafc", "Unknown cell"),
305 ];
306 for (index, (color, label)) in legend_rows.iter().enumerate() {
307 let row_y = legend_y + 28.0 + index as f64 * 26.0;
308 svg.push_str(&format!(
309 "<rect x='{:.1}' y='{:.1}' width='22' height='14' rx='4' fill='{color}' stroke='#cbd5e1'/>\n",
310 legend_x + 14.0,
311 row_y - 10.0
312 ));
313 svg.push_str(&format!(
314 "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>{label}</text>\n",
315 legend_x + 48.0,
316 row_y + 1.0
317 ));
318 }
319 let obstacle_row_y = legend_y + 112.0;
320 svg.push_str(&format!(
321 "<circle cx='{:.1}' cy='{obstacle_row_y:.1}' r='4.0' fill='#111827' stroke='white' stroke-width='1.2'/>\n",
322 legend_x + 25.0
323 ));
324 svg.push_str(&format!(
325 "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>Obstacle points</text>\n",
326 legend_x + 48.0,
327 obstacle_row_y + 1.0
328 ));
329 let sensor_row_y = legend_y + 138.0;
330 svg.push_str(&format!(
331 "<circle cx='{:.1}' cy='{sensor_row_y:.1}' r='5.5' fill='#2563eb' stroke='white' stroke-width='1.5'/>\n",
332 legend_x + 25.0
333 ));
334 svg.push_str(&format!(
335 "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>Sensor origin</text>\n",
336 legend_x + 48.0,
337 sensor_row_y + 1.0
338 ));
339
340 svg.push_str(&format!(
341 "<text x='{:.1}' y='{:.1}' font-size='15' font-family='sans-serif' font-weight='700' fill='#111827'>Occupancy bands</text>\n",
342 legend_x + 18.0,
343 legend_y + 190.0
344 ));
345 svg.push_str(&format!(
346 "<text x='{:.1}' y='{:.1}' font-size='13' font-family='monospace' fill='#374151'>1.0 obstacle</text>\n",
347 legend_x + 18.0,
348 legend_y + 216.0
349 ));
350 svg.push_str(&format!(
351 "<text x='{:.1}' y='{:.1}' font-size='13' font-family='monospace' fill='#374151'>0.5 free</text>\n",
352 legend_x + 18.0,
353 legend_y + 238.0
354 ));
355 svg.push_str(&format!(
356 "<text x='{:.1}' y='{:.1}' font-size='13' font-family='monospace' fill='#374151'>0.0 unknown</text>\n",
357 legend_x + 18.0,
358 legend_y + 260.0
359 ));
360
361 svg.push_str(&format!(
362 "<text x='{:.1}' y='{:.1}' text-anchor='middle' font-size='16' font-family='sans-serif' fill='#111827'>x [m]</text>\n",
363 plot_left + plot_width / 2.0,
364 plot_top + plot_height + 52.0
365 ));
366 svg.push_str(&format!(
367 "<text x='{:.1}' y='{:.1}' text-anchor='middle' font-size='16' font-family='sans-serif' fill='#111827' transform='rotate(-90, {:.1}, {:.1})'>y [m]</text>\n",
368 plot_left - 48.0,
369 plot_top + plot_height / 2.0,
370 plot_left - 48.0,
371 plot_top + plot_height / 2.0
372 ));
373 svg.push_str("</svg>\n");
374
375 svg
376}
377
378pub fn save_ray_casting_grid_map_svg(
380 filename: &str,
381 grid_map: &RayCastingGridMap,
382 ox: &[f64],
383 oy: &[f64],
384) -> std::io::Result<()> {
385 let svg = build_ray_casting_grid_map_svg(grid_map, ox, oy);
386 let mut file = File::create(filename)?;
387 file.write_all(svg.as_bytes())?;
388 Ok(())
389}
390
391#[cfg(test)]
392mod tests {
393 use super::*;
394
395 #[test]
396 fn test_ray_casting_svg_has_white_background() {
397 let grid_map = RayCastingGridMap::new(&[-1.0, 1.0], &[0.0, 0.0], 1.0, PI / 4.0);
398 let svg = build_ray_casting_grid_map_svg(&grid_map, &[-1.0, 1.0], &[0.0, 0.0]);
399
400 assert!(svg.contains("<rect width='100%' height='100%' fill='white'/>"));
401 assert!(svg.contains("Ray Casting Grid Map"));
402 assert!(svg.contains("Occupied cell"));
403 assert!(svg.contains("Sensor origin"));
404 }
405}