braid_sim/
world.rs

1// Copyright (C) The Strand-Braid Authors
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4//! The deterministic ground-truth world: insect 3D positions as a pure function
5//! of time.
6
7use std::f64::consts::PI;
8
9use braid_mvg::PointWorldFrame;
10use nalgebra::Point3;
11
12use crate::scenario::Scenario;
13
14/// Ground-truth state of one insect at one instant.
15#[derive(Debug, Clone)]
16pub struct InsectState {
17    /// Ground-truth identity.
18    pub id: u32,
19    /// 3D position in the world (arena) frame, meters.
20    pub pos: PointWorldFrame<f64>,
21}
22
23/// The simulated world. [`World::state_at`] is a pure function of time, so any
24/// number of independent fake-camera processes can reconstruct the same world
25/// for a given synchronized frame without any communication.
26#[derive(Debug, Clone)]
27pub struct World {
28    scenario: Scenario,
29}
30
31impl World {
32    /// Create a world from a scenario.
33    pub fn new(scenario: Scenario) -> Self {
34        World { scenario }
35    }
36
37    /// The scenario backing this world.
38    pub fn scenario(&self) -> &Scenario {
39        &self.scenario
40    }
41
42    /// Ground-truth positions of all insects present at time `t` (seconds).
43    ///
44    /// An insect is present for `enter_t <= t < exit_t` (with no upper bound if
45    /// `exit_t` is `None`). The result is ordered by the order of insects in the
46    /// scenario.
47    pub fn state_at(&self, t: f64) -> Vec<InsectState> {
48        let center = self.scenario.arena.center();
49        let half = self.scenario.arena.half_extent();
50        self.scenario
51            .insects
52            .iter()
53            .filter(|spec| t >= spec.enter_t && spec.exit_t.is_none_or(|exit| t < exit))
54            .map(|spec| {
55                let m = &spec.motion;
56                let mut p = [0.0f64; 3];
57                for k in 0..3 {
58                    let amp = half[k] * m.fill;
59                    p[k] = center[k] + amp * (2.0 * PI * m.freq_hz[k] * t + m.phase[k]).sin();
60                    // Optional high-frequency maneuver overlay: small amplitude,
61                    // high frequency -> large acceleration the constant-velocity
62                    // tracker cannot predict. Per-axis phase offset so axes are
63                    // not synchronized.
64                    if m.maneuver_amp_m > 0.0 && m.maneuver_freq_hz > 0.0 {
65                        let ph = k as f64 * 2.0;
66                        p[k] += m.maneuver_amp_m * (2.0 * PI * m.maneuver_freq_hz * t + ph).sin();
67                    }
68                }
69                InsectState {
70                    id: spec.id,
71                    pos: PointWorldFrame {
72                        coords: Point3::new(p[0], p[1], p[2]),
73                    },
74                }
75            })
76            .collect()
77    }
78}
braid_sim/world.rs

braid_sim/
world.rs
