429 lines
15 KiB
Rust
429 lines
15 KiB
Rust
//! Flight / coverage-optimization patterns for swarm area search.
|
|
//!
|
|
//! Different strategies trade off coverage completeness, time, and robustness:
|
|
//! - Boustrophedon: systematic lawnmower; complete but drones overlap if unpartitioned
|
|
//! - PartitionedLawnmower: area split into per-drone strips → no overlap, ~Nx faster coverage
|
|
//! - Spiral: outward spiral from a seed; good for centred search (last-known-position SAR)
|
|
//! - Pheromone: stigmergic — steer away from recently-visited cells; robust to dropout
|
|
//! - PotentialField: repelled by visited cells + peers, attracted to unscanned frontier
|
|
//! - LevyFlight: heavy-tailed random walk; good exploration when target location unknown
|
|
|
|
use crate::types::{NodeId, Position3D};
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
|
|
pub enum FlightPattern {
|
|
Boustrophedon,
|
|
#[default]
|
|
PartitionedLawnmower,
|
|
Spiral,
|
|
Pheromone,
|
|
PotentialField,
|
|
LevyFlight,
|
|
}
|
|
|
|
impl FlightPattern {
|
|
// Intentional inherent infallible parser (returns Self, not Result); shipped API.
|
|
#[allow(clippy::should_implement_trait)]
|
|
pub fn from_str(s: &str) -> Self {
|
|
match s.to_lowercase().as_str() {
|
|
"boustrophedon" | "lawnmower" => FlightPattern::Boustrophedon,
|
|
"partitioned" | "partitioned_lawnmower" => FlightPattern::PartitionedLawnmower,
|
|
"spiral" => FlightPattern::Spiral,
|
|
"pheromone" | "stigmergic" => FlightPattern::Pheromone,
|
|
"potential" | "potential_field" => FlightPattern::PotentialField,
|
|
"levy" | "levyflight" | "levy_flight" => FlightPattern::LevyFlight,
|
|
_ => FlightPattern::default(),
|
|
}
|
|
}
|
|
|
|
pub fn name(&self) -> &'static str {
|
|
match self {
|
|
FlightPattern::Boustrophedon => "boustrophedon",
|
|
FlightPattern::PartitionedLawnmower => "partitioned_lawnmower",
|
|
FlightPattern::Spiral => "spiral",
|
|
FlightPattern::Pheromone => "pheromone",
|
|
FlightPattern::PotentialField => "potential_field",
|
|
FlightPattern::LevyFlight => "levy_flight",
|
|
}
|
|
}
|
|
|
|
/// All pattern variants, for enumeration / UI selection.
|
|
pub fn all() -> [FlightPattern; 6] {
|
|
[
|
|
FlightPattern::Boustrophedon,
|
|
FlightPattern::PartitionedLawnmower,
|
|
FlightPattern::Spiral,
|
|
FlightPattern::Pheromone,
|
|
FlightPattern::PotentialField,
|
|
FlightPattern::LevyFlight,
|
|
]
|
|
}
|
|
}
|
|
|
|
/// Inputs for computing the next waypoint under a pattern.
|
|
pub struct PatternContext<'a> {
|
|
pub drone_id: NodeId,
|
|
pub swarm_size: usize,
|
|
pub current: Position3D,
|
|
pub area_w: f64,
|
|
pub area_h: f64,
|
|
pub altitude_z: f64, // flight z (negative NED)
|
|
pub scan_width_m: f64, // strip spacing
|
|
pub step: u64, // tick counter (for deterministic pseudo-random patterns)
|
|
pub visited: &'a [Position3D], // recently visited cell centres (for pheromone/potential)
|
|
pub peers: &'a [Position3D], // peer positions (for potential-field repulsion)
|
|
}
|
|
|
|
impl FlightPattern {
|
|
/// Compute the next target position for a drone under this pattern.
|
|
pub fn next_target(&self, ctx: &PatternContext) -> Position3D {
|
|
match self {
|
|
FlightPattern::Boustrophedon => boustrophedon(ctx),
|
|
FlightPattern::PartitionedLawnmower => partitioned_lawnmower(ctx),
|
|
FlightPattern::Spiral => spiral(ctx),
|
|
FlightPattern::Pheromone => pheromone(ctx),
|
|
FlightPattern::PotentialField => potential_field(ctx),
|
|
FlightPattern::LevyFlight => levy_flight(ctx),
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Clamp a candidate (x, y) to the area bounds and lift it to the flight altitude.
|
|
fn clamp_to_area(x: f64, y: f64, ctx: &PatternContext) -> Position3D {
|
|
Position3D {
|
|
x: x.clamp(0.0, ctx.area_w),
|
|
y: y.clamp(0.0, ctx.area_h),
|
|
z: ctx.altitude_z,
|
|
}
|
|
}
|
|
|
|
/// Serpentine waypoint within a rectangular sub-region.
|
|
///
|
|
/// Walks rows of height `scan_width_m`; on each row sweeps left→right or
|
|
/// right→left depending on the row parity, advancing one `scan_width_m`
|
|
/// segment per `step`.
|
|
fn serpentine_in_region(
|
|
x0: f64,
|
|
x1: f64,
|
|
y0: f64,
|
|
y1: f64,
|
|
scan_width_m: f64,
|
|
step: u64,
|
|
) -> (f64, f64) {
|
|
let strip_w = (x1 - x0).max(scan_width_m);
|
|
let height = (y1 - y0).max(scan_width_m);
|
|
|
|
// Number of horizontal segments per row before stepping to the next row.
|
|
let cols = ((strip_w / scan_width_m).ceil() as u64).max(1);
|
|
// Number of rows in this region.
|
|
let rows = ((height / scan_width_m).ceil() as u64).max(1);
|
|
let total = cols * rows;
|
|
let s = step % total;
|
|
|
|
let row = s / cols;
|
|
let col = s % cols;
|
|
|
|
// Centre of the current row band.
|
|
let y = y0 + (row as f64 + 0.5) * scan_width_m;
|
|
let y = y.min(y1);
|
|
|
|
// Serpentine: even rows L→R, odd rows R→L.
|
|
let along = if row % 2 == 0 { col } else { cols - 1 - col };
|
|
let x = x0 + (along as f64 + 0.5) * scan_width_m;
|
|
let x = x.min(x1);
|
|
|
|
(x, y)
|
|
}
|
|
|
|
/// Classic full-area serpentine lawnmower (drones may overlap — baseline).
|
|
fn boustrophedon(ctx: &PatternContext) -> Position3D {
|
|
let (x, y) = serpentine_in_region(
|
|
0.0,
|
|
ctx.area_w,
|
|
0.0,
|
|
ctx.area_h,
|
|
ctx.scan_width_m,
|
|
ctx.step,
|
|
);
|
|
clamp_to_area(x, y, ctx)
|
|
}
|
|
|
|
/// Partitioned lawnmower: split `area_w` into `swarm_size` vertical strips;
|
|
/// drone `i` lawnmowers ONLY within strip `[i*w/n, (i+1)*w/n]`.
|
|
///
|
|
/// This is the clustering fix: each drone covers a disjoint band, so total
|
|
/// coverage scales ~linearly with swarm size instead of all drones tracing
|
|
/// the same path.
|
|
fn partitioned_lawnmower(ctx: &PatternContext) -> Position3D {
|
|
let n = ctx.swarm_size.max(1);
|
|
let i = (ctx.drone_id.0 as usize) % n;
|
|
let strip_w = ctx.area_w / n as f64;
|
|
let x0 = i as f64 * strip_w;
|
|
let x1 = x0 + strip_w;
|
|
|
|
let (x, y) =
|
|
serpentine_in_region(x0, x1, 0.0, ctx.area_h, ctx.scan_width_m, ctx.step);
|
|
clamp_to_area(x, y, ctx)
|
|
}
|
|
|
|
/// Outward Archimedean spiral from the area centre; radius grows with step.
|
|
fn spiral(ctx: &PatternContext) -> Position3D {
|
|
let cx = ctx.area_w / 2.0;
|
|
let cy = ctx.area_h / 2.0;
|
|
|
|
// Angular step keeps successive waypoints roughly `scan_width_m` apart.
|
|
let theta = ctx.step as f64 * 0.6;
|
|
// Archimedean spiral r = b * theta; b chosen so each turn adds scan_width_m.
|
|
let b = ctx.scan_width_m / (2.0 * std::f64::consts::PI);
|
|
let r = b * theta;
|
|
|
|
let x = cx + r * theta.cos();
|
|
let y = cy + r * theta.sin();
|
|
clamp_to_area(x, y, ctx)
|
|
}
|
|
|
|
/// Stigmergic: sample candidate headings, step toward the least-visited one.
|
|
fn pheromone(ctx: &PatternContext) -> Position3D {
|
|
let step_len = ctx.scan_width_m.max(1.0);
|
|
// Deterministic base heading offset per drone so they diverge.
|
|
let base = ctx.drone_id.0 as f64 * (std::f64::consts::PI / 3.0);
|
|
|
|
let n_candidates = 8;
|
|
let mut best: Option<(f64, f64, f64)> = None; // (score, x, y); lower score = less visited
|
|
for k in 0..n_candidates {
|
|
let theta = base + (k as f64) * (2.0 * std::f64::consts::PI / n_candidates as f64);
|
|
let cx = ctx.current.x + step_len * theta.cos();
|
|
let cy = ctx.current.y + step_len * theta.sin();
|
|
let cx = cx.clamp(0.0, ctx.area_w);
|
|
let cy = cy.clamp(0.0, ctx.area_h);
|
|
|
|
// Penalty = sum of inverse-distance to recently-visited cell centres.
|
|
let mut visit_pressure = 0.0;
|
|
for v in ctx.visited {
|
|
let d = (cx - v.x).hypot(cy - v.y);
|
|
visit_pressure += 1.0 / (1.0 + d);
|
|
}
|
|
if best.as_ref().is_none_or(|(bs, _, _)| visit_pressure < *bs) {
|
|
best = Some((visit_pressure, cx, cy));
|
|
}
|
|
}
|
|
|
|
let (_, x, y) = best.unwrap_or((0.0, ctx.current.x, ctx.current.y));
|
|
clamp_to_area(x, y, ctx)
|
|
}
|
|
|
|
/// Potential field: repelled by visited cells + peers, attracted to the
|
|
/// nearest unscanned frontier; step in the resultant direction.
|
|
fn potential_field(ctx: &PatternContext) -> Position3D {
|
|
let mut fx = 0.0;
|
|
let mut fy = 0.0;
|
|
|
|
// Repulsion from recently-visited cells.
|
|
for v in ctx.visited {
|
|
let dx = ctx.current.x - v.x;
|
|
let dy = ctx.current.y - v.y;
|
|
let d2 = dx * dx + dy * dy + 1.0;
|
|
let mag = 1.0 / d2;
|
|
fx += dx / d2.sqrt() * mag;
|
|
fy += dy / d2.sqrt() * mag;
|
|
}
|
|
|
|
// Repulsion from peers (collision / overlap avoidance).
|
|
for p in ctx.peers {
|
|
let dx = ctx.current.x - p.x;
|
|
let dy = ctx.current.y - p.y;
|
|
let d2 = dx * dx + dy * dy + 1.0;
|
|
let mag = 2.0 / d2; // peers repel more strongly than stale trail
|
|
fx += dx / d2.sqrt() * mag;
|
|
fy += dy / d2.sqrt() * mag;
|
|
}
|
|
|
|
// Attraction toward the nearest unscanned frontier point. Sample a grid of
|
|
// candidate area points; pick the one with greatest distance to any visited
|
|
// cell (i.e. the least-explored region) and pull toward it.
|
|
let mut frontier: Option<(f64, f64, f64)> = None; // (openness, x, y)
|
|
let samples = 5;
|
|
for ix in 0..=samples {
|
|
for iy in 0..=samples {
|
|
let px = ctx.area_w * ix as f64 / samples as f64;
|
|
let py = ctx.area_h * iy as f64 / samples as f64;
|
|
let mut nearest = f64::INFINITY;
|
|
for v in ctx.visited {
|
|
let d = (px - v.x).hypot(py - v.y);
|
|
if d < nearest {
|
|
nearest = d;
|
|
}
|
|
}
|
|
if !nearest.is_finite() {
|
|
nearest = (px - ctx.current.x).hypot(py - ctx.current.y);
|
|
}
|
|
if frontier.as_ref().is_none_or(|(o, _, _)| nearest > *o) {
|
|
frontier = Some((nearest, px, py));
|
|
}
|
|
}
|
|
}
|
|
if let Some((_, gx, gy)) = frontier {
|
|
let dx = gx - ctx.current.x;
|
|
let dy = gy - ctx.current.y;
|
|
let d = (dx * dx + dy * dy).sqrt().max(1e-6);
|
|
fx += dx / d * 1.5; // attraction gain
|
|
fy += dy / d * 1.5;
|
|
}
|
|
|
|
let fmag = (fx * fx + fy * fy).sqrt();
|
|
let step_len = ctx.scan_width_m.max(1.0);
|
|
let (x, y) = if fmag > 1e-9 {
|
|
(
|
|
ctx.current.x + fx / fmag * step_len,
|
|
ctx.current.y + fy / fmag * step_len,
|
|
)
|
|
} else {
|
|
(ctx.current.x, ctx.current.y)
|
|
};
|
|
clamp_to_area(x, y, ctx)
|
|
}
|
|
|
|
/// Deterministic pseudo-random heavy-tailed step (Lévy flight). Most steps are
|
|
/// short; occasional long jumps. Seeded from drone_id + step via an LCG so the
|
|
/// trajectory is reproducible.
|
|
fn levy_flight(ctx: &PatternContext) -> Position3D {
|
|
// Linear congruential generator (Numerical Recipes constants).
|
|
let seed = (ctx.drone_id.0 as u64)
|
|
.wrapping_mul(0x9E37_79B9_7F4A_7C15)
|
|
.wrapping_add(ctx.step.wrapping_mul(0x2545_F491_4F6C_DD1D));
|
|
let r1 = lcg(seed);
|
|
let r2 = lcg(r1);
|
|
|
|
let u_angle = (r1 >> 11) as f64 / (1u64 << 53) as f64; // [0,1)
|
|
let u_len = ((r2 >> 11) as f64 / (1u64 << 53) as f64).max(1e-6); // (0,1]
|
|
|
|
let theta = u_angle * 2.0 * std::f64::consts::PI;
|
|
// Heavy-tailed step length: inverse power-law (Pareto-like), exponent ~1.5.
|
|
let step_len = ctx.scan_width_m.max(1.0) * u_len.powf(-1.0 / 1.5);
|
|
// Cap to the area diagonal so a single jump can't shoot arbitrarily far.
|
|
let max_jump = (ctx.area_w * ctx.area_w + ctx.area_h * ctx.area_h).sqrt();
|
|
let step_len = step_len.min(max_jump);
|
|
|
|
let x = ctx.current.x + step_len * theta.cos();
|
|
let y = ctx.current.y + step_len * theta.sin();
|
|
clamp_to_area(x, y, ctx)
|
|
}
|
|
|
|
#[inline]
|
|
fn lcg(state: u64) -> u64 {
|
|
state
|
|
.wrapping_mul(6364136223846793005)
|
|
.wrapping_add(1442695040888963407)
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
fn ctx<'a>(
|
|
drone_id: u32,
|
|
swarm_size: usize,
|
|
step: u64,
|
|
current: Position3D,
|
|
visited: &'a [Position3D],
|
|
peers: &'a [Position3D],
|
|
) -> PatternContext<'a> {
|
|
PatternContext {
|
|
drone_id: NodeId(drone_id),
|
|
swarm_size,
|
|
current,
|
|
area_w: 100.0,
|
|
area_h: 80.0,
|
|
altitude_z: -20.0,
|
|
scan_width_m: 5.0,
|
|
step,
|
|
visited,
|
|
peers,
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_partitioned_strips_disjoint() {
|
|
let empty: [Position3D; 0] = [];
|
|
// Two drones, swarm of 2: drone 0 owns left half, drone 1 the right half.
|
|
let mut d0_xs = Vec::new();
|
|
let mut d1_xs = Vec::new();
|
|
for s in 0..40u64 {
|
|
let c0 = ctx(0, 2, s, Position3D::zero(), &empty, &empty);
|
|
let c1 = ctx(1, 2, s, Position3D::zero(), &empty, &empty);
|
|
d0_xs.push(FlightPattern::PartitionedLawnmower.next_target(&c0).x);
|
|
d1_xs.push(FlightPattern::PartitionedLawnmower.next_target(&c1).x);
|
|
}
|
|
let mid = 100.0 / 2.0;
|
|
// Drone 0 stays strictly in the left half, drone 1 strictly in the right.
|
|
assert!(d0_xs.iter().all(|&x| x <= mid), "drone 0 left of midline");
|
|
assert!(d1_xs.iter().all(|&x| x >= mid), "drone 1 right of midline");
|
|
// And they never share an x position (disjoint strips → no overlap).
|
|
for &a in &d0_xs {
|
|
for &b in &d1_xs {
|
|
assert!(a < b || (a <= mid && b >= mid), "strips overlap: {a} vs {b}");
|
|
}
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_all_patterns_in_bounds() {
|
|
let visited = [
|
|
Position3D { x: 10.0, y: 10.0, z: -20.0 },
|
|
Position3D { x: 50.0, y: 40.0, z: -20.0 },
|
|
];
|
|
let peers = [Position3D { x: 30.0, y: 20.0, z: -20.0 }];
|
|
for pat in FlightPattern::all() {
|
|
let mut current = Position3D { x: 25.0, y: 25.0, z: -20.0 };
|
|
for s in 0..20u64 {
|
|
let c = ctx(1, 4, s, current, &visited, &peers);
|
|
let t = pat.next_target(&c);
|
|
assert!(
|
|
t.x >= 0.0 && t.x <= 100.0,
|
|
"{} x out of bounds at step {s}: {}",
|
|
pat.name(),
|
|
t.x
|
|
);
|
|
assert!(
|
|
t.y >= 0.0 && t.y <= 80.0,
|
|
"{} y out of bounds at step {s}: {}",
|
|
pat.name(),
|
|
t.y
|
|
);
|
|
assert_eq!(t.z, -20.0, "{} altitude wrong", pat.name());
|
|
current = t;
|
|
}
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_pattern_from_str_roundtrip() {
|
|
for pat in FlightPattern::all() {
|
|
assert_eq!(
|
|
FlightPattern::from_str(pat.name()),
|
|
pat,
|
|
"roundtrip failed for {}",
|
|
pat.name()
|
|
);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_spiral_radius_grows() {
|
|
let empty: [Position3D; 0] = [];
|
|
let centre_x = 100.0 / 2.0;
|
|
let centre_y = 80.0 / 2.0;
|
|
let dist = |s: u64| {
|
|
let c = ctx(0, 1, s, Position3D::zero(), &empty, &empty);
|
|
let t = FlightPattern::Spiral.next_target(&c);
|
|
((t.x - centre_x).powi(2) + (t.y - centre_y).powi(2)).sqrt()
|
|
};
|
|
let near = dist(1);
|
|
let far = dist(50);
|
|
assert!(
|
|
far > near,
|
|
"spiral radius should grow: step1={near}, step50={far}"
|
|
);
|
|
}
|
|
}
|