street_engine/core/geometry/
angle.rs

1/// Provides calculation of angles.
2/// As the angle is 0, the direction is the negative y-axis.
3#[derive(Debug, Clone, Copy)]
4pub struct Angle(f64);
5
6impl PartialEq for Angle {
7    fn eq(&self, other: &Self) -> bool {
8        self.0 == other.0
9    }
10}
11
12impl Eq for Angle {}
13
14impl Angle {
15    /// Create an angle from the radian.
16    pub fn new(radian: f64) -> Self {
17        Self(radian).normalize()
18    }
19
20    /// Get the radian.
21    pub fn radian(&self) -> f64 {
22        self.0
23    }
24
25    /// Get opposite angle.
26    pub fn opposite(&self) -> Self {
27        Self::new(self.0 + std::f64::consts::PI)
28    }
29
30    /// Get the clockwise right angle.
31    pub fn right_clockwise(&self) -> Self {
32        Self::new(self.0 + 0.5 * std::f64::consts::PI)
33    }
34
35    /// Get the counterclockwise right angle.
36    pub fn right_counterclockwise(&self) -> Self {
37        Self::new(self.0 - 0.5 * std::f64::consts::PI)
38    }
39
40    pub fn unit_x(&self) -> f64 {
41        self.0.sin()
42    }
43
44    pub fn unit_y(&self) -> f64 {
45        -self.0.cos()
46    }
47
48    /// Normalize to the range of (-PI, PI].
49    fn normalize(&self) -> Self {
50        let radian = self.0.rem_euclid(2.0 * std::f64::consts::PI);
51        let radian = if radian > std::f64::consts::PI {
52            radian - 2.0 * std::f64::consts::PI
53        } else {
54            radian
55        };
56        Self(radian)
57    }
58
59    /// Calculate the clockwise angle difference.
60    fn diff_clockwise_to(&self, to: &Self) -> f64 {
61        let diff = to.0 - self.0;
62        if diff < 0.0 {
63            diff + 2.0 * std::f64::consts::PI
64        } else {
65            diff
66        }
67    }
68
69    /// Calculate the counterclockwise angle difference.
70    fn diff_counterclockwise_to(&self, to: &Self) -> f64 {
71        let diff = self.0 - to.0;
72        if diff < 0.0 {
73            diff + 2.0 * std::f64::consts::PI
74        } else {
75            diff
76        }
77    }
78
79    /// Create an iterator of angles between two angles.
80    fn iter_range_closer(&self, other: &Self, step_num: usize) -> AngleIter {
81        let (rad_from, rad_to) = {
82            let diff_clockwise = self.diff_clockwise_to(other);
83            let diff_counterclockwise = self.diff_counterclockwise_to(other);
84            if diff_clockwise < diff_counterclockwise {
85                (self.0, self.0 + diff_clockwise)
86            } else {
87                (other.0, other.0 + diff_counterclockwise)
88            }
89        };
90
91        AngleIter {
92            rad_from,
93            rad_to,
94            step_num,
95            step_current: 0,
96        }
97    }
98
99    /// Create an iterator of angles around the specified angle.
100    pub fn iter_range_around(&self, radian_range: f64, step_num: usize) -> AngleIter {
101        if step_num == 1 || radian_range == 0.0 {
102            return AngleIter {
103                rad_from: self.0,
104                rad_to: self.0,
105                step_num: 1,
106                step_current: 0,
107            };
108        }
109        let frac_radian_range_2 = radian_range / 2.0;
110        Self::new(self.0 - frac_radian_range_2)
111            .iter_range_closer(&Self::new(self.0 + frac_radian_range_2), step_num)
112    }
113}
114
115/// An iterator of angles.
116pub struct AngleIter {
117    rad_from: f64,
118    rad_to: f64,
119    step_num: usize,
120    step_current: usize,
121}
122
123impl Iterator for AngleIter {
124    type Item = Angle;
125
126    fn next(&mut self) -> Option<Self::Item> {
127        if self.step_current < self.step_num {
128            if self.step_num == 1 {
129                self.step_current += 1;
130                Some(Angle::new(self.rad_from))
131            } else {
132                let angle = self.rad_from
133                    + (self.rad_to - self.rad_from) * (self.step_current as f64)
134                        / ((self.step_num - 1) as f64);
135                self.step_current += 1;
136                Some(Angle::new(angle))
137            }
138        } else {
139            None
140        }
141    }
142}
143
144#[cfg(test)]
145mod tests {
146    use super::*;
147
148    #[test]
149    fn test_angle_normalize() {
150        assert_eq!(Angle::new(0.0).0, 0.0);
151        assert_eq!(Angle::new(std::f64::consts::PI).0, std::f64::consts::PI);
152        assert_eq!(Angle::new(2.0 * std::f64::consts::PI).0, 0.0);
153        assert_eq!(Angle::new(-std::f64::consts::PI).0, std::f64::consts::PI);
154        assert_eq!(Angle::new(-2.0 * std::f64::consts::PI).0, 0.0);
155    }
156
157    #[test]
158    fn test_angle_diff_clockwise_to() {
159        assert_eq!(
160            Angle::new(0.0).diff_clockwise_to(&Angle::new(std::f64::consts::PI)),
161            std::f64::consts::PI
162        );
163        assert_eq!(
164            Angle::new(std::f64::consts::PI).diff_clockwise_to(&Angle::new(0.0)),
165            std::f64::consts::PI
166        );
167        assert_eq!(
168            Angle::new(0.0).diff_clockwise_to(&Angle::new(0.5 * std::f64::consts::PI)),
169            0.5 * std::f64::consts::PI
170        );
171        assert_eq!(
172            Angle::new(0.5 * std::f64::consts::PI).diff_clockwise_to(&Angle::new(0.0)),
173            1.5 * std::f64::consts::PI
174        );
175    }
176
177    #[test]
178    fn test_angle_diff_counterclockwise() {
179        assert_eq!(
180            Angle::new(0.0).diff_counterclockwise_to(&Angle::new(std::f64::consts::PI)),
181            std::f64::consts::PI
182        );
183        assert_eq!(
184            Angle::new(std::f64::consts::PI).diff_counterclockwise_to(&Angle::new(0.0)),
185            std::f64::consts::PI
186        );
187        assert_eq!(
188            Angle::new(0.0).diff_counterclockwise_to(&Angle::new(0.5 * std::f64::consts::PI)),
189            1.5 * std::f64::consts::PI
190        );
191        assert_eq!(
192            Angle::new(0.5 * std::f64::consts::PI).diff_counterclockwise_to(&Angle::new(0.0)),
193            0.5 * std::f64::consts::PI
194        );
195    }
196
197    #[test]
198    fn test_angle_iter_range_closer() {
199        let mut iter =
200            Angle::new(0.0).iter_range_closer(&Angle::new(std::f64::consts::PI * 0.5), 5);
201        assert_eq!(iter.next(), Some(Angle::new(0.0)));
202        assert_eq!(iter.next(), Some(Angle::new(0.125 * std::f64::consts::PI)));
203        assert_eq!(iter.next(), Some(Angle::new(0.25 * std::f64::consts::PI)));
204        assert_eq!(iter.next(), Some(Angle::new(0.375 * std::f64::consts::PI)));
205        assert_eq!(iter.next(), Some(Angle::new(0.5 * std::f64::consts::PI)));
206        assert_eq!(iter.next(), None);
207
208        let mut iter =
209            Angle::new(std::f64::consts::PI * 0.5).iter_range_closer(&Angle::new(0.0), 5);
210        assert_eq!(iter.next(), Some(Angle::new(0.0)));
211        assert_eq!(iter.next(), Some(Angle::new(0.125 * std::f64::consts::PI)));
212        assert_eq!(iter.next(), Some(Angle::new(0.25 * std::f64::consts::PI)));
213        assert_eq!(iter.next(), Some(Angle::new(0.375 * std::f64::consts::PI)));
214        assert_eq!(iter.next(), Some(Angle::new(0.5 * std::f64::consts::PI)));
215        assert_eq!(iter.next(), None);
216
217        let mut iter = Angle::new(std::f64::consts::PI * 1.8)
218            .iter_range_closer(&Angle::new(std::f64::consts::PI * 1.4), 5);
219        assert_eq!(iter.next(), Some(Angle::new(1.4 * std::f64::consts::PI)));
220        assert_eq!(iter.next(), Some(Angle::new(1.5 * std::f64::consts::PI)));
221        let iter_next = iter.next();
222        // check if the radian is normalized
223        assert_eq!(iter_next.unwrap().radian(), -0.4 * std::f64::consts::PI);
224        assert_eq!(iter_next, Some(Angle::new(1.6 * std::f64::consts::PI)));
225        assert_eq!(iter.next(), Some(Angle::new(1.7 * std::f64::consts::PI)));
226        assert_eq!(iter.next(), Some(Angle::new(1.8 * std::f64::consts::PI)));
227        assert_eq!(iter.next(), None);
228
229        let mut iter =
230            Angle::new(0.0).iter_range_closer(&Angle::new(std::f64::consts::PI * 1.5), 5);
231        assert_eq!(iter.next(), Some(Angle::new(1.5 * std::f64::consts::PI)));
232        assert_eq!(iter.next(), Some(Angle::new(1.625 * std::f64::consts::PI)));
233        assert_eq!(iter.next(), Some(Angle::new(1.75 * std::f64::consts::PI)));
234        assert_eq!(iter.next(), Some(Angle::new(1.875 * std::f64::consts::PI)));
235        assert_eq!(iter.next(), Some(Angle::new(2.0 * std::f64::consts::PI)));
236        assert_eq!(iter.next(), None);
237    }
238}
street_engine/core/geometry/angle.rs

street_engine/core/geometry/
angle.rs
