1use std::cmp::Ordering;
24use std::collections::{BinaryHeap, VecDeque};
25
26use rust_robotics_core::{RoboticsError, RoboticsResult};
27
28const SQRT2: f64 = std::f64::consts::SQRT_2;
29
30#[derive(Debug, Clone, Copy, PartialEq, Eq)]
32pub enum Knowledge {
33 Unknown,
34 Free,
35 Occupied,
36}
37
38#[derive(Debug, Clone, PartialEq)]
40pub struct FrontierNavWorld {
41 pub width: i32,
42 pub height: i32,
43 pub occupied: Vec<Vec<bool>>,
45 pub start: (i32, i32),
46 pub goal: (i32, i32),
47}
48
49impl FrontierNavWorld {
50 pub fn new(
52 width: i32,
53 height: i32,
54 start: (i32, i32),
55 goal: (i32, i32),
56 ) -> RoboticsResult<Self> {
57 if width <= 0 || height <= 0 {
58 return Err(RoboticsError::InvalidParameter(
59 "frontier-nav world dimensions must be positive".to_string(),
60 ));
61 }
62 Ok(Self {
63 width,
64 height,
65 occupied: vec![vec![false; height as usize]; width as usize],
66 start,
67 goal,
68 })
69 }
70
71 fn in_bounds(&self, x: i32, y: i32) -> bool {
72 x >= 0 && y >= 0 && x < self.width && y < self.height
73 }
74
75 fn is_occupied(&self, x: i32, y: i32) -> bool {
76 !self.in_bounds(x, y) || self.occupied[x as usize][y as usize]
77 }
78
79 pub fn fill_rect(&mut self, min_x: i32, max_x: i32, min_y: i32, max_y: i32) {
81 for x in min_x.max(0)..=max_x.min(self.width - 1) {
82 for y in min_y.max(0)..=max_y.min(self.height - 1) {
83 self.occupied[x as usize][y as usize] = true;
84 }
85 }
86 }
87}
88
89#[derive(Debug, Clone, Copy, PartialEq)]
91pub struct FrontierChoice {
92 pub robot: (i32, i32),
93 pub frontier: (i32, i32),
94 pub score: f64,
95 pub line_of_sight: bool,
96}
97
98#[derive(Debug, Clone, PartialEq)]
100pub struct FrontierNavConfig {
101 pub sensor_range: f64,
103 pub step_budget: usize,
105 pub max_iterations: usize,
107 pub goal_weight: f64,
109 pub cost_weight: f64,
111 pub line_of_sight_weight: f64,
113 pub openness_weight: f64,
115}
116
117impl Default for FrontierNavConfig {
118 fn default() -> Self {
119 Self {
120 sensor_range: 5.0,
121 step_budget: 3,
122 max_iterations: 200,
123 goal_weight: 1.0,
124 cost_weight: 0.35,
125 line_of_sight_weight: 1.5,
126 openness_weight: 0.15,
127 }
128 }
129}
130
131#[derive(Debug, Clone, PartialEq)]
133pub struct FrontierNavReport {
134 pub path: Vec<(i32, i32)>,
135 pub reached_goal: bool,
136 pub path_length: f64,
137 pub frontier_selections: usize,
138 pub iterations: usize,
139 pub known_free_cells: usize,
140 pub selected_frontiers: Vec<FrontierChoice>,
141 pub known: Vec<Vec<Knowledge>>,
143}
144
145#[derive(Debug, Clone, PartialEq)]
147pub struct FrontierCandidate {
148 pub representative: (i32, i32),
149 pub members: Vec<(i32, i32)>,
150 pub openness: usize,
152 pub normalized_openness: f64,
153 pub goal_gain: f64,
154 pub travel_cost: Option<f64>,
155 pub line_of_sight: bool,
156 pub score: Option<f64>,
158}
159
160#[derive(Debug, Clone, Copy, PartialEq, Eq)]
162pub enum FrontierNavPhase {
163 Frontier,
164 Goal,
165 Arrived,
166 NoFrontiers,
167 NoReachableFrontier,
168 NoProgress,
169 IterationLimit,
170}
171
172#[derive(Debug, Clone, PartialEq)]
174pub struct FrontierNavStep {
175 pub robot: (i32, i32),
176 pub path_index: usize,
179 pub known: Vec<Vec<Knowledge>>,
180 pub candidates: Vec<FrontierCandidate>,
181 pub selected: Option<usize>,
182 pub phase: FrontierNavPhase,
183}
184
185struct Trace {
186 enabled: bool,
187 steps: Vec<FrontierNavStep>,
188}
189impl Trace {
190 fn capture(
191 &mut self,
192 phase: FrontierNavPhase,
193 at: ((i32, i32), usize),
194 known: &[Vec<Knowledge>],
195 candidates: &[FrontierCandidate],
196 selected: Option<usize>,
197 ) {
198 if self.enabled {
199 self.steps.push(FrontierNavStep {
200 robot: at.0,
201 path_index: at.1,
202 known: known.to_vec(),
203 candidates: candidates.to_vec(),
204 selected,
205 phase,
206 });
207 }
208 }
209}
210
211pub fn simulate_frontier_navigation(
213 world: &FrontierNavWorld,
214 config: &FrontierNavConfig,
215) -> RoboticsResult<FrontierNavReport> {
216 simulate_observed(world, config, false).map(|(report, _)| report)
217}
218
219pub fn simulate_frontier_navigation_with_trace(
222 world: &FrontierNavWorld,
223 config: &FrontierNavConfig,
224) -> RoboticsResult<(FrontierNavReport, Vec<FrontierNavStep>)> {
225 simulate_observed(world, config, true)
226}
227
228fn simulate_observed(
229 world: &FrontierNavWorld,
230 config: &FrontierNavConfig,
231 record: bool,
232) -> RoboticsResult<(FrontierNavReport, Vec<FrontierNavStep>)> {
233 validate(world, config)?;
234 let mut trace = Trace {
235 enabled: record,
236 steps: Vec::new(),
237 };
238 let mut outcome = FrontierNavPhase::IterationLimit;
239
240 let (w, h) = (world.width, world.height);
241 let mut known = vec![vec![Knowledge::Unknown; h as usize]; w as usize];
242 let mut robot = world.start;
243 let mut path = vec![robot];
244 let mut path_length = 0.0;
245 let mut selected_frontiers: Vec<FrontierChoice> = Vec::new();
246 let mut reached_goal = false;
247 let mut iterations = 0;
248
249 sense(world, &mut known, robot, config.sensor_range);
250
251 for _ in 0..config.max_iterations {
252 iterations += 1;
253
254 if known[world.goal.0 as usize][world.goal.1 as usize] == Knowledge::Free {
256 if let Some(dist) = dijkstra_from(&known, world.goal) {
257 if dist[robot.0 as usize][robot.1 as usize].is_finite() {
258 trace.capture(
259 FrontierNavPhase::Goal,
260 (robot, path.len() - 1),
261 &known,
262 &[],
263 None,
264 );
265 follow_gradient(
266 &known,
267 &dist,
268 &mut robot,
269 world.goal,
270 usize::MAX,
271 &mut path,
272 &mut path_length,
273 );
274 if robot == world.goal {
275 reached_goal = true;
276 outcome = FrontierNavPhase::Arrived;
277 break;
278 }
279 }
280 }
281 }
282
283 let frontiers = compute_frontiers(&known);
284 if frontiers.is_empty() {
285 outcome = FrontierNavPhase::NoFrontiers;
286 break;
287 }
288
289 let reach = match dijkstra_from(&known, robot) {
291 Some(field) => field,
292 None => {
293 outcome = FrontierNavPhase::NoReachableFrontier;
294 break;
295 }
296 };
297
298 let goal_distance = euclid(robot, world.goal);
299 let max_openness = frontiers
300 .iter()
301 .map(|f| f.openness)
302 .max()
303 .unwrap_or(1)
304 .max(1) as f64;
305
306 let mut best: Option<(f64, usize)> = None;
307 let mut candidates = Vec::new();
308 for (index, frontier) in frontiers.iter().enumerate() {
309 let reach_cost = reach[frontier.rep.0 as usize][frontier.rep.1 as usize];
310 let los = line_of_sight(&known, robot, frontier.rep);
311 let goal_gain = goal_distance - euclid(frontier.rep, world.goal);
312 let openness = frontier.openness as f64 / max_openness;
313 let score = config.goal_weight * goal_gain - config.cost_weight * reach_cost
314 + config.line_of_sight_weight * if los { 1.0 } else { 0.0 }
315 + config.openness_weight * openness;
316 if record {
317 candidates.push(FrontierCandidate {
318 representative: frontier.rep,
319 members: frontier.members.clone(),
320 openness: frontier.openness,
321 normalized_openness: openness,
322 goal_gain,
323 travel_cost: reach_cost.is_finite().then_some(reach_cost),
324 line_of_sight: los,
325 score: reach_cost.is_finite().then_some(score),
326 });
327 }
328 if !reach_cost.is_finite() {
329 continue;
330 }
331 let better = match best {
332 None => true,
333 Some((best_score, best_index)) => {
334 score > best_score
335 || (score == best_score && frontier.rep < frontiers[best_index].rep)
336 }
337 };
338 if better {
339 best = Some((score, index));
340 }
341 }
342
343 let Some((score, index)) = best else {
344 outcome = FrontierNavPhase::NoReachableFrontier;
345 break;
346 };
347 let frontier = &frontiers[index];
348 trace.capture(
349 FrontierNavPhase::Frontier,
350 (robot, path.len() - 1),
351 &known,
352 &candidates,
353 Some(index),
354 );
355 let target = frontier.rep;
356 selected_frontiers.push(FrontierChoice {
357 robot,
358 frontier: target,
359 score,
360 line_of_sight: line_of_sight(&known, robot, target),
361 });
362
363 let field = match dijkstra_from(&known, target) {
365 Some(field) => field,
366 None => {
367 outcome = FrontierNavPhase::NoReachableFrontier;
368 break;
369 }
370 };
371 let before = robot;
372 follow_gradient(
373 &known,
374 &field,
375 &mut robot,
376 target,
377 config.step_budget,
378 &mut path,
379 &mut path_length,
380 );
381 if robot == before {
382 outcome = FrontierNavPhase::NoProgress;
383 break; }
385 sense(world, &mut known, robot, config.sensor_range);
386 }
387
388 let known_free_cells = known
389 .iter()
390 .flatten()
391 .filter(|c| **c == Knowledge::Free)
392 .count();
393
394 trace.capture(outcome, (robot, path.len() - 1), &known, &[], None);
395 Ok((
396 FrontierNavReport {
397 path,
398 reached_goal,
399 path_length,
400 frontier_selections: selected_frontiers.len(),
401 iterations,
402 known_free_cells,
403 selected_frontiers,
404 known,
405 },
406 trace.steps,
407 ))
408}
409
410fn validate(world: &FrontierNavWorld, config: &FrontierNavConfig) -> RoboticsResult<()> {
411 if !world.in_bounds(world.start.0, world.start.1)
412 || world.is_occupied(world.start.0, world.start.1)
413 {
414 return Err(RoboticsError::InvalidParameter(
415 "frontier-nav start must be a free in-bounds cell".to_string(),
416 ));
417 }
418 if !world.in_bounds(world.goal.0, world.goal.1) || world.is_occupied(world.goal.0, world.goal.1)
419 {
420 return Err(RoboticsError::InvalidParameter(
421 "frontier-nav goal must be a free in-bounds cell".to_string(),
422 ));
423 }
424 if !config.sensor_range.is_finite() || config.sensor_range <= 0.0 {
425 return Err(RoboticsError::InvalidParameter(
426 "frontier-nav sensor range must be finite and positive".to_string(),
427 ));
428 }
429 if config.step_budget == 0 || config.max_iterations == 0 {
430 return Err(RoboticsError::InvalidParameter(
431 "frontier-nav step budget and iterations must be positive".to_string(),
432 ));
433 }
434 Ok(())
435}
436
437#[derive(Debug, Clone, PartialEq)]
439struct FrontierCluster {
440 rep: (i32, i32),
441 openness: usize,
442 members: Vec<(i32, i32)>,
443}
444
445fn sense(world: &FrontierNavWorld, known: &mut [Vec<Knowledge>], from: (i32, i32), range: f64) {
447 let r = range.ceil() as i32;
448 for dx in -r..=r {
449 for dy in -r..=r {
450 let x = from.0 + dx;
451 let y = from.1 + dy;
452 if !world.in_bounds(x, y) {
453 continue;
454 }
455 if ((dx * dx + dy * dy) as f64).sqrt() > range {
456 continue;
457 }
458 if visible(world, from, (x, y)) {
459 known[x as usize][y as usize] = if world.is_occupied(x, y) {
460 Knowledge::Occupied
461 } else {
462 Knowledge::Free
463 };
464 }
465 }
466 }
467}
468
469fn visible(world: &FrontierNavWorld, from: (i32, i32), target: (i32, i32)) -> bool {
471 let steps = (((target.0 - from.0).abs()).max((target.1 - from.1).abs()) * 3).max(1);
472 for i in 1..=steps {
473 let t = i as f64 / steps as f64;
474 let x = (from.0 as f64 + t * (target.0 - from.0) as f64).round() as i32;
475 let y = (from.1 as f64 + t * (target.1 - from.1) as f64).round() as i32;
476 if (x, y) == target {
477 return true;
478 }
479 if world.is_occupied(x, y) {
480 return false; }
482 }
483 true
484}
485
486fn line_of_sight(known: &[Vec<Knowledge>], from: (i32, i32), target: (i32, i32)) -> bool {
488 let steps = (((target.0 - from.0).abs()).max((target.1 - from.1).abs()) * 3).max(1);
489 for i in 1..=steps {
490 let t = i as f64 / steps as f64;
491 let x = (from.0 as f64 + t * (target.0 - from.0) as f64).round() as i32;
492 let y = (from.1 as f64 + t * (target.1 - from.1) as f64).round() as i32;
493 if (x, y) == target {
494 return true;
495 }
496 if known[x as usize][y as usize] == Knowledge::Occupied {
497 return false;
498 }
499 }
500 true
501}
502
503fn compute_frontiers(known: &[Vec<Knowledge>]) -> Vec<FrontierCluster> {
505 let w = known.len() as i32;
506 let h = known[0].len() as i32;
507 let is_free = |x: i32, y: i32| known[x as usize][y as usize] == Knowledge::Free;
508 let unknown_neighbors = |x: i32, y: i32| {
509 let mut count = 0;
510 for (dx, dy) in [(1, 0), (-1, 0), (0, 1), (0, -1)] {
511 let nx = x + dx;
512 let ny = y + dy;
513 if nx >= 0
514 && ny >= 0
515 && nx < w
516 && ny < h
517 && known[nx as usize][ny as usize] == Knowledge::Unknown
518 {
519 count += 1;
520 }
521 }
522 count
523 };
524
525 let mut is_frontier = vec![vec![false; h as usize]; w as usize];
526 for x in 0..w {
527 for y in 0..h {
528 if is_free(x, y) && unknown_neighbors(x, y) > 0 {
529 is_frontier[x as usize][y as usize] = true;
530 }
531 }
532 }
533
534 let mut visited = vec![vec![false; h as usize]; w as usize];
536 let mut clusters = Vec::new();
537 for x in 0..w {
538 for y in 0..h {
539 if !is_frontier[x as usize][y as usize] || visited[x as usize][y as usize] {
540 continue;
541 }
542 let mut queue = VecDeque::new();
543 queue.push_back((x, y));
544 visited[x as usize][y as usize] = true;
545 let mut members = Vec::new();
546 let mut openness = 0;
547 while let Some((cx, cy)) = queue.pop_front() {
548 members.push((cx, cy));
549 openness += unknown_neighbors(cx, cy);
550 for dx in -1..=1 {
551 for dy in -1..=1 {
552 if dx == 0 && dy == 0 {
553 continue;
554 }
555 let nx = cx + dx;
556 let ny = cy + dy;
557 if nx >= 0
558 && ny >= 0
559 && nx < w
560 && ny < h
561 && is_frontier[nx as usize][ny as usize]
562 && !visited[nx as usize][ny as usize]
563 {
564 visited[nx as usize][ny as usize] = true;
565 queue.push_back((nx, ny));
566 }
567 }
568 }
569 }
570 let cx = members.iter().map(|m| m.0).sum::<i32>() as f64 / members.len() as f64;
572 let cy = members.iter().map(|m| m.1).sum::<i32>() as f64 / members.len() as f64;
573 let rep = *members
574 .iter()
575 .min_by(|a, b| {
576 let da = (a.0 as f64 - cx).powi(2) + (a.1 as f64 - cy).powi(2);
577 let db = (b.0 as f64 - cx).powi(2) + (b.1 as f64 - cy).powi(2);
578 da.partial_cmp(&db)
579 .unwrap_or(Ordering::Equal)
580 .then_with(|| a.cmp(b))
581 })
582 .unwrap();
583 clusters.push(FrontierCluster {
584 rep,
585 openness,
586 members,
587 });
588 }
589 }
590 clusters
591}
592
593fn dijkstra_from(known: &[Vec<Knowledge>], source: (i32, i32)) -> Option<Vec<Vec<f64>>> {
595 let w = known.len() as i32;
596 let h = known[0].len() as i32;
597 if known[source.0 as usize][source.1 as usize] != Knowledge::Free {
598 return None;
599 }
600 let mut dist = vec![vec![f64::INFINITY; h as usize]; w as usize];
601 dist[source.0 as usize][source.1 as usize] = 0.0;
602 let mut heap = BinaryHeap::new();
603 heap.push(DijkstraNode {
604 cost: 0.0,
605 cell: source,
606 });
607 while let Some(DijkstraNode { cost, cell }) = heap.pop() {
608 if cost > dist[cell.0 as usize][cell.1 as usize] {
609 continue;
610 }
611 for dx in -1..=1 {
612 for dy in -1..=1 {
613 if dx == 0 && dy == 0 {
614 continue;
615 }
616 let nx = cell.0 + dx;
617 let ny = cell.1 + dy;
618 if nx < 0 || ny < 0 || nx >= w || ny >= h {
619 continue;
620 }
621 if known[nx as usize][ny as usize] != Knowledge::Free {
622 continue;
623 }
624 let step = if dx != 0 && dy != 0 { SQRT2 } else { 1.0 };
625 let next = cost + step;
626 if next < dist[nx as usize][ny as usize] {
627 dist[nx as usize][ny as usize] = next;
628 heap.push(DijkstraNode {
629 cost: next,
630 cell: (nx, ny),
631 });
632 }
633 }
634 }
635 }
636 Some(dist)
637}
638
639fn follow_gradient(
641 known: &[Vec<Knowledge>],
642 field: &[Vec<f64>],
643 robot: &mut (i32, i32),
644 target: (i32, i32),
645 budget: usize,
646 path: &mut Vec<(i32, i32)>,
647 path_length: &mut f64,
648) {
649 let w = known.len() as i32;
650 let h = known[0].len() as i32;
651 for _ in 0..budget {
652 if *robot == target {
653 break;
654 }
655 let here = field[robot.0 as usize][robot.1 as usize];
656 let mut best: Option<(f64, (i32, i32), f64)> = None;
657 for dx in -1..=1 {
658 for dy in -1..=1 {
659 if dx == 0 && dy == 0 {
660 continue;
661 }
662 let nx = robot.0 + dx;
663 let ny = robot.1 + dy;
664 if nx < 0 || ny < 0 || nx >= w || ny >= h {
665 continue;
666 }
667 if known[nx as usize][ny as usize] != Knowledge::Free {
668 continue;
669 }
670 let d = field[nx as usize][ny as usize];
671 if !d.is_finite() || d >= here {
672 continue;
673 }
674 let step = if dx != 0 && dy != 0 { SQRT2 } else { 1.0 };
675 let better = match best {
676 None => true,
677 Some((best_d, best_cell, _)) => {
678 d < best_d || (d == best_d && (nx, ny) < best_cell)
679 }
680 };
681 if better {
682 best = Some((d, (nx, ny), step));
683 }
684 }
685 }
686 let Some((_, next, step)) = best else { break };
687 *robot = next;
688 path.push(next);
689 *path_length += step;
690 }
691}
692
693#[derive(Debug, Clone, Copy)]
694struct DijkstraNode {
695 cost: f64,
696 cell: (i32, i32),
697}
698
699impl PartialEq for DijkstraNode {
700 fn eq(&self, other: &Self) -> bool {
701 self.cost == other.cost
702 }
703}
704impl Eq for DijkstraNode {}
705impl PartialOrd for DijkstraNode {
706 fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
707 Some(self.cmp(other))
708 }
709}
710impl Ord for DijkstraNode {
711 fn cmp(&self, other: &Self) -> Ordering {
712 other
714 .cost
715 .partial_cmp(&self.cost)
716 .unwrap_or(Ordering::Equal)
717 .then_with(|| other.cell.cmp(&self.cell))
718 }
719}
720
721fn euclid(a: (i32, i32), b: (i32, i32)) -> f64 {
722 let dx = (a.0 - b.0) as f64;
723 let dy = (a.1 - b.1) as f64;
724 (dx * dx + dy * dy).sqrt()
725}
726
727#[cfg(test)]
728mod tests {
729 use super::*;
730 use std::collections::HashSet;
731
732 fn walled_world() -> FrontierNavWorld {
734 let mut world = FrontierNavWorld::new(24, 16, (1, 8), (22, 8)).unwrap();
735 world.fill_rect(12, 13, 0, 12);
737 world
738 }
739
740 #[test]
741 fn trace_preserves_rollout_and_records_only_actual_sensing_boundaries() {
742 let world = walled_world();
743 for range in [3.0, 5.0, 7.0] {
744 let config = FrontierNavConfig {
745 sensor_range: range,
746 ..FrontierNavConfig::default()
747 };
748 let legacy = simulate_frontier_navigation(&world, &config).unwrap();
749 let (report, steps) = simulate_frontier_navigation_with_trace(&world, &config).unwrap();
750 assert_eq!(legacy, report);
751 assert!(!steps.is_empty());
752 assert_eq!(steps[0].path_index, 0);
753 assert_eq!(steps.last().unwrap().robot, *report.path.last().unwrap());
754 assert_eq!(steps.last().unwrap().known, report.known);
755 let mut observed = vec![vec![Knowledge::Unknown; 16]; 24];
756 sense(&world, &mut observed, world.start, range);
757 assert_eq!(steps[0].known, observed);
758 let mut length = 0.;
759 for pair in steps.windows(2) {
760 let (a, b) = (&pair[0], &pair[1]);
761 assert!(a.path_index <= b.path_index);
762 assert_eq!(report.path[a.path_index], a.robot);
763 for segment in report.path[a.path_index..=b.path_index].windows(2) {
764 let (p, q) = (segment[0], segment[1]);
765 assert_eq!(a.known[q.0 as usize][q.1 as usize], Knowledge::Free);
766 assert!((p.0 - q.0).abs() <= 1 && (p.1 - q.1).abs() <= 1 && p != q);
767 length += euclid(p, q);
768 }
769 if a.phase == FrontierNavPhase::Frontier && a.path_index != b.path_index {
770 sense(&world, &mut observed, b.robot, range);
771 }
772 assert_eq!(b.known, observed);
773 }
774 assert_eq!(length, report.path_length);
775 assert_eq!(
776 steps
777 .iter()
778 .filter(|s| s.phase == FrontierNavPhase::Frontier)
779 .count(),
780 report.frontier_selections
781 );
782 }
783 }
784
785 #[test]
786 fn trace_exposes_candidate_partition_scores_ties_and_terminal_reasons() {
787 let world = walled_world();
788 let config = FrontierNavConfig::default();
789 let (_, steps) = simulate_frontier_navigation_with_trace(&world, &config).unwrap();
790 for s in steps
791 .iter()
792 .filter(|s| s.phase == FrontierNavPhase::Frontier)
793 {
794 let mut actual = HashSet::new();
795 let mut expected = HashSet::new();
796 for x in 0..world.width {
797 for y in 0..world.height {
798 if s.known[x as usize][y as usize] == Knowledge::Free
799 && [(1, 0), (-1, 0), (0, 1), (0, -1)].iter().any(|(dx, dy)| {
800 let (a, b) = (x + dx, y + dy);
801 world.in_bounds(a, b)
802 && s.known[a as usize][b as usize] == Knowledge::Unknown
803 })
804 {
805 expected.insert((x, y));
806 }
807 }
808 }
809 let maximum = s
810 .candidates
811 .iter()
812 .map(|c| c.openness)
813 .max()
814 .unwrap()
815 .max(1) as f64;
816 for c in &s.candidates {
817 assert!(c.members.contains(&c.representative));
818 let mut openness = 0;
819 for &(x, y) in &c.members {
820 assert!(actual.insert((x, y)));
821 for (dx, dy) in [(1, 0), (-1, 0), (0, 1), (0, -1)] {
822 let (a, b) = (x + dx, y + dy);
823 if world.in_bounds(a, b)
824 && s.known[a as usize][b as usize] == Knowledge::Unknown
825 {
826 openness += 1;
827 }
828 }
829 }
830 assert_eq!(openness, c.openness);
831 assert_eq!(c.normalized_openness, openness as f64 / maximum);
832 assert_eq!(
833 c.goal_gain,
834 euclid(s.robot, world.goal) - euclid(c.representative, world.goal)
835 );
836 assert_eq!(
837 c.score,
838 c.travel_cost.map(|cost| config.goal_weight * c.goal_gain
839 - config.cost_weight * cost
840 + config.line_of_sight_weight * if c.line_of_sight { 1. } else { 0. }
841 + config.openness_weight * c.normalized_openness)
842 );
843 }
844 assert_eq!(actual, expected);
845 let chosen = &s.candidates[s.selected.unwrap()];
846 for c in &s.candidates {
847 if let Some(score) = c.score {
848 assert!(chosen.score.unwrap() >= score);
849 if chosen.score == c.score {
850 assert!(chosen.representative <= c.representative);
851 }
852 }
853 }
854 }
855 let limit = FrontierNavConfig {
856 max_iterations: 1,
857 ..config.clone()
858 };
859 let (_, trace) = simulate_frontier_navigation_with_trace(&world, &limit).unwrap();
860 assert_eq!(
861 trace.last().unwrap().phase,
862 FrontierNavPhase::IterationLimit
863 );
864 let mut sealed = FrontierNavWorld::new(8, 1, (1, 0), (6, 0)).unwrap();
867 sealed.fill_rect(3, 3, 0, 0);
868 let (_, trace) = simulate_frontier_navigation_with_trace(
869 &sealed,
870 &FrontierNavConfig {
871 sensor_range: 100.,
872 ..config
873 },
874 )
875 .unwrap();
876 assert_eq!(trace.last().unwrap().phase, FrontierNavPhase::NoFrontiers);
877 assert_eq!(trace.last().unwrap().path_index, 0);
878 }
879
880 #[test]
881 fn reaches_goal_around_occlusion() {
882 let report =
883 simulate_frontier_navigation(&walled_world(), &FrontierNavConfig::default()).unwrap();
884 assert!(report.reached_goal, "did not reach goal");
885 assert!(*report.path.last().unwrap() == (22, 8));
886 assert!(report.frontier_selections >= 1);
888 }
889
890 #[test]
891 fn open_world_reaches_goal() {
892 let world = FrontierNavWorld::new(20, 12, (1, 6), (18, 6)).unwrap();
893 let report = simulate_frontier_navigation(&world, &FrontierNavConfig::default()).unwrap();
894 assert!(report.reached_goal);
895 }
896
897 #[test]
898 fn is_deterministic() {
899 let world = walled_world();
900 let config = FrontierNavConfig::default();
901 let first = simulate_frontier_navigation(&world, &config).unwrap();
902 let second = simulate_frontier_navigation(&world, &config).unwrap();
903 assert_eq!(first.path, second.path);
904 assert_eq!(first.frontier_selections, second.frontier_selections);
905 }
906
907 #[test]
908 fn sensing_is_occlusion_aware() {
909 let mut world = FrontierNavWorld::new(24, 16, (1, 8), (22, 8)).unwrap();
911 world.fill_rect(4, 4, 5, 11);
912 let mut known = vec![vec![Knowledge::Unknown; 16]; 24];
913 sense(&world, &mut known, (1, 8), 6.0);
914 assert_eq!(known[3][8], Knowledge::Free);
916 assert_eq!(known[4][8], Knowledge::Occupied);
918 assert_eq!(known[6][8], Knowledge::Unknown);
920 }
921
922 #[test]
923 fn rejects_blocked_start() {
924 let mut world = FrontierNavWorld::new(10, 10, (1, 1), (8, 8)).unwrap();
925 world.fill_rect(1, 1, 1, 1);
926 assert!(simulate_frontier_navigation(&world, &FrontierNavConfig::default()).is_err());
927 }
928}