bcar/
bcar.rs

1//! Bicycle car object and related methods.
2
3use crate::geometry::Point;
4use crate::geometry::Pose;
5use crate::properties::Motion;
6use crate::properties::Size;
7
8/// Bicycle car object.
9///
10///
11/// Examples
12/// ========
13///
14/// ```
15/// let bc = bcar::BCar::default();
16/// assert_eq!(bc, bcar::BCar::new(
17///     bcar::Pose::default(),
18///     bcar::Size::default(),
19///     bcar::Motion::default(),
20/// ));
21///
22/// let bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
23/// assert_eq!(bc.pose, bcar::Pose::new(1.0, 2.0, 3.0));
24/// let default_size = bcar::Size::default();
25/// assert_eq!(bc.size, default_size);
26/// assert_eq!(bc.motion, bcar::Motion::default());
27/// ```
28#[derive(Copy, Clone, PartialEq, Debug)]
29pub struct BCar {
30    pub pose: Pose,
31    pub size: Size,
32    pub motion: Motion,
33}
34
35impl BCar {
36    pub fn new(pose: Pose, size: Size, motion: Motion) -> BCar {
37        BCar {pose, size, motion}
38    }
39
40    /// Return new `BCar`.
41    ///
42    /// Arguments
43    /// ---------
44    ///
45    /// - `x` -- Horizontal coordinate of rear axle center.
46    /// - `y` -- Vertical coordinate of rear axle center.
47    /// - `h` -- The heading of the car in the interval [-pi, +pi] radians.
48    ///
49    ///
50    /// Examples
51    /// ========
52    ///
53    /// ```
54    /// let bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
55    /// let bc_pose = bc.pose;
56    /// assert_eq!(1.0, bc_pose.x);
57    /// assert_eq!(2.0, bc_pose.y);
58    /// assert_eq!(3.0, bc_pose.h);
59    /// ```
60    pub fn new_xyh(x: f64, y: f64, h: f64) -> BCar {
61        BCar {
62            pose: Pose::new(x, y, h),
63            size: Size::default(),
64            motion: Motion::default(),
65        }
66    }
67
68    /// Return default `BCar`.
69    ///
70    ///
71    /// Examples
72    /// ========
73    ///
74    /// ```
75    /// let bc = bcar::BCar::default();
76    /// let p = bcar::Pose::default();
77    /// let s = bcar::Size::default();
78    /// let m = bcar::Motion::default();
79    /// assert_eq!(bc, bcar::BCar::new(p, s, m));
80    /// ```
81    pub fn default() -> BCar {
82        BCar {
83            pose: Pose::default(),
84            size: Size::default(),
85            motion: Motion::default(),
86        }
87    }
88
89    /// Return `BCar` size.
90    pub fn size(&self) -> Size {
91        self.size
92    }
93
94    /// Set `BCar` dimensions.
95    ///
96    /// Arguments
97    /// ---------
98    ///
99    /// - `s` -- New size of the car.
100    ///
101    ///
102    /// Examples
103    /// ========
104    ///
105    /// ```
106    /// let size = bcar::Size::new(10.0, 2.0, 3.0, 4.0, 5.0);
107    /// let mut bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
108    /// bc.set_size(size);
109    /// assert_eq!(bc.size, size);
110    /// ```
111    pub fn set_size(&mut self, s: Size) {
112        self.size = s;
113    }
114
115    /// Return `BCar` pose.
116    pub fn pose(&self) -> Pose {
117        self.pose
118    }
119
120    /// Set `BCar` pose.
121    ///
122    /// Arguments
123    /// ---------
124    ///
125    /// - `p` -- New pose of the car.
126    ///
127    ///
128    /// Examples
129    /// ========
130    ///
131    /// ```
132    /// let pose = bcar::Pose::new(1.0, 4.0, 9.0);
133    /// let mut bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
134    /// bc.set_pose(pose);
135    /// assert_eq!(bc.pose, pose);
136    /// ```
137    pub fn set_pose(&mut self, p: Pose) {
138        self.pose = p
139    }
140
141    /// Return `BCar` motion.
142    pub fn motion(&self) -> Motion {
143        self.motion
144    }
145
146    /// Set `BCar` motion.
147    ///
148    /// Arguments
149    /// ---------
150    ///
151    /// - `m` -- New motion of the car.
152    ///
153    ///
154    /// Examples
155    /// ========
156    ///
157    /// ```
158    /// let m = bcar::Motion::new(10.0, 0.1);
159    /// let mut bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
160    /// bc.set_motion(m);
161    /// assert_eq!(bc.motion, m);
162    /// ```
163    pub fn set_motion(&mut self, m: Motion) {
164        self.motion = m
165    }
166
167    /// Return if it's possible to drive to the `pose` trivially.
168    ///
169    /// Trivially in this context means to reach the `pose` with combination of
170    /// line-arc-line segments, where arc is less than pi / 2.
171    ///
172    /// Arguments
173    /// ---------
174    ///
175    /// - `pose` -- Pose to drive to.
176    pub fn drivable(&self, pose: Pose) -> bool {
177        let pi = std::f64::consts::PI;
178        let pi2 = pi / 2.0;
179
180        let mut a_1 = (pose.y - self.pose.y).atan2(pose.x - self.pose.x);
181        a_1 -= self.pose.h;
182        while a_1 < -pi { a_1 += 2.0 * pi; }
183        while a_1 > pi { a_1 -= 2.0 * pi; }
184
185        let mut h_d = pose.h - self.pose.h;
186        while h_d < -pi { h_d += 2.0 * pi; }
187        while h_d > pi { h_d -= 2.0 * pi; }
188
189        let mut a_2 = 0.0;
190        let mut zb = self.pose;
191
192        if h_d == 0.0 && (a_1 == 0.0 || a_2 == pi || a_2 == -pi) {
193            return true;
194        } else if 0.0 < a_1 && a_1 <= pi2 { // Q2
195            zb.rotate(self.ccl(), h_d);
196            // assert zb.h == self.pose.h
197            if pose.y == zb.y && pose.x == zb.x { // pose on zone border
198                return true;
199            }
200            a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
201            while a_2 < -pi { a_2 += 2.0 * pi; }
202            while a_2 > pi { a_2 -= 2.0 * pi; }
203            if zb.h >= a_2 && a_2 >= self.pose.h {
204                return true;
205            }
206        } else if pi2 < a_1 && a_1 <= pi { // Q3
207            zb.rotate(self.ccl(), h_d);
208            // assert zb.h == self.pose.h
209            if pose.y == zb.y && pose.x == zb.x { // pose on zone border
210                return true;
211            }
212            a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
213            a_2 -= pi;
214            while a_2 < -pi { a_2 += 2.0 * pi; }
215            while a_2 > pi { a_2 -= 2.0 * pi; }
216            if self.pose.h >= a_2 && a_2 >= zb.h {
217                return true;
218            }
219        } else if 0.0 > a_1 && a_1 >= -pi2 { // Q1
220            zb.rotate(self.ccr(), h_d);
221            // assert zb.h == self.pose.h
222            if pose.y == zb.y && pose.x == zb.x { // pose on zone border
223                return true;
224            }
225            a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
226            while a_2 < -pi { a_2 += 2.0 * pi; }
227            while a_2 > pi { a_2 -= 2.0 * pi; }
228            if self.pose.h >= a_2 && a_2 >= zb.h {
229                return true;
230            }
231        } else if -pi2 > a_1 && a_1 >= -pi { // Q4
232            zb.rotate(self.ccr(), h_d);
233            // assert zb.h == self.pose.h
234            if pose.y == zb.y && pose.x == zb.x { // pose on zone border
235                return true;
236            }
237            a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
238            a_2 -= pi;
239            while a_2 < -pi { a_2 += 2.0 * pi; }
240            while a_2 > pi { a_2 -= 2.0 * pi; }
241            if zb.h >= a_2 && a_2 >= self.pose.h {
242                return true;
243            }
244        }
245
246        false
247    }
248
249    /// Compute minimum turning radius (MTR).
250    pub fn mtr(&self) -> f64 {
251        (
252            (self.size.curb_to_curb / 2.0).powi(2)
253            - self.size.wheelbase.powi(2)
254        ).sqrt()
255        - self.size.width / 2.0
256    }
257
258    /// Compute MTR circle center on the left side of the car.
259    pub fn ccl(&self) -> Point {
260        let x = self.pose.x;
261        let y = self.pose.y;
262        let h = self.pose.h;
263        Point::new(
264            x + self.mtr() * (h + std::f64::consts::PI / 2.0).cos(),
265            y + self.mtr() * (h + std::f64::consts::PI / 2.0).sin(),
266        )
267    }
268
269    /// Compute MTR circle center on the right side of the car.
270    pub fn ccr(&self) -> Point {
271        let x = self.pose.x;
272        let y = self.pose.y;
273        let h = self.pose.h;
274        Point::new(
275            x + self.mtr() * (h - std::f64::consts::PI / 2.0).cos(),
276            y + self.mtr() * (h - std::f64::consts::PI / 2.0).sin(),
277        )
278    }
279
280    /// Return car frame's center front.
281    pub fn cf(&self) -> Point {
282        let mut x = self.pose.x;
283        x += self.size.distance_to_front * self.pose.h.cos();
284        let mut y = self.pose.y;
285        y += self.size.distance_to_front * self.pose.h.sin();
286        Point {x, y}
287    }
288
289    /// Return car frame's left front corner.
290    pub fn lf(&self) -> Point {
291        let pi = std::f64::consts::PI;
292        let mut x = self.pose.x;
293        x += self.size.width/2.0 * (self.pose.h + pi/2.0).cos();
294        x += self.size.distance_to_front * self.pose.h.cos();
295        let mut y = self.pose.y;
296        y += self.size.width/2.0 * (self.pose.h + pi/2.0).sin();
297        y += self.size.distance_to_front * self.pose.h.sin();
298        Point {x, y}
299    }
300
301    /// Return car frame's coordinate for rear axle on left.
302    pub fn la(&self) -> Point {
303        let pi = std::f64::consts::PI;
304        let mut x = self.pose.x;
305        x += self.size.width/2.0 * (self.pose.h + pi/2.0).cos();
306        let mut y = self.pose.y;
307        y += self.size.width/2.0 * (self.pose.h + pi/2.0).sin();
308        Point {x, y}
309    }
310
311    /// Return car frame's left rear corner.
312    pub fn lr(&self) -> Point {
313        let pi = std::f64::consts::PI;
314        let distance_to_rear = self.size.length - self.size.distance_to_front;
315        let mut x = self.pose.x;
316        x += self.size.width/2.0 * (self.pose.h + pi/2.0).cos();
317        x += -distance_to_rear * self.pose.h.cos();
318        let mut y = self.pose.y;
319        y += self.size.width/2.0 * (self.pose.h + pi/2.0).sin();
320        y += -distance_to_rear * self.pose.h.sin();
321        Point {x, y}
322    }
323
324    /// Return car frame's right rear corner.
325    pub fn rr(&self) -> Point {
326        let pi = std::f64::consts::PI;
327        let distance_to_rear = self.size.length - self.size.distance_to_front;
328        let mut x = self.pose.x;
329        x += self.size.width/2.0 * (self.pose.h - pi/2.0).cos();
330        x += -distance_to_rear * self.pose.h.cos();
331        let mut y = self.pose.y;
332        y += self.size.width/2.0 * (self.pose.h - pi/2.0).sin();
333        y += -distance_to_rear * self.pose.h.sin();
334        Point {x, y}
335    }
336
337    /// Return car frame's coordinate for rear axle on right.
338    pub fn ra(&self) -> Point {
339        let pi = std::f64::consts::PI;
340        let mut x = self.pose.x;
341        x += self.size.width/2.0 * (self.pose.h - pi/2.0).cos();
342        let mut y = self.pose.y;
343        y += self.size.width/2.0 * (self.pose.h - pi/2.0).sin();
344        Point {x, y}
345    }
346
347    /// Return car frame's right front corner.
348    pub fn rf(&self) -> Point {
349        let pi = std::f64::consts::PI;
350        let mut x = self.pose.x;
351        x += self.size.width/2.0 * (self.pose.h - pi/2.0).cos();
352        x += self.size.distance_to_front * self.pose.h.cos();
353        let mut y = self.pose.y;
354        y += self.size.width/2.0 * (self.pose.h - pi/2.0).sin();
355        y += self.size.distance_to_front * self.pose.h.sin();
356        Point {x, y}
357    }
358}
359
360impl Iterator for BCar {
361    type Item = BCar;
362
363    /// Return `BCar` new state.
364    ///
365    ///
366    /// Examples
367    /// ========
368    ///
369    /// ```
370    /// let mut bc = bcar::BCar::default();
371    /// bc.set_motion(bcar::Motion::new(1.0, 0.0));
372    /// bc.next();
373    /// assert_eq!(bc, bcar::BCar::new(
374    ///     bcar::Pose::new(1.0, 0.0, 0.0),
375    ///     bcar::Size::default(),
376    ///     bcar::Motion::new(1.0, 0.0),
377    /// ));
378    /// ```
379    fn next(&mut self) -> Option<Self::Item> {
380        self.pose.x += self.motion.speed * self.pose.h.cos();
381        self.pose.y += self.motion.speed * self.pose.h.sin();
382        self.pose.h += self.motion.speed
383            / self.size.wheelbase
384            * self.motion.steer.tan();
385        Some(*self)
386    }
387}
bcar/bcar.rs

bcar/
bcar.rs
