75 lines
2.1 KiB
Rust
75 lines
2.1 KiB
Rust
//! Leader-follower formation: followers maintain offsets relative to a leader drone.
|
|
|
|
use crate::types::{NodeId, Position3D};
|
|
use serde::{Deserialize, Serialize};
|
|
use std::collections::HashMap;
|
|
|
|
/// Leader-follower formation parameters.
|
|
#[derive(Debug, Clone, Serialize, Deserialize)]
|
|
pub struct LeaderFollower {
|
|
pub leader_id: NodeId,
|
|
/// Follower → (dx, dy, dz) offset from leader's position.
|
|
pub offsets: HashMap<NodeId, (f64, f64, f64)>,
|
|
}
|
|
|
|
impl LeaderFollower {
|
|
pub fn new(leader_id: NodeId) -> Self {
|
|
Self {
|
|
leader_id,
|
|
offsets: HashMap::new(),
|
|
}
|
|
}
|
|
|
|
pub fn add_follower(&mut self, follower: NodeId, offset: (f64, f64, f64)) {
|
|
self.offsets.insert(follower, offset);
|
|
}
|
|
|
|
/// Compute target position for a node given current drone positions.
|
|
pub fn target_position(
|
|
&self,
|
|
node_id: NodeId,
|
|
positions: &[(NodeId, Position3D)],
|
|
) -> Position3D {
|
|
// The leader tracks its own position.
|
|
if node_id == self.leader_id {
|
|
return positions
|
|
.iter()
|
|
.find(|(id, _)| *id == self.leader_id)
|
|
.map(|(_, p)| *p)
|
|
.unwrap_or_default();
|
|
}
|
|
let leader_pos = positions
|
|
.iter()
|
|
.find(|(id, _)| *id == self.leader_id)
|
|
.map(|(_, p)| *p)
|
|
.unwrap_or_default();
|
|
|
|
if let Some(&(dx, dy, dz)) = self.offsets.get(&node_id) {
|
|
Position3D {
|
|
x: leader_pos.x + dx,
|
|
y: leader_pos.y + dy,
|
|
z: leader_pos.z + dz,
|
|
}
|
|
} else {
|
|
leader_pos
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn test_follower_tracks_leader() {
|
|
let mut lf = LeaderFollower::new(NodeId(0));
|
|
lf.add_follower(NodeId(1), (-5.0, 0.0, 0.0));
|
|
let positions = vec![
|
|
(NodeId(0), Position3D { x: 10.0, y: 20.0, z: -30.0 }),
|
|
];
|
|
let target = lf.target_position(NodeId(1), &positions);
|
|
assert!((target.x - 5.0).abs() < 1e-6);
|
|
assert!((target.y - 20.0).abs() < 1e-6);
|
|
}
|
|
}
|