gnxlxnxx/dubins_path
1
0
Fork 0
mirror of https://github.com/gnxlxnxx/dubins_path synced 2026-07-27 07:03:03 +02:00
Code Issues Releases Wiki Activity

Use Impl for the RouteCSC struct

Browse Source
This commit is contained in:
gnxlxnxx
2020-06-08 21:36:22 +02:00
parent a7747c0c4c
commit c0361d11b9
1 changed files with 280 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib.rs
+29 -7
View File
@@ -43,8 +43,10 @@ pub struct RouteCCC {
pub end: CircleVector,
}
/// Route with a start Circle, a tangent straight and a end Circle
impl RouteCSC {
/// right straight right route
pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
pub fn rsr(end: Vector) -> Result<Self, ()> {
let mut route_csc = RouteCSC {
start: CircleVector {
center: Point::new(end.magnitude, 0.0),
@@ -111,7 +113,7 @@ pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
}
/// left straight left route
pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
pub fn lsl(end: Vector) -> Result<Self, ()> {
let mut route_csc = RouteCSC {
start: CircleVector {
center: Point::new(-end.magnitude, 0.0),
@@ -182,7 +184,7 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
}
/// right straight left route
pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
pub fn rsl(end: Vector) -> Result<Self, ()> {
let mut route_csc = RouteCSC {
start: CircleVector {
center: Point::new(end.magnitude, 0.0),
@@ -229,7 +231,8 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
// get the tangent angle
route_csc.tangent.angle = Angle::radians(
((route_csc.end.center.y - tangent_middle.y) / (route_csc.end.center.x - tangent_middle.x))
((route_csc.end.center.y - tangent_middle.y)
/ (route_csc.end.center.x - tangent_middle.x))
.atan()
- (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(),
);
@@ -251,13 +254,14 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
.transform_vector(Vector2D::new(route_csc.start.radius, 0.0));
// get the angle of the end circle
route_csc.end.angle = ((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive();
route_csc.end.angle =
((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive();
Ok(route_csc)
}
/// left straight right route
pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
pub fn lsr(end: Vector) -> Result<Self, ()> {
let mut route_csc = RouteCSC {
start: CircleVector {
center: Point::new(-end.magnitude, 0.0),
@@ -305,7 +309,8 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
// get the tangent angle
route_csc.tangent.angle = Angle::radians(
((route_csc.end.center.y - tangent_middle.y) / (route_csc.end.center.x - tangent_middle.x))
((route_csc.end.center.y - tangent_middle.y)
/ (route_csc.end.center.x - tangent_middle.x))
.atan()
+ (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(),
);
@@ -320,6 +325,23 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
// get the angle of the start circle
route_csc.start.angle = (route_csc.tangent.angle - Angle::frac_pi_2()).positive();
// get the tangent origin by moving the vector from the start circle center
// 90° to it's own direction and the magnitude of the circle radius
route_csc.tangent.origin = route_csc.start.center
+ Rotation::new(route_csc.start.angle)
.transform_vector(Vector2D::new(route_csc.start.radius, 0.0));
// get the angle of the end circle
route_csc.end.angle =
((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive();
Ok(route_csc)
}
}
// get the angle of the start circle
route_csc.start.angle = (route_csc.tangent.angle - Angle::frac_pi_2()).positive();
// get the tangent origin by moving the vector from the start circle center
// 90° to it's own direction and the magnitude of the circle radius
route_csc.tangent.origin = route_csc.start.center