gnxlxnxx
/
dubins_path
mirror of https://github.com/gnxlxnxx/dubins_path
1
0
Fork 0
Code Issues Releases Wiki Activity

let rust-analyzer do it's thing

Browse Source 
some reformatting to make the code more readable
master
Roman Kretschmer 5 years ago
parent 7e02bc5a98
commit 88d978be8a
Signed by: gnxlxnxx
GPG Key ID: A7499F8F45C21DD4
1 changed files with 99 additions and 88 deletions
Show Stats Download Patch File Download Diff File

187
src/lib.rs
Unescape View File

@ -30,13 +30,24 @@
//! //!
//! //!
use euclid::{approxeq::ApproxEq, Point2D, Rotation2D, UnknownUnit}; use std::{
use num_traits; cmp::PartialOrd,
convert,
ops::{Add, Mul, Rem, Sub},
};
use convert::From;
pub use euclid::Angle;
use euclid::{approxeq::ApproxEq, Point2D, Rotation2D, Trig, UnknownUnit, Vector2D};
use num_traits::{
self,
float::{Float, FloatConst},
Zero,
};
use thiserror::Error; use thiserror::Error;
pub type Angle<T> = euclid::Angle<T>;
pub type Point<T> = Point2D<T, UnknownUnit>; pub type Point<T> = Point2D<T, UnknownUnit>;
pub type Vector<T> = euclid::Vector2D<T, UnknownUnit>; pub type Vector<T> = Vector2D<T, UnknownUnit>;
type Rotation<T> = Rotation2D<T, UnknownUnit, UnknownUnit>; type Rotation<T> = Rotation2D<T, UnknownUnit, UnknownUnit>;
#[derive(Debug, Error)] #[derive(Debug, Error)]
@ -54,7 +65,7 @@ pub struct StraightPath<T> {
pub vector: Vector<T>, pub vector: Vector<T>,
} }
impl<T: euclid::approxeq::ApproxEq<T>> StraightPath<T> { impl<T: ApproxEq<T>> StraightPath<T> {
/// approximate equality to other Vector /// approximate equality to other Vector
pub fn approx_eq(&self, other: Self) -> bool { pub fn approx_eq(&self, other: Self) -> bool {
ApproxEq::approx_eq(&self.vector, &other.vector) ApproxEq::approx_eq(&self.vector, &other.vector)
@ -66,13 +77,13 @@ impl<T: euclid::approxeq::ApproxEq<T>> StraightPath<T> {
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub struct CirclePath<T> pub struct CirclePath<T>
where where
T: std::ops::Mul<T, Output = T> T: Mul<T, Output = T>
+ euclid::approxeq::ApproxEq<T> + ApproxEq<T>
+ std::ops::Rem<Output = T> + Rem<Output = T>
+ std::ops::Sub<Output = T> + Sub<Output = T>
+ std::ops::Add<Output = T> + Add<Output = T>
+ num_traits::Zero + Zero
+ num_traits::FloatConst + FloatConst
+ PartialOrd + PartialOrd
+ Copy, + Copy,
{ {
@ -83,13 +94,13 @@ where
impl<T> CirclePath<T> impl<T> CirclePath<T>
where where
T: std::ops::Mul<T, Output = T> T: Mul<T, Output = T>
+ euclid::approxeq::ApproxEq<T> + ApproxEq<T>
+ std::ops::Rem<Output = T> + Rem<Output = T>
+ std::ops::Sub<Output = T> + Sub<Output = T>
+ std::ops::Add<Output = T> + Add<Output = T>
+ num_traits::Zero + Zero
+ num_traits::FloatConst + FloatConst
+ PartialOrd + PartialOrd
+ Copy, + Copy,
{ {
@ -117,14 +128,14 @@ where
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub struct RouteCSC<T> pub struct RouteCSC<T>
where where
T: std::ops::Mul<T, Output = T> T: Mul<T, Output = T>
+ std::ops::Mul + Mul
+ euclid::approxeq::ApproxEq<T> + ApproxEq<T>
+ std::ops::Rem<Output = T> + Rem<Output = T>
+ std::ops::Sub<Output = T> + Sub<Output = T>
+ std::ops::Add<Output = T> + Add<Output = T>
+ num_traits::Zero + Zero
+ num_traits::FloatConst + FloatConst
+ PartialOrd + PartialOrd
+ Copy, + Copy,
{ {
@ -137,14 +148,14 @@ where
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub struct RouteCCC<T> pub struct RouteCCC<T>
where where
T: std::ops::Mul<T, Output = T> T: Mul<T, Output = T>
+ std::ops::Mul + Mul
+ euclid::approxeq::ApproxEq<T> + ApproxEq<T>
+ std::ops::Rem<Output = T> + Rem<Output = T>
+ std::ops::Sub<Output = T> + Sub<Output = T>
+ std::ops::Add<Output = T> + Add<Output = T>
+ num_traits::Zero + Zero
+ num_traits::FloatConst + FloatConst
+ PartialOrd + PartialOrd
+ Copy, + Copy,
{ {
@ -156,14 +167,14 @@ where
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub enum Path<T> pub enum Path<T>
where where
T: std::ops::Mul<T, Output = T> T: Mul<T, Output = T>
+ std::ops::Mul + Mul
+ euclid::approxeq::ApproxEq<T> + ApproxEq<T>
+ std::ops::Rem<Output = T> + Rem<Output = T>
+ std::ops::Sub<Output = T> + Sub<Output = T>
+ std::ops::Add<Output = T> + Add<Output = T>
+ num_traits::Zero + Zero
+ num_traits::FloatConst + FloatConst
+ PartialOrd + PartialOrd
+ Copy, + Copy,
{ {
@ -174,15 +185,15 @@ where
/// Route with a start Circle, a tangent straight and a end Circle /// Route with a start Circle, a tangent straight and a end Circle
impl<T> RouteCSC<T> impl<T> RouteCSC<T>
where where
T: std::ops::Add T: Add
+ std::ops::Mul + Mul
+ std::ops::Mul<f64, Output = T> + Mul<f64, Output = T>
+ num_traits::float::FloatConst + FloatConst
+ num_traits::float::Float + Float
+ std::cmp::PartialOrd + PartialOrd
+ std::convert::From<f64> + From<f64>
+ euclid::approxeq::ApproxEq<T> + ApproxEq<T>
+ euclid::Trig, + Trig,
{ {
/// right straight right route /// right straight right route
pub fn rsr(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Result<Self, Error> { pub fn rsr(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Result<Self, Error> {
@ -233,7 +244,7 @@ where
Ok(Self { Ok(Self {
start: CirclePath { start: CirclePath {
center: start_center, center: start_center,
radius: radius, radius,
angle: start_angle, angle: start_angle,
}, },
tangent: StraightPath { tangent: StraightPath {
@ -242,7 +253,7 @@ where
}, },
end: CirclePath { end: CirclePath {
center: end_center, center: end_center,
radius: radius, radius,
angle: end_angle, angle: end_angle,
}, },
}) })
@ -296,7 +307,7 @@ where
Ok(Self { Ok(Self {
start: CirclePath { start: CirclePath {
center: start_center, center: start_center,
radius: radius, radius,
angle: start_angle, angle: start_angle,
}, },
tangent: StraightPath { tangent: StraightPath {
@ -305,7 +316,7 @@ where
}, },
end: CirclePath { end: CirclePath {
center: end_center, center: end_center,
radius: radius, radius,
angle: end_angle, angle: end_angle,
}, },
}) })
@ -368,7 +379,7 @@ where
Ok(Self { Ok(Self {
start: CirclePath { start: CirclePath {
center: start_center, center: start_center,
radius: radius, radius,
angle: start_angle, angle: start_angle,
}, },
tangent: StraightPath { tangent: StraightPath {
@ -377,7 +388,7 @@ where
}, },
end: CirclePath { end: CirclePath {
center: end_center, center: end_center,
radius: radius, radius,
angle: end_angle, angle: end_angle,
}, },
}) })
@ -440,7 +451,7 @@ where
Ok(Self { Ok(Self {
start: CirclePath { start: CirclePath {
center: start_center, center: start_center,
radius: radius, radius,
angle: start_angle, angle: start_angle,
}, },
tangent: StraightPath { tangent: StraightPath {
@ -449,7 +460,7 @@ where
}, },
end: CirclePath { end: CirclePath {
center: end_center, center: end_center,
radius: radius, radius,
angle: end_angle, angle: end_angle,
}, },
}) })
@ -495,15 +506,15 @@ where
/// Route with 3 Circles /// Route with 3 Circles
impl<T> RouteCCC<T> impl<T> RouteCCC<T>
where where
T: std::ops::Add T: Add
+ std::ops::Mul + Mul
+ std::ops::Mul<f64, Output = T> + Mul<f64, Output = T>
+ num_traits::float::FloatConst + FloatConst
+ num_traits::float::Float + Float
+ std::cmp::PartialOrd + PartialOrd
+ std::convert::From<f64> + From<f64>
+ euclid::approxeq::ApproxEq<T> + ApproxEq<T>
+ euclid::Trig, + Trig,
{ {
/// right left right route (not working yet) /// right left right route (not working yet)
pub fn rlr(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Result<Self, Error> { pub fn rlr(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Result<Self, Error> {
@ -537,8 +548,8 @@ where
+ (vector_start_center_end_center.length() / (radius * 4.0)).acos(); + (vector_start_center_end_center.length() / (radius * 4.0)).acos();
vector_start_center_middle_center = Vector::new( vector_start_center_middle_center = Vector::new(
(radius * 2.0) * euclid::Trig::cos(vector_start_center_middle_center_angle), (radius * 2.0) * Trig::cos(vector_start_center_middle_center_angle),
(radius * 2.0) * euclid::Trig::sin(vector_start_center_middle_center_angle), (radius * 2.0) * Trig::sin(vector_start_center_middle_center_angle),
); );
Point::new( Point::new(
@ -571,17 +582,17 @@ where
Ok(Self { Ok(Self {
start: CirclePath { start: CirclePath {
center: start_center, center: start_center,
radius: radius, radius,
angle: start_angle, angle: start_angle,
}, },
middle: CirclePath { middle: CirclePath {
center: middle_center, center: middle_center,
radius: radius, radius,
angle: middle_angle, angle: middle_angle,
}, },
end: CirclePath { end: CirclePath {
center: end_center, center: end_center,
radius: radius, radius,
angle: end_angle, angle: end_angle,
}, },
}) })
@ -618,8 +629,8 @@ where
- (vector_start_center_end_center.length() / (radius * 4.0)).acos(); - (vector_start_center_end_center.length() / (radius * 4.0)).acos();
vector_start_center_middle_center = Vector::new( vector_start_center_middle_center = Vector::new(
(radius * 2.0) * euclid::Trig::cos(vector_start_center_middle_center_angle), (radius * 2.0) * Trig::cos(vector_start_center_middle_center_angle),
(radius * 2.0) * euclid::Trig::sin(vector_start_center_middle_center_angle), (radius * 2.0) * Trig::sin(vector_start_center_middle_center_angle),
); );
Point::new( Point::new(
@ -648,17 +659,17 @@ where
Ok(Self { Ok(Self {
start: CirclePath { start: CirclePath {
center: start_center, center: start_center,
radius: radius, radius,
angle: start_angle, angle: start_angle,
}, },
middle: CirclePath { middle: CirclePath {
center: middle_center, center: middle_center,
radius: radius, radius,
angle: middle_angle, angle: middle_angle,
}, },
end: CirclePath { end: CirclePath {
center: end_center, center: end_center,
radius: radius, radius,
angle: end_angle, angle: end_angle,
}, },
}) })
@ -699,15 +710,15 @@ where
/// get the shortest path /// get the shortest path
pub fn get_shortest<T>(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Path<T> pub fn get_shortest<T>(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Path<T>
where where
T: std::ops::Add T: Add
+ std::ops::Mul + Mul
+ std::ops::Mul<f64, Output = T> + Mul<f64, Output = T>
+ num_traits::float::FloatConst + FloatConst
+ num_traits::float::Float + Float
+ std::cmp::PartialOrd + PartialOrd
+ std::convert::From<f64> + From<f64>
+ euclid::approxeq::ApproxEq<T> + ApproxEq<T>
+ euclid::Trig, + Trig,
{ {
let route_csc = RouteCSC::get_shortest(radius, end_point, end_angle).unwrap(); let route_csc = RouteCSC::get_shortest(radius, end_point, end_angle).unwrap();
let route_ccc = RouteCCC::get_shortest(radius, end_point, end_angle); let route_ccc = RouteCCC::get_shortest(radius, end_point, end_angle);

Write Preview
Loading…
Cancel
Save