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

Use euclide `Point2D`

Browse Source
pull/1/head
David Sawatzke 6 years ago
parent a80b6bc1d9
commit 788ace12c8
1 changed files with 53 additions and 79 deletions
Show Stats Download Patch File Download Diff File

132
src/lib.rs
Unescape View File

@ -7,13 +7,11 @@
//! //!
//! Every struct defined here is 2 dimensional and uses f64 //! Every struct defined here is 2 dimensional and uses f64
use euclid::Angle; use euclid::{Angle, Point2D, Rotation2D, UnknownUnit};
/// Point type Point = Point2D<f64, UnknownUnit>;
pub struct Point { type Vector2D = euclid::Vector2D<f64, UnknownUnit>;
pub x: f64, type Rotation = Rotation2D<f64, UnknownUnit, UnknownUnit>;
pub y: f64,
}
/// Vector with origin, angle and magnitude /// Vector with origin, angle and magnitude
pub struct Vector { pub struct Vector {
@ -54,22 +52,19 @@ 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: Angle::zero(), angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
origin: Point { x: 0.0, y: 0.0 }, origin: Point::zero(),
angle: Angle::zero(), 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: Angle::zero(), angle: Angle::zero(),
@ -79,11 +74,11 @@ pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
// 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 2π - 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 * (Angle::two_pi() - end.angle).get().cos(), + Rotation::new(end.angle)
y: end.origin.y + end.magnitude * (Angle::two_pi() - end.angle).get().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
@ -112,12 +107,9 @@ pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
// 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 * (Angle::pi() - route_csc.start.angle).get().cos(), .transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0));
y: route_csc.start.circle.center.y
+ route_csc.start.circle.radius * (Angle::pi() - route_csc.start.angle).get().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
@ -132,22 +124,19 @@ 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: Angle::zero(), angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
origin: Point { x: 0.0, y: 0.0 }, origin: Point::zero(),
angle: Angle::zero(), 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: Angle::zero(), angle: Angle::zero(),
@ -158,10 +147,9 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
// we have to rotate the vector π (π/2 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 π/2 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 * (Angle::pi() - end.angle).get().cos(), + Rotation::new(Angle::pi() - end.angle)
y: end.origin.y + end.magnitude * (Angle::pi() - end.angle).get().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
@ -195,12 +183,9 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
// 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.get().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.get().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
@ -215,22 +200,19 @@ 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: Angle::zero(), angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
origin: Point { x: 0.0, y: 0.0 }, origin: Point::zero(),
angle: Angle::zero(), 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: Angle::zero(), angle: Angle::zero(),
@ -241,10 +223,9 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
// we have to rotate the vector π (π/2 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 π/2 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 * (Angle::pi() - end.angle).get().cos(), + Rotation::new(Angle::pi() - end.angle)
y: end.origin.y + end.magnitude * (Angle::pi() - end.angle).get().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).powi(2) if ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2)
@ -263,10 +244,11 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
.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 = Angle::radians( route_csc.tangent.angle = Angle::radians(
@ -288,12 +270,9 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
// 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 * (Angle::pi() - route_csc.start.angle).get().cos(), .transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0));
y: route_csc.start.circle.center.y
+ route_csc.start.circle.radius * (Angle::pi() - route_csc.start.angle).get().sin(),
};
// get the angle of the end circle // 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();
@ -306,22 +285,19 @@ 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: Angle::zero(), angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
origin: Point { x: 0.0, y: 0.0 }, origin: Point::zero(),
angle: Angle::zero(), 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: Angle::zero(), angle: Angle::zero(),
@ -332,10 +308,10 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
// 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 2π - 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 * (Angle::two_pi() - end.angle).get().cos(), + Rotation::new(end.angle)
y: end.origin.y + end.magnitude * (Angle::two_pi() - end.angle).get().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).powi(2) if ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2)
@ -354,10 +330,11 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
.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 = Angle::radians( route_csc.tangent.angle = Angle::radians(
@ -379,12 +356,9 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
// 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.get().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.get().sin(),
};
// get the angle of the end circle // 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();

Write Preview
Loading…
Cancel
Save