//! Bicycle car object and related methods.
use crate::geometry::Point;
use crate::geometry::Pose;
use crate::properties::Motion;
use crate::properties::Size;
/// Bicycle car object.
///
///
/// Examples
/// ========
///
/// ```
/// let bc = bcar::BCar::default();
/// assert_eq!(bc, bcar::BCar::new(
/// bcar::Pose::default(),
/// bcar::Size::default(),
/// bcar::Motion::default(),
/// ));
///
/// let bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
/// assert_eq!(bc.pose, bcar::Pose::new(1.0, 2.0, 3.0));
/// let default_size = bcar::Size::default();
/// assert_eq!(bc.size, default_size);
/// assert_eq!(bc.motion, bcar::Motion::default());
/// ```
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct BCar {
pub pose: Pose,
pub size: Size,
pub motion: Motion,
}
impl BCar {
pub fn new(pose: Pose, size: Size, motion: Motion) -> BCar {
BCar {pose, size, motion}
}
/// Return new `BCar`.
///
/// Arguments
/// ---------
///
/// - `x` -- Horizontal coordinate of rear axle center.
/// - `y` -- Vertical coordinate of rear axle center.
/// - `h` -- The heading of the car in the interval [-pi, +pi] radians.
///
///
/// Examples
/// ========
///
/// ```
/// let bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
/// let bc_pose = bc.pose;
/// assert_eq!(1.0, bc_pose.x);
/// assert_eq!(2.0, bc_pose.y);
/// assert_eq!(3.0, bc_pose.h);
/// ```
pub fn new_xyh(x: f64, y: f64, h: f64) -> BCar {
BCar {
pose: Pose::new(x, y, h),
size: Size::default(),
motion: Motion::default(),
}
}
/// Return default `BCar`.
///
///
/// Examples
/// ========
///
/// ```
/// let bc = bcar::BCar::default();
/// let p = bcar::Pose::default();
/// let s = bcar::Size::default();
/// let m = bcar::Motion::default();
/// assert_eq!(bc, bcar::BCar::new(p, s, m));
/// ```
pub fn default() -> BCar {
BCar {
pose: Pose::default(),
size: Size::default(),
motion: Motion::default(),
}
}
/// Return `BCar` size.
pub fn size(&self) -> Size {
self.size
}
/// Set `BCar` dimensions.
///
/// Arguments
/// ---------
///
/// - `s` -- New size of the car.
///
///
/// Examples
/// ========
///
/// ```
/// let size = bcar::Size::new(10.0, 2.0, 3.0, 4.0, 5.0);
/// let mut bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
/// bc.set_size(size);
/// assert_eq!(bc.size, size);
/// ```
pub fn set_size(&mut self, s: Size) {
self.size = s;
}
/// Return `BCar` pose.
pub fn pose(&self) -> Pose {
self.pose
}
/// Set `BCar` pose.
///
/// Arguments
/// ---------
///
/// - `p` -- New pose of the car.
///
///
/// Examples
/// ========
///
/// ```
/// let pose = bcar::Pose::new(1.0, 4.0, 9.0);
/// let mut bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
/// bc.set_pose(pose);
/// assert_eq!(bc.pose, pose);
/// ```
pub fn set_pose(&mut self, p: Pose) {
self.pose = p
}
/// Return `BCar` motion.
pub fn motion(&self) -> Motion {
self.motion
}
/// Set `BCar` motion.
///
/// Arguments
/// ---------
///
/// - `m` -- New motion of the car.
///
///
/// Examples
/// ========
///
/// ```
/// let m = bcar::Motion::new(10.0, 0.1);
/// let mut bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
/// bc.set_motion(m);
/// assert_eq!(bc.motion, m);
/// ```
pub fn set_motion(&mut self, m: Motion) {
self.motion = m
}
/// Return if it's possible to drive to the `pose` trivially.
///
/// Trivially in this context means to reach the `pose` with combination of
/// line-arc-line segments, where arc is less than pi / 2.
///
/// Arguments
/// ---------
///
/// - `pose` -- Pose to drive to.
pub fn drivable(&self, pose: Pose) -> bool {
let pi = std::f64::consts::PI;
let pi2 = pi / 2.0;
let mut a_1 = (pose.y - self.pose.y).atan2(pose.x - self.pose.x);
a_1 -= self.pose.h;
while a_1 < -pi { a_1 += 2.0 * pi; }
while a_1 > pi { a_1 -= 2.0 * pi; }
let mut h_d = pose.h - self.pose.h;
while h_d < -pi { h_d += 2.0 * pi; }
while h_d > pi { h_d -= 2.0 * pi; }
let mut a_2 = 0.0;
let mut zb = self.pose;
if h_d == 0.0 && (a_1 == 0.0 || a_2 == pi || a_2 == -pi) {
return true;
} else if 0.0 < a_1 && a_1 <= pi2 { // Q2
zb.rotate(self.ccl(), h_d);
// assert zb.h == self.pose.h
if pose.y == zb.y && pose.x == zb.x { // pose on zone border
return true;
}
a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
while a_2 < -pi { a_2 += 2.0 * pi; }
while a_2 > pi { a_2 -= 2.0 * pi; }
if zb.h >= a_2 && a_2 >= self.pose.h {
return true;
}
} else if pi2 < a_1 && a_1 <= pi { // Q3
zb.rotate(self.ccl(), h_d);
// assert zb.h == self.pose.h
if pose.y == zb.y && pose.x == zb.x { // pose on zone border
return true;
}
a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
a_2 -= pi;
while a_2 < -pi { a_2 += 2.0 * pi; }
while a_2 > pi { a_2 -= 2.0 * pi; }
if self.pose.h >= a_2 && a_2 >= zb.h {
return true;
}
} else if 0.0 > a_1 && a_1 >= -pi2 { // Q1
zb.rotate(self.ccr(), h_d);
// assert zb.h == self.pose.h
if pose.y == zb.y && pose.x == zb.x { // pose on zone border
return true;
}
a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
while a_2 < -pi { a_2 += 2.0 * pi; }
while a_2 > pi { a_2 -= 2.0 * pi; }
if self.pose.h >= a_2 && a_2 >= zb.h {
return true;
}
} else if -pi2 > a_1 && a_1 >= -pi { // Q4
zb.rotate(self.ccr(), h_d);
// assert zb.h == self.pose.h
if pose.y == zb.y && pose.x == zb.x { // pose on zone border
return true;
}
a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
a_2 -= pi;
while a_2 < -pi { a_2 += 2.0 * pi; }
while a_2 > pi { a_2 -= 2.0 * pi; }
if zb.h >= a_2 && a_2 >= self.pose.h {
return true;
}
}
false
}
/// Compute minimum turning radius (MTR).
pub fn mtr(&self) -> f64 {
(
(self.size.curb_to_curb / 2.0).powi(2)
- self.size.wheelbase.powi(2)
).sqrt()
- self.size.width / 2.0
}
/// Compute MTR circle center on the left side of the car.
pub fn ccl(&self) -> Point {
let x = self.pose.x;
let y = self.pose.y;
let h = self.pose.h;
Point::new(
x + self.mtr() * (h + std::f64::consts::PI / 2.0).cos(),
y + self.mtr() * (h + std::f64::consts::PI / 2.0).sin(),
)
}
/// Compute MTR circle center on the right side of the car.
pub fn ccr(&self) -> Point {
let x = self.pose.x;
let y = self.pose.y;
let h = self.pose.h;
Point::new(
x + self.mtr() * (h - std::f64::consts::PI / 2.0).cos(),
y + self.mtr() * (h - std::f64::consts::PI / 2.0).sin(),
)
}
/// Return car frame's center front.
pub fn cf(&self) -> Point {
let mut x = self.pose.x;
x += self.size.distance_to_front * self.pose.h.cos();
let mut y = self.pose.y;
y += self.size.distance_to_front * self.pose.h.sin();
Point {x, y}
}
/// Return car frame's left front corner.
pub fn lf(&self) -> Point {
let pi = std::f64::consts::PI;
let mut x = self.pose.x;
x += self.size.width/2.0 * (self.pose.h + pi/2.0).cos();
x += self.size.distance_to_front * self.pose.h.cos();
let mut y = self.pose.y;
y += self.size.width/2.0 * (self.pose.h + pi/2.0).sin();
y += self.size.distance_to_front * self.pose.h.sin();
Point {x, y}
}
/// Return car frame's coordinate for rear axle on left.
pub fn la(&self) -> Point {
let pi = std::f64::consts::PI;
let mut x = self.pose.x;
x += self.size.width/2.0 * (self.pose.h + pi/2.0).cos();
let mut y = self.pose.y;
y += self.size.width/2.0 * (self.pose.h + pi/2.0).sin();
Point {x, y}
}
/// Return car frame's left rear corner.
pub fn lr(&self) -> Point {
let pi = std::f64::consts::PI;
let distance_to_rear = self.size.length - self.size.distance_to_front;
let mut x = self.pose.x;
x += self.size.width/2.0 * (self.pose.h + pi/2.0).cos();
x += -distance_to_rear * self.pose.h.cos();
let mut y = self.pose.y;
y += self.size.width/2.0 * (self.pose.h + pi/2.0).sin();
y += -distance_to_rear * self.pose.h.sin();
Point {x, y}
}
/// Return car frame's right rear corner.
pub fn rr(&self) -> Point {
let pi = std::f64::consts::PI;
let distance_to_rear = self.size.length - self.size.distance_to_front;
let mut x = self.pose.x;
x += self.size.width/2.0 * (self.pose.h - pi/2.0).cos();
x += -distance_to_rear * self.pose.h.cos();
let mut y = self.pose.y;
y += self.size.width/2.0 * (self.pose.h - pi/2.0).sin();
y += -distance_to_rear * self.pose.h.sin();
Point {x, y}
}
/// Return car frame's coordinate for rear axle on right.
pub fn ra(&self) -> Point {
let pi = std::f64::consts::PI;
let mut x = self.pose.x;
x += self.size.width/2.0 * (self.pose.h - pi/2.0).cos();
let mut y = self.pose.y;
y += self.size.width/2.0 * (self.pose.h - pi/2.0).sin();
Point {x, y}
}
/// Return car frame's right front corner.
pub fn rf(&self) -> Point {
let pi = std::f64::consts::PI;
let mut x = self.pose.x;
x += self.size.width/2.0 * (self.pose.h - pi/2.0).cos();
x += self.size.distance_to_front * self.pose.h.cos();
let mut y = self.pose.y;
y += self.size.width/2.0 * (self.pose.h - pi/2.0).sin();
y += self.size.distance_to_front * self.pose.h.sin();
Point {x, y}
}
}
impl Iterator for BCar {
type Item = BCar;
/// Return `BCar` new state.
///
///
/// Examples
/// ========
///
/// ```
/// let mut bc = bcar::BCar::default();
/// bc.set_motion(bcar::Motion::new(1.0, 0.0));
/// bc.next();
/// assert_eq!(bc, bcar::BCar::new(
/// bcar::Pose::new(1.0, 0.0, 0.0),
/// bcar::Size::default(),
/// bcar::Motion::new(1.0, 0.0),
/// ));
/// ```
fn next(&mut self) -> Option<Self::Item> {
self.pose.x += self.motion.speed * self.pose.h.cos();
self.pose.y += self.motion.speed * self.pose.h.sin();
self.pose.h += self.motion.speed
/ self.size.wheelbase
* self.motion.steer.tan();
Some(*self)
}
}