1use std::collections::HashMap;
7
8use crate::grid::{GridMap, Node};
9use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
10
11#[derive(Debug, Clone)]
13pub struct BidirectionalBFSConfig {
14 pub resolution: f64,
16 pub robot_radius: f64,
18}
19
20impl Default for BidirectionalBFSConfig {
21 fn default() -> Self {
22 Self {
23 resolution: 1.0,
24 robot_radius: 0.5,
25 }
26 }
27}
28
29impl BidirectionalBFSConfig {
30 pub fn validate(&self) -> RoboticsResult<()> {
31 if !self.resolution.is_finite() || self.resolution <= 0.0 {
32 return Err(RoboticsError::InvalidParameter(format!(
33 "resolution must be positive and finite, got {}",
34 self.resolution
35 )));
36 }
37 if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
38 return Err(RoboticsError::InvalidParameter(format!(
39 "robot_radius must be non-negative and finite, got {}",
40 self.robot_radius
41 )));
42 }
43 Ok(())
44 }
45}
46
47pub struct BidirectionalBFSPlanner {
49 grid_map: GridMap,
50 #[allow(dead_code)]
51 config: BidirectionalBFSConfig,
52 motion: Vec<(i32, i32, f64)>,
53}
54
55impl BidirectionalBFSPlanner {
56 pub fn new(ox: &[f64], oy: &[f64], config: BidirectionalBFSConfig) -> Self {
57 Self::try_new(ox, oy, config).expect("invalid BidirectionalBFS planner input")
58 }
59
60 pub fn try_new(ox: &[f64], oy: &[f64], config: BidirectionalBFSConfig) -> RoboticsResult<Self> {
61 config.validate()?;
62 let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
63 let motion = Self::get_motion_model();
64 Ok(Self {
65 grid_map,
66 config,
67 motion,
68 })
69 }
70
71 pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
72 let config = BidirectionalBFSConfig {
73 resolution,
74 robot_radius,
75 };
76 Self::new(ox, oy, config)
77 }
78
79 pub fn from_obstacle_points(
80 obstacles: &Obstacles,
81 config: BidirectionalBFSConfig,
82 ) -> RoboticsResult<Self> {
83 config.validate()?;
84 let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
85 let motion = Self::get_motion_model();
86 Ok(Self {
87 grid_map,
88 config,
89 motion,
90 })
91 }
92
93 #[deprecated(note = "use plan() or plan_xy() instead")]
94 pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
95 match self.plan_xy(sx, sy, gx, gy) {
96 Ok(path) => Some((path.x_coords(), path.y_coords())),
97 Err(_) => None,
98 }
99 }
100
101 pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
102 self.plan_impl(start, goal)
103 }
104
105 pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
106 self.plan_impl(Point2D::new(sx, sy), Point2D::new(gx, gy))
107 }
108
109 pub fn grid_map(&self) -> &GridMap {
110 &self.grid_map
111 }
112
113 fn get_motion_model() -> Vec<(i32, i32, f64)> {
114 vec![
115 (1, 0, 1.0),
116 (0, 1, 1.0),
117 (-1, 0, 1.0),
118 (0, -1, 1.0),
119 (-1, -1, std::f64::consts::SQRT_2),
120 (-1, 1, std::f64::consts::SQRT_2),
121 (1, -1, std::f64::consts::SQRT_2),
122 (1, 1, std::f64::consts::SQRT_2),
123 ]
124 }
125
126 fn build_half_path(&self, goal_index: usize, node_storage: &[Node]) -> Vec<Point2D> {
127 let mut points = Vec::new();
128 let mut current_index = Some(goal_index);
129 while let Some(index) = current_index {
130 let node = &node_storage[index];
131 points.push(Point2D::new(
132 self.grid_map.calc_x_position(node.x),
133 self.grid_map.calc_y_position(node.y),
134 ));
135 current_index = node.parent_index;
136 }
137 points.reverse();
138 points
139 }
140
141 fn best_open_key(&self, open_set: &HashMap<i32, usize>, node_storage: &[Node]) -> (i32, f64) {
142 open_set
143 .iter()
144 .min_by(|a, b| {
145 node_storage[*a.1]
146 .cost
147 .partial_cmp(&node_storage[*b.1].cost)
148 .unwrap_or(std::cmp::Ordering::Equal)
149 })
150 .map(|(k, idx)| (*k, node_storage[*idx].cost))
151 .unwrap()
152 }
153
154 fn best_known_index(
155 open_set: &HashMap<i32, usize>,
156 closed_set: &HashMap<i32, usize>,
157 node_storage: &[Node],
158 grid_index: i32,
159 ) -> Option<usize> {
160 match (open_set.get(&grid_index), closed_set.get(&grid_index)) {
161 (Some(&open_idx), Some(&closed_idx)) => {
162 if node_storage[open_idx].cost <= node_storage[closed_idx].cost {
163 Some(open_idx)
164 } else {
165 Some(closed_idx)
166 }
167 }
168 (Some(&open_idx), None) => Some(open_idx),
169 (None, Some(&closed_idx)) => Some(closed_idx),
170 (None, None) => None,
171 }
172 }
173
174 fn update_best_meeting(
175 best_meeting: &mut Option<(usize, usize, f64)>,
176 forward_index: usize,
177 forward_cost: f64,
178 backward_index: usize,
179 backward_cost: f64,
180 ) {
181 let total_cost = forward_cost + backward_cost;
182 if best_meeting
183 .as_ref()
184 .map_or(true, |(_, _, best_cost)| total_cost < *best_cost)
185 {
186 *best_meeting = Some((forward_index, backward_index, total_cost));
187 }
188 }
189
190 fn build_meeting_path(
191 &self,
192 forward_index: usize,
193 backward_index: usize,
194 fwd_storage: &[Node],
195 bwd_storage: &[Node],
196 ) -> Path2D {
197 let mut path = self.build_half_path(forward_index, fwd_storage);
198 let mut bwd_path = self.build_half_path(backward_index, bwd_storage);
199 bwd_path.reverse();
200 if !bwd_path.is_empty() {
201 path.extend_from_slice(&bwd_path[1..]);
202 }
203 Path2D::from_points(path)
204 }
205
206 fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
207 if self.grid_map.is_valid(x, y) {
208 return Ok(());
209 }
210 Err(RoboticsError::PlanningError(format!(
211 "{} position is invalid",
212 label
213 )))
214 }
215
216 fn plan_impl(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
217 let start_x = self.grid_map.calc_x_index(start.x);
218 let start_y = self.grid_map.calc_y_index(start.y);
219 let goal_x = self.grid_map.calc_x_index(goal.x);
220 let goal_y = self.grid_map.calc_y_index(goal.y);
221
222 self.ensure_query_is_valid(start_x, start_y, "Start")?;
223 self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;
224
225 let mut fwd_open: HashMap<i32, usize> = HashMap::new();
227 let mut fwd_closed: HashMap<i32, usize> = HashMap::new();
228 let mut fwd_storage: Vec<Node> = Vec::new();
229
230 fwd_storage.push(Node::new(start_x, start_y, 0.0, None));
231 fwd_open.insert(self.grid_map.calc_index(start_x, start_y), 0);
232
233 let mut bwd_open: HashMap<i32, usize> = HashMap::new();
235 let mut bwd_closed: HashMap<i32, usize> = HashMap::new();
236 let mut bwd_storage: Vec<Node> = Vec::new();
237
238 bwd_storage.push(Node::new(goal_x, goal_y, 0.0, None));
239 bwd_open.insert(self.grid_map.calc_index(goal_x, goal_y), 0);
240
241 let mut best_meeting: Option<(usize, usize, f64)> = None;
242
243 loop {
244 if fwd_open.is_empty() || bwd_open.is_empty() {
245 return best_meeting
246 .map(|(fwd_idx, bwd_idx, _)| {
247 self.build_meeting_path(fwd_idx, bwd_idx, &fwd_storage, &bwd_storage)
248 })
249 .ok_or_else(|| RoboticsError::PlanningError("No path found".to_string()));
250 }
251
252 let (fwd_key, min_cost_fwd) = self.best_open_key(&fwd_open, &fwd_storage);
253 let (bwd_key, min_cost_bwd) = self.best_open_key(&bwd_open, &bwd_storage);
254
255 if let Some((fwd_idx, bwd_idx, best_cost)) = best_meeting {
256 if min_cost_fwd >= best_cost && min_cost_bwd >= best_cost {
257 return Ok(self.build_meeting_path(
258 fwd_idx,
259 bwd_idx,
260 &fwd_storage,
261 &bwd_storage,
262 ));
263 }
264 }
265
266 if min_cost_fwd <= min_cost_bwd {
267 let fwd_idx = fwd_open.remove(&fwd_key).unwrap();
268 let fwd_node = &fwd_storage[fwd_idx];
269 let fx = fwd_node.x;
270 let fy = fwd_node.y;
271 let fcost = fwd_node.cost;
272 let fwd_grid_index = self.grid_map.calc_index(fx, fy);
273
274 if let Some(&existing_closed_idx) = fwd_closed.get(&fwd_grid_index) {
275 if fwd_storage[existing_closed_idx].cost <= fcost {
276 continue;
277 }
278 }
279
280 if let Some(bwd_idx) =
281 Self::best_known_index(&bwd_open, &bwd_closed, &bwd_storage, fwd_grid_index)
282 {
283 Self::update_best_meeting(
284 &mut best_meeting,
285 fwd_idx,
286 fcost,
287 bwd_idx,
288 bwd_storage[bwd_idx].cost,
289 );
290 }
291
292 fwd_closed.insert(fwd_grid_index, fwd_idx);
293
294 for &(dx, dy, move_cost) in &self.motion {
295 let nx = fx + dx;
296 let ny = fy + dy;
297 let new_grid_index = self.grid_map.calc_index(nx, ny);
298
299 if !self.grid_map.is_valid_offset(fx, fy, dx, dy) {
300 continue;
301 }
302
303 if let Some(&closed_idx) = fwd_closed.get(&new_grid_index) {
304 if fwd_storage[closed_idx].cost <= fcost + move_cost {
305 continue;
306 }
307 }
308
309 if let Some(&open_idx) = fwd_open.get(&new_grid_index) {
310 if fwd_storage[open_idx].cost <= fcost + move_cost {
311 continue;
312 }
313 }
314
315 fwd_storage.push(Node::new(nx, ny, fcost + move_cost, Some(fwd_idx)));
316 let new_idx = fwd_storage.len() - 1;
317 fwd_open.insert(new_grid_index, new_idx);
318
319 if let Some(bwd_idx) =
320 Self::best_known_index(&bwd_open, &bwd_closed, &bwd_storage, new_grid_index)
321 {
322 Self::update_best_meeting(
323 &mut best_meeting,
324 new_idx,
325 fwd_storage[new_idx].cost,
326 bwd_idx,
327 bwd_storage[bwd_idx].cost,
328 );
329 }
330 }
331 } else {
332 let bwd_idx = bwd_open.remove(&bwd_key).unwrap();
333 let bwd_node = &bwd_storage[bwd_idx];
334 let bx = bwd_node.x;
335 let by = bwd_node.y;
336 let bcost = bwd_node.cost;
337 let bwd_grid_index = self.grid_map.calc_index(bx, by);
338
339 if let Some(&existing_closed_idx) = bwd_closed.get(&bwd_grid_index) {
340 if bwd_storage[existing_closed_idx].cost <= bcost {
341 continue;
342 }
343 }
344
345 if let Some(fwd_idx) =
346 Self::best_known_index(&fwd_open, &fwd_closed, &fwd_storage, bwd_grid_index)
347 {
348 Self::update_best_meeting(
349 &mut best_meeting,
350 fwd_idx,
351 fwd_storage[fwd_idx].cost,
352 bwd_idx,
353 bcost,
354 );
355 }
356
357 bwd_closed.insert(bwd_grid_index, bwd_idx);
358
359 for &(dx, dy, move_cost) in &self.motion {
360 let nx = bx + dx;
361 let ny = by + dy;
362 let new_grid_index = self.grid_map.calc_index(nx, ny);
363
364 if !self.grid_map.is_valid_offset(bx, by, dx, dy) {
365 continue;
366 }
367
368 if let Some(&closed_idx) = bwd_closed.get(&new_grid_index) {
369 if bwd_storage[closed_idx].cost <= bcost + move_cost {
370 continue;
371 }
372 }
373
374 if let Some(&open_idx) = bwd_open.get(&new_grid_index) {
375 if bwd_storage[open_idx].cost <= bcost + move_cost {
376 continue;
377 }
378 }
379
380 bwd_storage.push(Node::new(nx, ny, bcost + move_cost, Some(bwd_idx)));
381 let new_idx = bwd_storage.len() - 1;
382 bwd_open.insert(new_grid_index, new_idx);
383
384 if let Some(fwd_idx) =
385 Self::best_known_index(&fwd_open, &fwd_closed, &fwd_storage, new_grid_index)
386 {
387 Self::update_best_meeting(
388 &mut best_meeting,
389 fwd_idx,
390 fwd_storage[fwd_idx].cost,
391 new_idx,
392 bwd_storage[new_idx].cost,
393 );
394 }
395 }
396 }
397 }
398 }
399}
400
401impl PathPlanner for BidirectionalBFSPlanner {
402 fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
403 self.plan_impl(start, goal)
404 }
405}
406
407#[cfg(test)]
408mod tests {
409 use super::*;
410 use crate::moving_ai::{MovingAiMap, MovingAiScenario};
411
412 fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
413 let mut ox = Vec::new();
414 let mut oy = Vec::new();
415
416 for i in 0..11 {
417 ox.push(i as f64);
418 oy.push(0.0);
419 ox.push(i as f64);
420 oy.push(10.0);
421 ox.push(0.0);
422 oy.push(i as f64);
423 ox.push(10.0);
424 oy.push(i as f64);
425 }
426
427 for i in 4..7 {
428 ox.push(5.0);
429 oy.push(i as f64);
430 }
431
432 (ox, oy)
433 }
434
435 #[test]
436 #[allow(deprecated)]
437 fn test_bidirectional_bfs_finds_path() {
438 let (ox, oy) = create_simple_obstacles();
439 let planner = BidirectionalBFSPlanner::from_obstacles(&ox, &oy, 0.5, 0.1);
440
441 let result = planner.planning(2.0, 2.0, 8.0, 8.0);
442 assert!(result.is_some());
443 let (rx, ry) = result.unwrap();
444 assert!(rx.len() > 2);
445 assert_eq!(rx.len(), ry.len());
446 }
447
448 #[test]
449 fn test_bidirectional_bfs_plan_returns_valid_path() {
450 let (ox, oy) = create_simple_obstacles();
451 let planner = BidirectionalBFSPlanner::from_obstacles(&ox, &oy, 0.5, 0.1);
452
453 let path = planner.plan(Point2D::new(2.0, 2.0), Point2D::new(8.0, 8.0));
454 assert!(path.is_ok());
455 let path = path.unwrap();
456 assert!(path.len() > 2);
457 }
458
459 #[test]
460 fn test_bidirectional_bfs_no_path() {
461 let mut ox = Vec::new();
462 let mut oy = Vec::new();
463
464 for i in -1..=61 {
466 let v = i as f64;
467 ox.push(v);
468 oy.push(-1.0);
469 ox.push(v);
470 oy.push(61.0);
471 ox.push(-1.0);
472 oy.push(v);
473 ox.push(61.0);
474 oy.push(v);
475 }
476
477 for i in -1..=61 {
479 for dx in -1..=1 {
480 ox.push(30.0 + dx as f64);
481 oy.push(i as f64);
482 }
483 }
484
485 let planner = BidirectionalBFSPlanner::from_obstacles(&ox, &oy, 1.0, 0.0);
486 let result = planner.plan(Point2D::new(10.0, 30.0), Point2D::new(50.0, 30.0));
487 assert!(result.is_err());
488 }
489
490 #[test]
491 fn test_bidirectional_bfs_from_obstacle_points() {
492 let (ox, oy) = create_simple_obstacles();
493 let obstacles = Obstacles::try_from_xy(&ox, &oy).expect("obstacle creation should succeed");
494 let config = BidirectionalBFSConfig {
495 resolution: 0.5,
496 robot_radius: 0.1,
497 };
498 let planner = BidirectionalBFSPlanner::from_obstacle_points(&obstacles, config);
499 assert!(planner.is_ok());
500 }
501
502 #[test]
503 #[allow(deprecated)]
504 fn test_bidirectional_bfs_legacy_interface() {
505 let (ox, oy) = create_simple_obstacles();
506 let planner = BidirectionalBFSPlanner::from_obstacles(&ox, &oy, 0.5, 0.1);
507
508 let result = planner.planning(2.0, 2.0, 8.0, 8.0);
509 assert!(result.is_some());
510 }
511
512 #[test]
513 #[ignore = "long-running MovingAI benchmark"]
514 fn test_bidirectional_bfs_matches_moving_ai_arena2_bucket_80_optimal_length() {
515 let map = MovingAiMap::parse_str(include_str!("../benchdata/moving_ai/dao/arena2.map"))
516 .expect("arena2 MovingAI map should parse");
517 let scenario =
518 MovingAiScenario::parse_str(include_str!("../benchdata/moving_ai/dao/arena2.map.scen"))
519 .expect("arena2 MovingAI scenarios should parse")
520 .into_iter()
521 .find(|row| row.bucket == 80)
522 .expect("arena2 MovingAI bucket 80 scenario should exist");
523
524 let planner = BidirectionalBFSPlanner::from_obstacle_points(
525 &map.to_obstacles(),
526 BidirectionalBFSConfig {
527 resolution: 1.0,
528 robot_radius: 0.5,
529 },
530 )
531 .expect("Bidirectional BFS planner should build from arena2 obstacles");
532
533 let start = map
534 .planning_point(scenario.start_x, scenario.start_y)
535 .expect("arena2 start should be valid");
536 let goal = map
537 .planning_point(scenario.goal_x, scenario.goal_y)
538 .expect("arena2 goal should be valid");
539
540 let path = planner
541 .plan(start, goal)
542 .expect("Bidirectional BFS should solve the arena2 bucket 80 scenario");
543
544 assert!(
545 (path.total_length() - scenario.optimal_length).abs() < 1e-6,
546 "Bidirectional BFS path length {} should match MovingAI optimal {}",
547 path.total_length(),
548 scenario.optimal_length
549 );
550 }
551}