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

Use euclid `Angle`

Browse Source
pull/1/head
David Sawatzke 6 years ago
parent e51114621b
commit a80b6bc1d9
2 changed files with 81 additions and 146 deletions
Show Stats Download Patch File Download Diff File

1
Cargo.toml
Unescape View File

@ -15,3 +15,4 @@ edition = "2018"
[dependencies]
euclid = "0.20.11"

218
src/lib.rs
Unescape View File

@ -1,11 +1,13 @@
/*! Call all functions with a Vector as argument the vector should contain:
- the end point as origin
- the end angle as angle in degrees in clockwise direction (eg. 0° facing north, 90° facing east, ...)
- the circle radius as magnitude
//! Call all functions with a Vector as argument the vector should contain:
//! - the end point as origin
//! - the end angle as angle in degrees in clockwise direction (eg. 0° facing north, 90° facing east, ...)
//! - 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
Every struct defined here is 2 dimensional and uses f64 */
use euclid::Angle;
/// Point
pub struct Point {
@ -16,7 +18,7 @@ pub struct Point {
/// Vector with origin, angle and magnitude
pub struct Vector {
pub origin: Point,
pub angle: f64,
pub angle: Angle<f64>,
pub magnitude: f64,
}
@ -29,7 +31,7 @@ pub struct Circle {
/// Circle route with a circle and a angle for how long to drive on this circle
pub struct CircleRoute {
pub circle: Circle,
pub angle: f64,
pub angle: Angle<f64>,
}
/// Route with a start Circle, a tangent straight and a end Circle (eg. rsl, rsr, lsr, lsl)
@ -58,11 +60,11 @@ pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
},
radius: end.magnitude,
},
angle: 0.0,
angle: Angle::zero(),
},
tangent: Vector {
origin: Point { x: 0.0, y: 0.0 },
angle: 0.0,
angle: Angle::zero(),
magnitude: 0.0,
},
end: CircleRoute {
@ -70,31 +72,32 @@ pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
center: Point { x: 0.0, y: 0.0 },
radius: end.magnitude,
},
angle: 0.0,
angle: Angle::zero(),
},
};
// 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
// 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 {
x: end.origin.x + end.magnitude * (360.0 - end.angle).to_radians().cos(),
y: end.origin.y + end.magnitude * (360.0 - end.angle).to_radians().sin(),
x: end.origin.x + end.magnitude * (Angle::two_pi() - end.angle).get().cos(),
y: end.origin.y + end.magnitude * (Angle::two_pi() - end.angle).get().sin(),
};
// get the tangent pitch which is the same as the pitch between the two
// 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.y - route_csc.start.circle.center.y)
/ (route_csc.end.circle.center.x - route_csc.start.circle.center.x))
.atan()
.to_degrees();
.atan(),
);
// if the end circle center x value is smaller than the
// start circle center x value
// the angle would be 180° rotated so to prevent that:
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
@ -105,37 +108,21 @@ pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
.sqrt();
// get the angle of the start circle
route_csc.start.angle = 90.0 - route_csc.tangent.angle;
// 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;
}
route_csc.start.angle = (Angle::frac_pi_2() - route_csc.tangent.angle).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 = Point {
x: route_csc.start.circle.center.x
+ route_csc.start.circle.radius * (180.0 - route_csc.start.angle).to_radians().cos(),
+ route_csc.start.circle.radius * (Angle::pi() - route_csc.start.angle).get().cos(),
y: route_csc.start.circle.center.y
+ route_csc.start.circle.radius * (180.0 - route_csc.start.angle).to_radians().sin(),
+ route_csc.start.circle.radius * (Angle::pi() - route_csc.start.angle).get().sin(),
};
// get the angle of the start circle
// the angle where we start from the tangent equals the one we finish
// so we can use that in here
route_csc.end.angle = end.angle - route_csc.start.angle;
// 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;
}
route_csc.end.angle = (end.angle - route_csc.start.angle).positive();
Ok(route_csc)
}
@ -151,11 +138,11 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
},
radius: end.magnitude,
},
angle: 0.0,
angle: Angle::zero(),
},
tangent: Vector {
origin: Point { x: 0.0, y: 0.0 },
angle: 0.0,
angle: Angle::zero(),
magnitude: 0.0,
},
end: CircleRoute {
@ -163,36 +150,33 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
center: Point { x: 0.0, y: 0.0 },
radius: end.magnitude,
},
angle: 0.0,
angle: Angle::zero(),
},
};
// 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
// and 90 more to not get the tangent but the vector to the center point)
// we have to rotate the vector π (π/2 because the given angle is from the y axis
// 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
route_csc.end.circle.center = Point {
x: end.origin.x + end.magnitude * (180.0 - end.angle).to_radians().cos(),
y: end.origin.y + end.magnitude * (180.0 - end.angle).to_radians().sin(),
x: end.origin.x + end.magnitude * (Angle::pi() - end.angle).get().cos(),
y: end.origin.y + end.magnitude * (Angle::pi() - end.angle).get().sin(),
};
// get the tangent pitch which is the same as the pitch between the two
// 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.y - route_csc.start.circle.center.y)
/ (route_csc.end.circle.center.x - route_csc.start.circle.center.x))
.atan()
.to_degrees();
.atan(),
)
.positive();
// if the end circle center x value is smaller than the
// 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 {
route_csc.tangent.angle += 180.0;
}
// make the angle positive
if route_csc.tangent.angle < 0.0 {
route_csc.tangent.angle += 360.0;
route_csc.tangent.angle = (route_csc.tangent.angle + Angle::pi()).positive();
}
// get the tangent magnitude this, again, is the same as the distance
@ -207,37 +191,21 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
.sqrt();
// get the angle of the start circle
route_csc.start.angle = route_csc.tangent.angle - 90.0;
// 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;
}
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 = Point {
x: route_csc.start.circle.center.x
+ route_csc.start.circle.radius * route_csc.start.angle.to_radians().cos(),
+ route_csc.start.circle.radius * route_csc.start.angle.get().cos(),
y: route_csc.start.circle.center.y
+ route_csc.start.circle.radius * route_csc.start.angle.to_radians().sin(),
+ route_csc.start.circle.radius * route_csc.start.angle.get().sin(),
};
// get the angle of the start circle
// the angle where we start from the tangent equals the one we finish
// so we can use that in here
route_csc.end.angle = end.angle - route_csc.start.angle;
// 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;
}
route_csc.end.angle = (end.angle - route_csc.start.angle).positive();
Ok(route_csc)
}
@ -253,11 +221,11 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
},
radius: end.magnitude,
},
angle: 0.0,
angle: Angle::zero(),
},
tangent: Vector {
origin: Point { x: 0.0, y: 0.0 },
angle: 0.0,
angle: Angle::zero(),
magnitude: 0.0,
},
end: CircleRoute {
@ -265,17 +233,17 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
center: Point { x: 0.0, y: 0.0 },
radius: end.magnitude,
},
angle: 0.0,
angle: Angle::zero(),
},
};
// 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
// and 90 more to not get the tangent but the vector to the center point)
// we have to rotate the vector π (π/2 because the given angle is from the y axis
// 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
route_csc.end.circle.center = Point {
x: end.origin.x + end.magnitude * (180.0 - end.angle).to_radians().cos(),
y: end.origin.y + end.magnitude * (180.0 - end.angle).to_radians().sin(),
x: end.origin.x + end.magnitude * (Angle::pi() - end.angle).get().cos(),
y: end.origin.y + end.magnitude * (Angle::pi() - end.angle).get().sin(),
};
// check if inside tangent can even be constructed
@ -301,51 +269,34 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
};
// 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.y - tangent_middle.y)
/ (route_csc.end.circle.center.x - tangent_middle.x))
.atan()
.to_degrees()
- (2.0 * end.magnitude / route_csc.tangent.magnitude)
.atan()
.to_degrees();
- (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(),
);
// if the end circle center x value is smaller than the
// 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 {
route_csc.tangent.angle += 180.0;
route_csc.tangent.angle += Angle::pi();
}
// get the angle of the start circle
route_csc.start.angle = 90.0 - route_csc.tangent.angle;
// 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;
}
route_csc.start.angle = (Angle::frac_pi_2() - route_csc.tangent.angle).positive();
// get the tangent origin by moving the vector from the start circle center
// along its right angle vector
route_csc.tangent.origin = Point {
x: route_csc.start.circle.center.x
+ route_csc.start.circle.radius * (180.0 - route_csc.start.angle).to_radians().cos(),
+ route_csc.start.circle.radius * (Angle::pi() - route_csc.start.angle).get().cos(),
y: route_csc.start.circle.center.y
+ route_csc.start.circle.radius * (180.0 - route_csc.start.angle).to_radians().sin(),
+ route_csc.start.circle.radius * (Angle::pi() - route_csc.start.angle).get().sin(),
};
// get the angle of the end circle
route_csc.end.angle = (90.0 - end.angle) - route_csc.tangent.angle;
// 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;
}
route_csc.end.angle = ((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive();
Ok(route_csc)
}
@ -361,11 +312,11 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
},
radius: end.magnitude,
},
angle: 0.0,
angle: Angle::zero(),
},
tangent: Vector {
origin: Point { x: 0.0, y: 0.0 },
angle: 0.0,
angle: Angle::zero(),
magnitude: 0.0,
},
end: CircleRoute {
@ -373,17 +324,17 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
center: Point { x: 0.0, y: 0.0 },
radius: end.magnitude,
},
angle: 0.0,
angle: Angle::zero(),
},
};
// 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
// 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 {
x: end.origin.x + end.magnitude * (360.0 - end.angle).to_radians().cos(),
y: end.origin.y + end.magnitude * (360.0 - end.angle).to_radians().sin(),
x: end.origin.x + end.magnitude * (Angle::two_pi() - end.angle).get().cos(),
y: end.origin.y + end.magnitude * (Angle::two_pi() - end.angle).get().sin(),
};
// check if inside tangent can even be constructed
@ -409,51 +360,34 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
};
// 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.y - tangent_middle.y)
/ (route_csc.end.circle.center.x - tangent_middle.x))
.atan()
.to_degrees()
+ (2.0 * end.magnitude / route_csc.tangent.magnitude)
.atan()
.to_degrees();
+ (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(),
);
// if the end circle center x value is smaller than the
// start circle center x value
// the angle would be 180° rotated so to prevent that:
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
route_csc.start.angle = route_csc.tangent.angle - 90.0;
// 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;
}
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 = Point {
x: route_csc.start.circle.center.x
+ route_csc.start.circle.radius * route_csc.start.angle.to_radians().cos(),
+ route_csc.start.circle.radius * route_csc.start.angle.get().cos(),
y: route_csc.start.circle.center.y
+ route_csc.start.circle.radius * route_csc.start.angle.to_radians().sin(),
+ route_csc.start.circle.radius * route_csc.start.angle.get().sin(),
};
// get the angle of the end circle
route_csc.end.angle = (90.0 - end.angle) - route_csc.tangent.angle;
// 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;
}
route_csc.end.angle = ((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive();
Ok(route_csc)
}

Write Preview
Loading…
Cancel
Save