traffic_sim/
group.rs

1use crate::math::{project_point_onto_curve, rot90};
2use crate::util::Interval;
3use crate::Link;
4use crate::{math::Point2d, LinkId};
5use cgmath::InnerSpace;
6use itertools::iproduct;
7use serde::{Deserialize, Serialize};
8
9/// A set of adjacent links.
10/// Lane changing is only permitted between members of a link group.
11#[derive(Clone)]
12pub struct LinkGroup {
13    num_links: usize,
14    projections: Vec<LinkProjection>,
15}
16
17#[derive(Clone)]
18pub(crate) struct LinkProjection {
19    src: LinkId,
20    dst: LinkId,
21    inv_segment_len: f64,
22    samples: Vec<ProjectedSample>,
23}
24
25/// Represents a vehicle's position relative to a link.
26#[derive(Clone, Copy)]
27pub struct Obstacle {
28    /// The approximate longitudinal position of the obstacle in m.
29    pub pos: f64,
30    /// The approximate lateral extents of the obstacle in m.
31    pub lat: Interval<f64>,
32    /// The rear coordinates of the obstacle in world space.
33    pub rear_coords: [Point2d; 2],
34    /// The velocity of the obstacle in m/s.
35    pub vel: f64,
36}
37
38#[derive(Clone, Copy, Serialize, Deserialize)]
39struct ProjectedSample {
40    pos: f64,
41    lat: f64,
42}
43
44impl LinkGroup {
45    pub(crate) fn new(links: &[&Link]) -> Self {
46        if links.len() < 2 {
47            panic!("Link group must contain atleast two links");
48        }
49
50        let segment_len = 2.0;
51
52        let projections = iproduct!(links, links)
53            .filter(|(src, dst)| src.id() != dst.id())
54            .map(|(src, dst)| LinkProjection {
55                src: src.id(),
56                dst: dst.id(),
57                inv_segment_len: 1.0 / segment_len,
58                samples: (0..)
59                    .map(|i| segment_len * i as f64)
60                    .take_while(|pos| *pos < src.curve().length())
61                    .map(|pos| src.curve().sample_centre(pos).pos)
62                    .scan(Some(0.0), |pos, point| {
63                        *pos = project_point_onto_curve(dst.curve(), point, 0.1, *pos);
64                        pos.map(|pos| (point, pos))
65                    })
66                    .map(|(point, pos)| {
67                        let s = dst.curve().sample_centre(pos);
68                        let lat = (point - s.pos).dot(rot90(s.tan));
69                        ProjectedSample { pos, lat }
70                    })
71                    .collect(),
72            })
73            .collect();
74
75        Self {
76            num_links: links.len(),
77            projections,
78        }
79    }
80
81    pub(crate) fn link_ids(&self) -> impl Iterator<Item = LinkId> + '_ {
82        let n = self.num_links - 1;
83        (0..self.num_links).map(move |i| self.projections[n * i].src)
84    }
85
86    pub(crate) fn projections(&self, src_link: LinkId) -> &[LinkProjection] {
87        let idx = self
88            .link_ids()
89            .position(|id| id == src_link)
90            .expect("Link ID is not in group.");
91        let n = self.num_links - 1;
92        &self.projections[(n * idx)..(n * (idx + 1))]
93    }
94
95    // pub(crate) fn project(&self, rear_coords: [Point2d; 2], vel: f64, out: &mut GroupPosition) {
96    //     // Update the segment index
97    //     out.segment = self
98    //         .segments
99    //         .iter()
100    //         .skip(out.segment)
101    //         .position(|(pos, tan)| rear_coords.iter().any(|c| (c - pos).dot(*tan) <= 0.0))
102    //         .map(|i| i + out.segment)
103    //         .unwrap_or(self.segments.len())
104    //         .saturating_sub(1);
105
106    //     // Get the local coordinate system around the vehicle
107    //     let (pos, tan) = unsafe {
108    //         // SAFETY: Use of `position` above guarantees the index is in bounds.
109    //         *self.segments.get_unchecked(out.segment)
110    //     };
111
112    //     // Project the vehicle's rear coordinates and save
113    //     let proj = rear_coords.map(|c| project_local(c, pos, rot90(tan), tan));
114    //     out.pos = f64::min(proj[0].y, proj[1].y);
115    //     out.lat = Interval::new(proj[0].x, proj[1].x);
116    //     out.vel = vel;
117    // }
118}
119
120impl LinkProjection {
121    pub fn link_id(&self) -> LinkId {
122        self.dst
123    }
124
125    pub fn project(
126        &self,
127        rear_coords: [Point2d; 2],
128        pos: f64,
129        lat: Interval<f64>,
130        vel: f64,
131    ) -> Obstacle {
132        let idx = usize::min(
133            (pos * self.inv_segment_len) as usize,
134            self.samples.len() - 1,
135        );
136        let sample = unsafe {
137            // SAFETY: Way index is generated guarantees it's within bounds
138            self.samples.get_unchecked(idx)
139        };
140        // Account for a negative `pos` value
141        let behind = f64::min(pos, 0.0);
142        Obstacle {
143            rear_coords,
144            pos: sample.pos + behind,
145            lat: lat + sample.lat,
146            vel,
147        }
148    }
149}
traffic_sim/group.rs

traffic_sim/
group.rs
