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

Use CircleVector to simplify stuff

Browse Source
master
Roman Kretschmer 6 years ago
parent 3b3d5430bd
commit 6bb9dc8f8c
Signed by: gnxlxnxx
GPG Key ID: E4EAB482427FA3A0
1 changed files with 72 additions and 107 deletions
Show Stats Download Patch File Download Diff File

137
src/lib.rs
Unescape View File

@ -21,41 +21,34 @@ pub struct Vector {
pub magnitude: f64, pub magnitude: f64,
} }
/// Circle /// Circle vector (Circle + Angle)
pub struct Circle { pub struct CircleVector {
pub center: Point, pub center: Point,
pub radius: f64, pub radius: f64,
}
/// Circle route with a circle and a angle for how long to drive on this circle
pub struct CircleRoute {
pub circle: Circle,
pub angle: Angle, 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)
pub struct RouteCSC { pub struct RouteCSC {
pub start: CircleRoute, pub start: CircleVector,
pub tangent: Vector, pub tangent: Vector,
pub end: CircleRoute, pub end: CircleVector,
} }
/// Route with 3 Circles (eg. rlr, lrl) (not yet implemented) /// Route with 3 Circles (eg. rlr, lrl) (not yet implemented)
#[allow(unused)] #[allow(unused)]
pub struct RouteCCC { pub struct RouteCCC {
pub start: Circle, pub start: CircleVector,
pub middle: Circle, pub middle: CircleVector,
pub end: Circle, pub end: CircleVector,
} }
/// right straight right route /// right straight right route
pub fn rsr(end: Vector) -> Result<RouteCSC, ()> { pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
let mut route_csc = RouteCSC { let mut route_csc = RouteCSC {
start: CircleRoute { start: CircleVector {
circle: Circle {
center: Point::new(end.magnitude, 0.0), center: Point::new(end.magnitude, 0.0),
radius: end.magnitude, radius: end.magnitude,
},
angle: Angle::zero(), angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
@ -63,11 +56,9 @@ pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
angle: Angle::zero(), angle: Angle::zero(),
magnitude: 0.0, magnitude: 0.0,
}, },
end: CircleRoute { end: CircleVector {
circle: Circle {
center: Point::zero(), center: Point::zero(),
radius: end.magnitude, radius: end.magnitude,
},
angle: Angle::zero(), angle: Angle::zero(),
}, },
}; };
@ -76,7 +67,7 @@ pub fn rsr(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 we use the inverse end.angle) // the angle has to be counter clockwise though (thats why we use the inverse end.angle)
route_csc.end.circle.center = end.origin route_csc.end.center = end.origin
+ Rotation::new(end.angle) + Rotation::new(end.angle)
.inverse() .inverse()
.transform_vector(Vector2D::new(end.magnitude, 0.0)); .transform_vector(Vector2D::new(end.magnitude, 0.0));
@ -84,23 +75,22 @@ pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
// 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 = Angle::radians( route_csc.tangent.angle = Angle::radians(
((route_csc.end.circle.center.y - route_csc.start.circle.center.y) ((route_csc.end.center.y - route_csc.start.center.y)
/ (route_csc.end.circle.center.x - route_csc.start.circle.center.x)) / (route_csc.end.center.x - route_csc.start.center.x))
.atan(), .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.center.x < route_csc.start.center.x {
route_csc.tangent.angle += Angle::pi(); 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.center.x - route_csc.start.center.x).powi(2)
((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2) + (route_csc.end.center.y - route_csc.start.center.y).powi(2))
+ (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
@ -108,9 +98,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 = route_csc.start.circle.center route_csc.tangent.origin = route_csc.start.center
+ Rotation::new(Angle::pi() - end.angle) + Rotation::new(Angle::pi() - end.angle)
.transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0)); .transform_vector(Vector2D::new(route_csc.start.radius, 0.0));
// 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
@ -123,11 +113,9 @@ pub fn rsr(end: Vector) -> Result<RouteCSC, ()> {
/// left straight left route /// left straight left route
pub fn lsl(end: Vector) -> Result<RouteCSC, ()> { pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
let mut route_csc = RouteCSC { let mut route_csc = RouteCSC {
start: CircleRoute { start: CircleVector {
circle: Circle {
center: Point::new(-end.magnitude, 0.0), center: Point::new(-end.magnitude, 0.0),
radius: end.magnitude, radius: end.magnitude,
},
angle: Angle::zero(), angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
@ -135,11 +123,9 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
angle: Angle::zero(), angle: Angle::zero(),
magnitude: 0.0, magnitude: 0.0,
}, },
end: CircleRoute { end: CircleVector {
circle: Circle {
center: Point::zero(), center: Point::zero(),
radius: end.magnitude, radius: end.magnitude,
},
angle: Angle::zero(), angle: Angle::zero(),
}, },
}; };
@ -148,15 +134,15 @@ 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 = end.origin route_csc.end.center = end.origin
+ Rotation::new(Angle::pi() - end.angle) + Rotation::new(Angle::pi() - end.angle)
.transform_vector(Vector2D::new(end.magnitude, 0.0)); .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 = Angle::radians( route_csc.tangent.angle = Angle::radians(
((route_csc.end.circle.center.y - route_csc.start.circle.center.y) ((route_csc.end.center.y - route_csc.start.center.y)
/ (route_csc.end.circle.center.x - route_csc.start.circle.center.x)) / (route_csc.end.center.x - route_csc.start.center.x))
.atan(), .atan(),
) )
.positive(); .positive();
@ -164,17 +150,16 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
// 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 π 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.center.x < route_csc.start.center.x {
route_csc.tangent.angle = (route_csc.tangent.angle + Angle::pi()).positive(); route_csc.tangent.angle = (route_csc.tangent.angle + Angle::pi()).positive();
} }
// 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.end.circle.center.x route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x)
- route_csc.start.circle.center.x)
.abs() .abs()
.powi(2) .powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y) + (route_csc.end.center.y - route_csc.start.center.y)
.abs() .abs()
.powi(2)) .powi(2))
.sqrt(); .sqrt();
@ -184,9 +169,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 = route_csc.start.circle.center route_csc.tangent.origin = route_csc.start.center
+ Rotation::new(route_csc.start.angle) + Rotation::new(route_csc.start.angle)
.transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0)); .transform_vector(Vector2D::new(route_csc.start.radius, 0.0));
// 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
@ -199,11 +184,9 @@ pub fn lsl(end: Vector) -> Result<RouteCSC, ()> {
/// right straight left route /// right straight left route
pub fn rsl(end: Vector) -> Result<RouteCSC, ()> { pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
let mut route_csc = RouteCSC { let mut route_csc = RouteCSC {
start: CircleRoute { start: CircleVector {
circle: Circle {
center: Point::new(end.magnitude, 0.0), center: Point::new(end.magnitude, 0.0),
radius: end.magnitude, radius: end.magnitude,
},
angle: Angle::zero(), angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
@ -211,11 +194,9 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
angle: Angle::zero(), angle: Angle::zero(),
magnitude: 0.0, magnitude: 0.0,
}, },
end: CircleRoute { end: CircleVector {
circle: Circle {
center: Point::zero(), center: Point::zero(),
radius: end.magnitude, radius: end.magnitude,
},
angle: Angle::zero(), angle: Angle::zero(),
}, },
}; };
@ -224,13 +205,13 @@ 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 = end.origin route_csc.end.center = end.origin
+ Rotation::new(Angle::pi() - end.angle) + Rotation::new(Angle::pi() - end.angle)
.transform_vector(Vector2D::new(end.magnitude, 0.0)); .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.center.x - route_csc.start.center.x).powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2)) + (route_csc.end.center.y - route_csc.start.center.y).powi(2))
.sqrt() .sqrt()
< 2.0 * end.magnitude < 2.0 * end.magnitude
{ {
@ -238,23 +219,17 @@ 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.center.x - route_csc.start.center.x).powi(2)
((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2) + (route_csc.end.center.y - route_csc.start.center.y).powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2)
- (2.0 * end.magnitude).powi(2)) - (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 = route_csc let tangent_middle = route_csc.end.center.lerp(route_csc.start.center, 0.5);
.end
.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(
((route_csc.end.circle.center.y - tangent_middle.y) ((route_csc.end.center.y - tangent_middle.y) / (route_csc.end.center.x - tangent_middle.x))
/ (route_csc.end.circle.center.x - tangent_middle.x))
.atan() .atan()
- (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(), - (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(),
); );
@ -262,7 +237,7 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
// 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 π 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.center.x < route_csc.start.center.x {
route_csc.tangent.angle += Angle::pi(); route_csc.tangent.angle += Angle::pi();
} }
@ -271,9 +246,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 = route_csc.start.circle.center route_csc.tangent.origin = route_csc.start.center
+ Rotation::new(Angle::pi() - route_csc.start.angle) + Rotation::new(Angle::pi() - route_csc.start.angle)
.transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0)); .transform_vector(Vector2D::new(route_csc.start.radius, 0.0));
// 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();
@ -284,11 +259,9 @@ pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
/// left straight right route /// left straight right route
pub fn lsr(end: Vector) -> Result<RouteCSC, ()> { pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
let mut route_csc = RouteCSC { let mut route_csc = RouteCSC {
start: CircleRoute { start: CircleVector {
circle: Circle {
center: Point::new(-end.magnitude, 0.0), center: Point::new(-end.magnitude, 0.0),
radius: end.magnitude, radius: end.magnitude,
},
angle: Angle::zero(), angle: Angle::zero(),
}, },
tangent: Vector { tangent: Vector {
@ -296,11 +269,9 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
angle: Angle::zero(), angle: Angle::zero(),
magnitude: 0.0, magnitude: 0.0,
}, },
end: CircleRoute { end: CircleVector {
circle: Circle {
center: Point::zero(), center: Point::zero(),
radius: end.magnitude, radius: end.magnitude,
},
angle: Angle::zero(), angle: Angle::zero(),
}, },
}; };
@ -309,14 +280,14 @@ 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 = end.origin route_csc.end.center = end.origin
+ Rotation::new(end.angle) + Rotation::new(end.angle)
.inverse() .inverse()
.transform_vector(Vector2D::new(end.magnitude, 0.0)); .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.center.x - route_csc.start.center.x).powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2)) + (route_csc.end.center.y - route_csc.start.center.y).powi(2))
.sqrt() .sqrt()
< 2.0 * end.magnitude < 2.0 * end.magnitude
{ {
@ -324,23 +295,17 @@ 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.center.x - route_csc.start.center.x).powi(2)
((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2) + (route_csc.end.center.y - route_csc.start.center.y).powi(2)
+ (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2)
- (2.0 * end.magnitude).powi(2)) - (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 = route_csc let tangent_middle = route_csc.end.center.lerp(route_csc.start.center, 0.5);
.end
.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(
((route_csc.end.circle.center.y - tangent_middle.y) ((route_csc.end.center.y - tangent_middle.y) / (route_csc.end.center.x - tangent_middle.x))
/ (route_csc.end.circle.center.x - tangent_middle.x))
.atan() .atan()
+ (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(), + (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(),
); );
@ -348,7 +313,7 @@ pub fn lsr(end: Vector) -> Result<RouteCSC, ()> {
// 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.center.x < route_csc.start.center.x {
route_csc.tangent.angle += Angle::pi(); route_csc.tangent.angle += Angle::pi();
} }
@ -357,9 +322,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 = route_csc.start.circle.center route_csc.tangent.origin = route_csc.start.center
+ Rotation::new(route_csc.start.angle) + Rotation::new(route_csc.start.angle)
.transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0)); .transform_vector(Vector2D::new(route_csc.start.radius, 0.0));
// 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