swarmkit_sailing/units/
path.rs

1use crate::spherical::LatLon;
2use crate::units::Floats;
3use derive_more::{Add, Deref, DerefMut, Sub};
4use std::f64;
5use std::ops::Mul;
6use swarmkit::{FieldwiseClamp, ParticleRefFrom, ParticleRefMut, SetTo};
7
8// ============================================================================
9// Type Definitions
10// ============================================================================
11
12/// Geometry of a single segment of a `Path<N>`. Pure geometry — no time
13/// component. Use [`ClockedSegment`] (via [`Path::iter_with_running_clock`])
14/// when you need per-segment departure/arrival times.
15#[derive(Copy, Clone, Debug)]
16pub struct RouteSegment {
17    pub origin: LatLon,
18    pub destination: LatLon,
19}
20
21/// A segment paired with its running-clock readings for a particular
22/// traversal.
23///
24/// `t_depart` and `t_arrive` are absolute times along a forward pass
25/// that started at some `start_time` (see
26/// [`Path::iter_with_running_clock`]); `segment_time = t_arrive -
27/// t_depart` and equals `Path::t[i + 1]`.
28///
29/// Returned by [`ClockedSegmentIterator`]. Distinct from [`RouteSegment`]
30/// because the timing values only make sense in the context of a
31/// running-clock traversal — yielding them by default would tempt callers
32/// to re-derive the cumulative time themselves and drift out of sync.
33#[derive(Copy, Clone, Debug)]
34pub struct ClockedSegment {
35    pub origin: LatLon,
36    pub destination: LatLon,
37    pub segment_time: f64,
38    pub t_depart: f64,
39    pub t_arrive: f64,
40}
41
42#[derive(Copy, Clone, Debug, PartialEq, Add, Sub)]
43pub struct Path<const N: usize> {
44    pub xy: PathXY<N>,
45    pub t: Time<N>,
46}
47
48#[derive(Copy, Clone, Debug, PartialEq, Add, Sub)]
49pub struct PathXY<const N: usize>(pub Floats<N>, pub Floats<N>);
50
51#[derive(Copy, Clone, Debug, PartialEq, Deref, DerefMut, Add, Sub)]
52pub struct Time<const N: usize>(pub Floats<N>);
53
54pub struct SegmentIterator<'a, const N: usize> {
55    i: usize,
56    path: &'a Path<N>,
57}
58
59/// Walks the segments of a `Path<N>` while accumulating an absolute
60/// clock, yielding [`ClockedSegment`] per pair.
61///
62/// The clock starts at the `start_time` passed to
63/// [`Path::iter_with_running_clock`] and advances by `segment_time`
64/// (= `Path::t[i + 1]`) at each step.
65pub struct ClockedSegmentIterator<'a, const N: usize> {
66    i: usize,
67    t_running: f64,
68    path: &'a Path<N>,
69}
70
71// ============================================================================
72// Path<N> Implementations
73// ============================================================================
74
75impl<const N: usize> Path<N> {
76    pub fn lat_lon(&self, i: usize) -> LatLon {
77        self.xy.lat_lon(i)
78    }
79
80    pub fn get_segment(&self, i: usize) -> RouteSegment {
81        RouteSegment {
82            origin: self.lat_lon(i),
83            destination: self.lat_lon(i + 1),
84        }
85    }
86
87    pub fn len(&self) -> usize {
88        N
89    }
90
91    /// Always `N == 0` — included for clippy's `len_without_is_empty` lint;
92    /// every `Path<N>` with `N >= 1` is non-empty by construction.
93    pub fn is_empty(&self) -> bool {
94        N == 0
95    }
96
97    /// Iterates segment geometry only — `(origin, destination)` per pair.
98    /// For traversals that need per-segment departure/arrival times, use
99    /// [`Path::iter_with_running_clock`] instead.
100    pub fn iter_segments(&'_ self) -> SegmentIterator<'_, N> {
101        SegmentIterator::new(self)
102    }
103
104    /// Iterates segments while advancing an absolute clock starting at
105    /// `start_time`. Each yielded [`ClockedSegment`] carries `t_depart`
106    /// and `t_arrive` (absolute times at the segment endpoints) plus
107    /// `segment_time` (the segment's stored duration, `t[i + 1]`).
108    /// Use this whenever a traversal needs cumulative timing — wind-field
109    /// integration, fuel accumulation, etc. — instead of hand-rolling the
110    /// `dep += segment_time` loop on top of `iter_segments`.
111    pub fn iter_with_running_clock(&'_ self, start_time: f64) -> ClockedSegmentIterator<'_, N> {
112        ClockedSegmentIterator {
113            i: 0,
114            t_running: start_time,
115            path: self,
116        }
117    }
118}
119
120impl<const N: usize> Default for Path<N> {
121    fn default() -> Self {
122        Self {
123            xy: PathXY(Floats([0.0; N]), Floats([0.0; N])),
124            t: Time(Floats([0.0; N])),
125        }
126    }
127}
128
129impl<const N: usize> FieldwiseClamp for Path<N> {
130    fn clamp(&self, min: Self, max: Self) -> Self {
131        Self {
132            xy: self.xy.clamp(min.xy, max.xy),
133            t: self.t.clamp(min.t, max.t),
134        }
135    }
136}
137
138impl<const N: usize> Mul<f64> for Path<N> {
139    type Output = Self;
140
141    fn mul(self, rhs: f64) -> Self::Output {
142        let mut result = Self::default();
143        for i in 0..N {
144            result.xy.0[i] = self.xy.0[i] * rhs;
145            result.t.0[i] = self.t.0[i] * rhs;
146        }
147        result
148    }
149}
150
151// ============================================================================
152// PathXY<N> Implementations
153// ============================================================================
154
155impl<const N: usize> PathXY<N> {
156    /// Read waypoint `i` as a [`LatLon`]. Mirror of [`Path::lat_lon`]
157    /// for callers that only have the spatial component (movers, fit
158    /// calcs, the segment-range cache builder).
159    pub fn lat_lon(&self, i: usize) -> LatLon {
160        LatLon::new(self.0[i], self.1[i])
161    }
162
163    /// Write waypoint `i` from a [`LatLon`]. Counterpart of
164    /// [`PathXY::lat_lon`] for the write side of mover loops, where
165    /// `p.xy.0[i] = pos.lon; p.xy.1[i] = pos.lat;` would otherwise
166    /// repeat per call site.
167    pub fn set_lat_lon(&mut self, i: usize, pos: LatLon) {
168        self.0[i] = pos.lon;
169        self.1[i] = pos.lat;
170    }
171}
172
173impl<const N: usize> Default for PathXY<N> {
174    fn default() -> Self {
175        Self(Floats([0.0; N]), Floats([0.0; N]))
176    }
177}
178
179impl<const N: usize> ParticleRefFrom for PathXY<N> {
180    type TSource = Path<N>;
181
182    fn divide_from<'a>(
183        source: &'a mut ParticleRefMut<'_, Self::TSource>,
184    ) -> ParticleRefMut<'a, Self>
185    where
186        Self: Copy,
187    {
188        ParticleRefMut {
189            pos: &mut source.pos.xy,
190            vel: &mut source.vel.xy,
191            fit: source.fit,
192            best_pos: &mut source.best_pos.xy,
193            best_fit: source.best_fit,
194        }
195    }
196}
197
198impl<const N: usize> From<Path<N>> for PathXY<N> {
199    fn from(value: Path<N>) -> Self {
200        value.xy
201    }
202}
203
204impl<const N: usize> FieldwiseClamp for PathXY<N> {
205    fn clamp(&self, min: Self, max: Self) -> Self {
206        Self(self.0.clamp(min.0, max.0), self.1.clamp(min.1, max.1))
207    }
208}
209
210impl<const N: usize> Mul<f64> for PathXY<N> {
211    type Output = Self;
212
213    fn mul(self, rhs: f64) -> Self::Output {
214        Self(self.0 * rhs, self.1 * rhs)
215    }
216}
217
218// ============================================================================
219// Time<N> Implementations
220// ============================================================================
221
222impl<const N: usize> Default for Time<N> {
223    fn default() -> Self {
224        Self(Floats([0.0; N]))
225    }
226}
227
228impl<const N: usize> ParticleRefFrom for Time<N> {
229    type TSource = Path<N>;
230
231    fn divide_from<'a>(
232        source: &'a mut ParticleRefMut<'_, Self::TSource>,
233    ) -> ParticleRefMut<'a, Self>
234    where
235        Self: Copy,
236    {
237        ParticleRefMut {
238            pos: &mut source.pos.t,
239            vel: &mut source.vel.t,
240            fit: source.fit,
241            best_pos: &mut source.best_pos.t,
242            best_fit: source.best_fit,
243        }
244    }
245}
246
247impl<const N: usize> SetTo for Time<N> {
248    type TTarget = Path<N>;
249
250    fn set_to_ref_mut(&self, target: &mut ParticleRefMut<'_, Self::TTarget>) {
251        target.pos.t = *self;
252    }
253}
254
255impl<const N: usize> From<Path<N>> for Time<N> {
256    fn from(value: Path<N>) -> Self {
257        value.t
258    }
259}
260
261impl<const N: usize> FieldwiseClamp for Time<N> {
262    fn clamp(&self, min: Self, max: Self) -> Self {
263        Self(self.0.clamp(min.0, max.0))
264    }
265}
266
267impl<const N: usize> Mul<f64> for Time<N> {
268    type Output = Self;
269
270    fn mul(self, rhs: f64) -> Self::Output {
271        Self(self.0 * rhs)
272    }
273}
274
275// ============================================================================
276// SegmentIterator Implementation
277// ============================================================================
278
279impl<'a, const N: usize> SegmentIterator<'a, N> {
280    pub fn new(path: &'a Path<N>) -> Self {
281        SegmentIterator { i: 0, path }
282    }
283}
284
285impl<const N: usize> Iterator for SegmentIterator<'_, N> {
286    type Item = RouteSegment;
287
288    fn next(&mut self) -> Option<Self::Item> {
289        let i = self.i;
290        self.i += 1;
291        if i + 1 < self.path.len() {
292            Some(self.path.get_segment(i))
293        } else {
294            None
295        }
296    }
297}
298
299impl<const N: usize> Iterator for ClockedSegmentIterator<'_, N> {
300    type Item = ClockedSegment;
301
302    fn next(&mut self) -> Option<Self::Item> {
303        let i = self.i;
304        if i + 1 >= self.path.len() {
305            return None;
306        }
307        self.i += 1;
308        let segment_time = self.path.t[i + 1];
309        let t_depart = self.t_running;
310        let t_arrive = t_depart + segment_time;
311        self.t_running = t_arrive;
312        Some(ClockedSegment {
313            origin: self.path.lat_lon(i),
314            destination: self.path.lat_lon(i + 1),
315            segment_time,
316            t_depart,
317            t_arrive,
318        })
319    }
320}
swarmkit_sailing/units/path.rs

swarmkit_sailing/units/
path.rs
