81 lines
2.6 KiB
Rust
81 lines
2.6 KiB
Rust
//! Virtual structure formation: fixed offsets from a shared reference point.
|
|
|
|
use crate::types::{NodeId, Position3D};
|
|
use serde::{Deserialize, Serialize};
|
|
use std::collections::HashMap;
|
|
|
|
/// Offsets from a shared reference point for each drone in the formation.
|
|
#[derive(Debug, Clone, Serialize, Deserialize)]
|
|
pub struct VirtualStructure {
|
|
/// NodeId → (dx, dy, dz) offset in metres from the reference.
|
|
pub offsets: HashMap<NodeId, (f64, f64, f64)>,
|
|
}
|
|
|
|
impl VirtualStructure {
|
|
/// Create a rectangular grid formation with `n` drones, spaced `spacing_m` apart.
|
|
pub fn grid_formation(n: usize, spacing_m: f64) -> Self {
|
|
let cols = (n as f64).sqrt().ceil() as usize;
|
|
let mut offsets = HashMap::new();
|
|
for i in 0..n {
|
|
let row = i / cols;
|
|
let col = i % cols;
|
|
offsets.insert(
|
|
NodeId(i as u32),
|
|
(row as f64 * spacing_m, col as f64 * spacing_m, 0.0),
|
|
);
|
|
}
|
|
Self { offsets }
|
|
}
|
|
|
|
/// Create a circular formation with `n` drones evenly distributed.
|
|
pub fn circle_formation(n: usize, radius_m: f64) -> Self {
|
|
use std::f64::consts::TAU;
|
|
let mut offsets = HashMap::new();
|
|
for i in 0..n {
|
|
let angle = TAU * i as f64 / n as f64;
|
|
offsets.insert(
|
|
NodeId(i as u32),
|
|
(radius_m * angle.cos(), radius_m * angle.sin(), 0.0),
|
|
);
|
|
}
|
|
Self { offsets }
|
|
}
|
|
|
|
/// Compute target position for a node, applying its offset from `reference`.
|
|
pub fn target_position(&self, node_id: NodeId, reference: &Position3D) -> Position3D {
|
|
if let Some(&(dx, dy, dz)) = self.offsets.get(&node_id) {
|
|
Position3D {
|
|
x: reference.x + dx,
|
|
y: reference.y + dy,
|
|
z: reference.z + dz,
|
|
}
|
|
} else {
|
|
*reference
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn test_grid_formation_4_drones() {
|
|
let vs = VirtualStructure::grid_formation(4, 5.0);
|
|
assert_eq!(vs.offsets.len(), 4);
|
|
let ref_pos = Position3D { x: 100.0, y: 200.0, z: -30.0 };
|
|
let p = vs.target_position(NodeId(0), &ref_pos);
|
|
assert!((p.x - 100.0).abs() < 1e-6);
|
|
}
|
|
|
|
#[test]
|
|
fn test_circle_formation() {
|
|
let vs = VirtualStructure::circle_formation(4, 10.0);
|
|
let ref_pos = Position3D::zero();
|
|
let p = vs.target_position(NodeId(0), &ref_pos);
|
|
// Node 0 at angle 0: x = 10, y = 0
|
|
assert!((p.x - 10.0).abs() < 1e-6);
|
|
assert!(p.y.abs() < 1e-6);
|
|
}
|
|
}
|