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

Merge pull request #1 from david-sawatzke/master

Browse Source 
Various improvements (höhö)
This commit is contained in:
gnxlxnxx
2020-05-21 10:52:39 +02:00
committed by GitHub
parent 75c0cdbc8e ddea6ecf73
commit 3b3d5430bd
2 changed files with 131 additions and 221 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Cargo.toml
+1
View File
@@ -15,3 +15,4 @@ edition = "2018"
[dependencies] [dependencies]
euclid = "0.20.11"
src/lib.rs
+130 -221
View File
@@ -1,22 +1,23 @@
/*! Call all functions with a Vector as argument the vector should contain: //! Call all functions with a Vector as argument the vector should contain:
- the end point as origin //! - the end point as origin
- the end angle as angle in degrees in clockwise direction (eg. 0° facing north, 90° facing east, ...) //! - the end angle as angle in degrees in clockwise direction (eg. 0° facing north, 90° facing east, ...)
- the circle radius as magnitude //! - the circle radius as magnitude
//!
//! Start Vector is in the origin facing in positive x-direction
//!
//! Every struct defined here is 2 dimensional and uses f64
Start Vector is in the origin facing in positive x-direction use euclid::{Point2D, Rotation2D, UnknownUnit};
Every struct defined here is 2 dimensional and uses f64 */ pub type Angle = euclid::Angle<f64>;
pub type Point = Point2D<f64, UnknownUnit>;
/// Point type Vector2D = euclid::Vector2D<f64, UnknownUnit>;
pub struct Point { type Rotation = Rotation2D<f64, UnknownUnit, UnknownUnit>;
pub x: f64,
pub y: f64,
}
/// Vector with origin, angle and magnitude /// Vector with origin, angle and magnitude
pub struct Vector { pub struct Vector {
pub origin: Point, pub origin: Point,
pub angle: f64, pub angle: Angle,
pub magnitude: f64, pub magnitude: f64,
} }
@@ -29,7 +30,7 @@ pub struct Circle {
/// Circle route with a circle and a angle for how long to drive on this circle /// Circle route with a circle and a angle for how long to drive on this circle
pub struct CircleRoute { pub struct CircleRoute {
pub circle: Circle, pub circle: Circle,
pub angle: f64, pub angle: Angle,
} }
/// Route with a start Circle, a tangent straight and a end Circle (eg. rsl, rsr, lsr, lsl) /// Route with a start Circle, a tangent straight and a end Circle (eg. rsl, rsr, lsr, lsl)
@@ -52,90 +53,69 @@ pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
let mut route_csc = RouteCSC { let mut route_csc = RouteCSC {
start: CircleRoute { start: CircleRoute {
circle: Circle { circle: Circle {
center: Point { center: Point::new(end.magnitude, 0.0),
x: end.magnitude,
y: 0.0,
},
radius: end.magnitude, radius: end.magnitude,
}, },
angle: 0.0, angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
origin: Point { x: 0.0, y: 0.0 }, origin: Point::zero(),
angle: 0.0, angle: Angle::zero(),
magnitude: 0.0, magnitude: 0.0,
}, },
end: CircleRoute { end: CircleRoute {
circle: Circle { circle: Circle {
center: Point { x: 0.0, y: 0.0 }, center: Point::zero(),
radius: end.magnitude, radius: end.magnitude,
}, },
angle: 0.0, angle: Angle::zero(),
}, },
}; };
// get the center point by adding the end vector to the end point // get the center point by adding the end vector to the end point
// this works because the argument is the angle in positive y direction // this works because the argument is the angle in positive y direction
// not positive x direction so we dont have to rotate it here anymore // not positive x direction so we dont have to rotate it here anymore
// the angle has to be counter clockwise though (thats why 360 - end.angle) // the angle has to be counter clockwise though (thats why we use the inverse end.angle)
route_csc.end.circle.center = Point { route_csc.end.circle.center = end.origin
x: end.origin.x + end.magnitude * (360.0 - end.angle).to_radians().cos(), + Rotation::new(end.angle)
y: end.origin.y + end.magnitude * (360.0 - end.angle).to_radians().sin(), .inverse()
}; .transform_vector(Vector2D::new(end.magnitude, 0.0));
// get the tangent pitch which is the same as the pitch between the two // get the tangent pitch which is the same as the pitch between the two
// circle centers since our circles have the same radius // circle centers since our circles have the same radius
route_csc.tangent.angle = ((route_csc.end.circle.center.y - route_csc.start.circle.center.y) route_csc.tangent.angle = Angle::radians(
/ (route_csc.end.circle.center.x - route_csc.start.circle.center.x)) ((route_csc.end.circle.center.y - route_csc.start.circle.center.y)
.atan() / (route_csc.end.circle.center.x - route_csc.start.circle.center.x))
.to_degrees(); .atan(),
);
// if the end circle center x value is smaller than the // if the end circle center x value is smaller than the
// start circle center x value // start circle center x value
// the angle would be 180° rotated so to prevent that: // the angle would be 180° rotated so to prevent that:
if route_csc.end.circle.center.x < route_csc.start.circle.center.x { if route_csc.end.circle.center.x < route_csc.start.circle.center.x {
route_csc.tangent.angle += 180.0; route_csc.tangent.angle += Angle::pi();
} }
// get the tangent magnitude this, again, is the same as the distance // get the tangent magnitude this, again, is the same as the distance
// between the two circle centers since our circles have the same radius // between the two circle centers since our circles have the same radius
route_csc.tangent.magnitude = route_csc.tangent.magnitude =
((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powf(2.0) ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powf(2.0)) + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2))
.sqrt(); .sqrt();
// get the angle of the start circle // get the angle of the start circle
route_csc.start.angle = 90.0 - route_csc.tangent.angle; route_csc.start.angle = (Angle::frac_pi_2() - route_csc.tangent.angle).positive();
// make the angle pretty
if route_csc.start.angle < 0.0 {
route_csc.start.angle += 360.0;
}
if route_csc.start.angle >= 360.0 {
route_csc.start.angle -= 360.0;
}
// get the tangent origin by moving the vector from the start circle center // 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 // 90° to it's own direction and the magnitude of the circle radius
route_csc.tangent.origin = Point { route_csc.tangent.origin = route_csc.start.circle.center
x: route_csc.start.circle.center.x + Rotation::new(Angle::pi() - end.angle)
+ route_csc.start.circle.radius * (180.0 - route_csc.start.angle).to_radians().cos(), .transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0));
y: route_csc.start.circle.center.y
+ route_csc.start.circle.radius * (180.0 - route_csc.start.angle).to_radians().sin(),
};
// get the angle of the start circle // get the angle of the start circle
// the angle where we start from the tangent equals the one we finish // the angle where we start from the tangent equals the one we finish
// so we can use that in here // so we can use that in here
route_csc.end.angle = end.angle - route_csc.start.angle; route_csc.end.angle = (end.angle - route_csc.start.angle).positive();
// make the angle pretty
if route_csc.end.angle < 0.0 {
route_csc.end.angle += 360.0;
}
if route_csc.end.angle >= 360.0 {
route_csc.end.angle -= 360.0;
}
Ok(route_csc) Ok(route_csc)
} }
@@ -145,54 +125,47 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
let mut route_csc = RouteCSC { let mut route_csc = RouteCSC {
start: CircleRoute { start: CircleRoute {
circle: Circle { circle: Circle {
center: Point { center: Point::new(-end.magnitude, 0.0),
x: -end.magnitude,
y: 0.0,
},
radius: end.magnitude, radius: end.magnitude,
}, },
angle: 0.0, angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
origin: Point { x: 0.0, y: 0.0 }, origin: Point::zero(),
angle: 0.0, angle: Angle::zero(),
magnitude: 0.0, magnitude: 0.0,
}, },
end: CircleRoute { end: CircleRoute {
circle: Circle { circle: Circle {
center: Point { x: 0.0, y: 0.0 }, center: Point::zero(),
radius: end.magnitude, radius: end.magnitude,
}, },
angle: 0.0, angle: Angle::zero(),
}, },
}; };
// get the center point by adding the end vector to the end point // get the center point by adding the end vector to the end point
// we have to rotate the vector 180° (90° because the given angle is from the y axis // we have to rotate the vector π (π/2 because the given angle is from the y axis
// and 90 more to not get the tangent but the vector to the center point) // and π/2 more to not get the tangent but the vector to the center point)
// and again we have to use the counter clockwise direction // and again we have to use the counter clockwise direction
route_csc.end.circle.center = Point { route_csc.end.circle.center = end.origin
x: end.origin.x + end.magnitude * (180.0 - end.angle).to_radians().cos(), + Rotation::new(Angle::pi() - end.angle)
y: end.origin.y + end.magnitude * (180.0 - end.angle).to_radians().sin(), .transform_vector(Vector2D::new(end.magnitude, 0.0));
};
// get the tangent pitch which is the same as the pitch between the two // get the tangent pitch which is the same as the pitch between the two
// circle centers since our circles have the same radius // circle centers since our circles have the same radius
route_csc.tangent.angle = ((route_csc.end.circle.center.y - route_csc.start.circle.center.y) route_csc.tangent.angle = Angle::radians(
/ (route_csc.end.circle.center.x - route_csc.start.circle.center.x)) ((route_csc.end.circle.center.y - route_csc.start.circle.center.y)
.atan() / (route_csc.end.circle.center.x - route_csc.start.circle.center.x))
.to_degrees(); .atan(),
)
.positive();
// if the end circle center x value is smaller than the // if the end circle center x value is smaller than the
// start circle center x value // start circle center x value
// the angle would be 180° rotated so to prevent that: // the angle would be π rotated so to prevent that:
if route_csc.end.circle.center.x < route_csc.start.circle.center.x { if route_csc.end.circle.center.x < route_csc.start.circle.center.x {
route_csc.tangent.angle += 180.0; route_csc.tangent.angle = (route_csc.tangent.angle + Angle::pi()).positive();
}
// make the angle positive
if route_csc.tangent.angle < 0.0 {
route_csc.tangent.angle += 360.0;
} }
// get the tangent magnitude this, again, is the same as the distance // get the tangent magnitude this, again, is the same as the distance
@@ -200,44 +173,25 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
route_csc.tangent.magnitude = ((route_csc.end.circle.center.x route_csc.tangent.magnitude = ((route_csc.end.circle.center.x
- route_csc.start.circle.center.x) - route_csc.start.circle.center.x)
.abs() .abs()
.powf(2.0) .powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y) + (route_csc.end.circle.center.y - route_csc.start.circle.center.y)
.abs() .abs()
.powf(2.0)) .powi(2))
.sqrt(); .sqrt();
// get the angle of the start circle // get the angle of the start circle
route_csc.start.angle = route_csc.tangent.angle - 90.0; route_csc.start.angle = (route_csc.tangent.angle - Angle::frac_pi_2()).positive();
// make the angle pretty
if route_csc.start.angle < 0.0 {
route_csc.start.angle += 360.0;
}
if route_csc.start.angle >= 360.0 {
route_csc.start.angle -= 360.0;
}
// get the tangent origin by moving the vector from the start circle center // 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 // 90° to it's own direction and the magnitude of the circle radius
route_csc.tangent.origin = Point { route_csc.tangent.origin = route_csc.start.circle.center
x: route_csc.start.circle.center.x + Rotation::new(route_csc.start.angle)
+ route_csc.start.circle.radius * route_csc.start.angle.to_radians().cos(), .transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0));
y: route_csc.start.circle.center.y
+ route_csc.start.circle.radius * route_csc.start.angle.to_radians().sin(),
};
// get the angle of the start circle // get the angle of the start circle
// the angle where we start from the tangent equals the one we finish // the angle where we start from the tangent equals the one we finish
// so we can use that in here // so we can use that in here
route_csc.end.angle = end.angle - route_csc.start.angle; route_csc.end.angle = (end.angle - route_csc.start.angle).positive();
// make the angle pretty
if route_csc.end.angle < 0.0 {
route_csc.end.angle += 360.0;
}
if route_csc.end.angle >= 360.0 {
route_csc.end.angle -= 360.0;
}
Ok(route_csc) Ok(route_csc)
} }
@@ -247,40 +201,36 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
let mut route_csc = RouteCSC { let mut route_csc = RouteCSC {
start: CircleRoute { start: CircleRoute {
circle: Circle { circle: Circle {
center: Point { center: Point::new(end.magnitude, 0.0),
x: end.magnitude,
y: 0.0,
},
radius: end.magnitude, radius: end.magnitude,
}, },
angle: 0.0, angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
origin: Point { x: 0.0, y: 0.0 }, origin: Point::zero(),
angle: 0.0, angle: Angle::zero(),
magnitude: 0.0, magnitude: 0.0,
}, },
end: CircleRoute { end: CircleRoute {
circle: Circle { circle: Circle {
center: Point { x: 0.0, y: 0.0 }, center: Point::zero(),
radius: end.magnitude, radius: end.magnitude,
}, },
angle: 0.0, angle: Angle::zero(),
}, },
}; };
// get the center point by adding the end vector to the end point // get the center point by adding the end vector to the end point
// we have to rotate the vector 180° (90° because the given angle is from the y axis // we have to rotate the vector π (π/2 because the given angle is from the y axis
// and 90 more to not get the tangent but the vector to the center point) // and π/2 more to not get the tangent but the vector to the center point)
// and again we have to use the counter clockwise direction // and again we have to use the counter clockwise direction
route_csc.end.circle.center = Point { route_csc.end.circle.center = end.origin
x: end.origin.x + end.magnitude * (180.0 - end.angle).to_radians().cos(), + Rotation::new(Angle::pi() - end.angle)
y: end.origin.y + end.magnitude * (180.0 - end.angle).to_radians().sin(), .transform_vector(Vector2D::new(end.magnitude, 0.0));
};
// check if inside tangent can even be constructed // check if inside tangent can even be constructed
if ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powf(2.0) if ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powf(2.0)) + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2))
.sqrt() .sqrt()
< 2.0 * end.magnitude < 2.0 * end.magnitude
{ {
@@ -289,63 +239,44 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
// get the tangent length via some simple trigonometry // get the tangent length via some simple trigonometry
route_csc.tangent.magnitude = route_csc.tangent.magnitude =
((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powf(2.0) ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powf(2.0) + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2)
- (2.0 * end.magnitude).powf(2.0)) - (2.0 * end.magnitude).powi(2))
.sqrt(); .sqrt();
// tangent middle is the same as the middle of the straight from the center of the start // tangent middle is the same as the middle of the straight from the center of the start
let tangent_middle = Point { let tangent_middle = route_csc
x: (route_csc.end.circle.center.x + route_csc.start.circle.center.x) / 2.0, .end
y: (route_csc.end.circle.center.y + route_csc.start.circle.center.y) / 2.0, .circle
}; .center
.lerp(route_csc.start.circle.center, 0.5);
// get the tangent angle // get the tangent angle
route_csc.tangent.angle = ((route_csc.end.circle.center.y - tangent_middle.y) route_csc.tangent.angle = Angle::radians(
/ (route_csc.end.circle.center.x - tangent_middle.x)) ((route_csc.end.circle.center.y - tangent_middle.y)
.atan() / (route_csc.end.circle.center.x - tangent_middle.x))
.to_degrees()
- (2.0 * end.magnitude / route_csc.tangent.magnitude)
.atan() .atan()
.to_degrees(); - (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(),
);
// if the end circle center x value is smaller than the // if the end circle center x value is smaller than the
// start circle center x value // start circle center x value
// the angle would be 180° rotated so to prevent that: // the angle would be π rotated so to prevent that:
if route_csc.end.circle.center.x < route_csc.start.circle.center.x { if route_csc.end.circle.center.x < route_csc.start.circle.center.x {
route_csc.tangent.angle += 180.0; route_csc.tangent.angle += Angle::pi();
} }
// get the angle of the start circle // get the angle of the start circle
route_csc.start.angle = 90.0 - route_csc.tangent.angle; route_csc.start.angle = (Angle::frac_pi_2() - route_csc.tangent.angle).positive();
// make the angle pretty
if route_csc.start.angle < 0.0 {
route_csc.start.angle += 360.0;
}
if route_csc.start.angle >= 360.0 {
route_csc.start.angle -= 360.0;
}
// get the tangent origin by moving the vector from the start circle center // get the tangent origin by moving the vector from the start circle center
// along its right angle vector // along its right angle vector
route_csc.tangent.origin = Point { route_csc.tangent.origin = route_csc.start.circle.center
x: route_csc.start.circle.center.x + Rotation::new(Angle::pi() - route_csc.start.angle)
+ route_csc.start.circle.radius * (180.0 - route_csc.start.angle).to_radians().cos(), .transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0));
y: route_csc.start.circle.center.y
+ route_csc.start.circle.radius * (180.0 - route_csc.start.angle).to_radians().sin(),
};
// get the angle of the end circle // get the angle of the end circle
route_csc.end.angle = (90.0 - end.angle) - route_csc.tangent.angle; route_csc.end.angle = ((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive();
// make the angle pretty
if route_csc.end.angle < 0.0 {
route_csc.end.angle += 360.0;
}
if route_csc.end.angle >= 360.0 {
route_csc.end.angle -= 360.0;
}
Ok(route_csc) Ok(route_csc)
} }
@@ -355,40 +286,37 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
let mut route_csc = RouteCSC { let mut route_csc = RouteCSC {
start: CircleRoute { start: CircleRoute {
circle: Circle { circle: Circle {
center: Point { center: Point::new(-end.magnitude, 0.0),
x: -end.magnitude,
y: 0.0,
},
radius: end.magnitude, radius: end.magnitude,
}, },
angle: 0.0, angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
origin: Point { x: 0.0, y: 0.0 }, origin: Point::zero(),
angle: 0.0, angle: Angle::zero(),
magnitude: 0.0, magnitude: 0.0,
}, },
end: CircleRoute { end: CircleRoute {
circle: Circle { circle: Circle {
center: Point { x: 0.0, y: 0.0 }, center: Point::zero(),
radius: end.magnitude, radius: end.magnitude,
}, },
angle: 0.0, angle: Angle::zero(),
}, },
}; };
// get the center point by adding the end vector to the end point // get the center point by adding the end vector to the end point
// this works because the argument is the angle in positive y direction // this works because the argument is the angle in positive y direction
// not positive x direction so we dont have to rotate it here anymore // not positive x direction so we dont have to rotate it here anymore
// the angle has to be counter clockwise though (thats why 360 - end.angle) // the angle has to be counter clockwise though (thats why 2π - end.angle)
route_csc.end.circle.center = Point { route_csc.end.circle.center = end.origin
x: end.origin.x + end.magnitude * (360.0 - end.angle).to_radians().cos(), + Rotation::new(end.angle)
y: end.origin.y + end.magnitude * (360.0 - end.angle).to_radians().sin(), .inverse()
}; .transform_vector(Vector2D::new(end.magnitude, 0.0));
// check if inside tangent can even be constructed // check if inside tangent can even be constructed
if ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powf(2.0) if ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powf(2.0)) + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2))
.sqrt() .sqrt()
< 2.0 * end.magnitude < 2.0 * end.magnitude
{ {
@@ -397,63 +325,44 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
// get the tangent length via some simple trigonometry // get the tangent length via some simple trigonometry
route_csc.tangent.magnitude = route_csc.tangent.magnitude =
((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powf(2.0) ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powf(2.0) + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2)
- (2.0 * end.magnitude).powf(2.0)) - (2.0 * end.magnitude).powi(2))
.sqrt(); .sqrt();
// tangent middle is the same as the middle of the straight from the center of the start // tangent middle is the same as the middle of the straight from the center of the start
let tangent_middle = Point { let tangent_middle = route_csc
x: (route_csc.end.circle.center.x + route_csc.start.circle.center.x) / 2.0, .end
y: (route_csc.end.circle.center.y + route_csc.start.circle.center.y) / 2.0, .circle
}; .center
.lerp(route_csc.start.circle.center, 0.5);
// get the tangent angle // get the tangent angle
route_csc.tangent.angle = ((route_csc.end.circle.center.y - tangent_middle.y) route_csc.tangent.angle = Angle::radians(
/ (route_csc.end.circle.center.x - tangent_middle.x)) ((route_csc.end.circle.center.y - tangent_middle.y)
.atan() / (route_csc.end.circle.center.x - tangent_middle.x))
.to_degrees()
+ (2.0 * end.magnitude / route_csc.tangent.magnitude)
.atan() .atan()
.to_degrees(); + (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(),
);
// if the end circle center x value is smaller than the // if the end circle center x value is smaller than the
// start circle center x value // start circle center x value
// the angle would be 180° rotated so to prevent that: // the angle would be 180° rotated so to prevent that:
if route_csc.end.circle.center.x < route_csc.start.circle.center.x { if route_csc.end.circle.center.x < route_csc.start.circle.center.x {
route_csc.tangent.angle += 180.0; route_csc.tangent.angle += Angle::pi();
} }
// get the angle of the start circle // get the angle of the start circle
route_csc.start.angle = route_csc.tangent.angle - 90.0; route_csc.start.angle = (route_csc.tangent.angle - Angle::frac_pi_2()).positive();
// make the angle pretty
if route_csc.start.angle < 0.0 {
route_csc.start.angle += 360.0;
}
if route_csc.start.angle >= 360.0 {
route_csc.start.angle -= 360.0;
}
// get the tangent origin by moving the vector from the start circle center // 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 // 90° to it's own direction and the magnitude of the circle radius
route_csc.tangent.origin = Point { route_csc.tangent.origin = route_csc.start.circle.center
x: route_csc.start.circle.center.x + Rotation::new(route_csc.start.angle)
+ route_csc.start.circle.radius * route_csc.start.angle.to_radians().cos(), .transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0));
y: route_csc.start.circle.center.y
+ route_csc.start.circle.radius * route_csc.start.angle.to_radians().sin(),
};
// get the angle of the end circle // get the angle of the end circle
route_csc.end.angle = (90.0 - end.angle) - route_csc.tangent.angle; route_csc.end.angle = ((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive();
// make the angle pretty
if route_csc.end.angle < 0.0 {
route_csc.end.angle += 360.0;
}
if route_csc.end.angle >= 360.0 {
route_csc.end.angle -= 360.0;
}
Ok(route_csc) Ok(route_csc)
} }