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

Use Impl for the RouteCSC struct

Browse Source
master
Roman Kretschmer 6 years ago
parent a7747c0c4c
commit c0361d11b9
Signed by: gnxlxnxx
GPG Key ID: E4EAB482427FA3A0
1 changed files with 287 additions and 265 deletions
Show Stats Download Patch File Download Diff File

552
src/lib.rs
Unescape View File

@ -43,279 +43,301 @@ pub struct RouteCCC {
pub end: CircleVector, pub end: CircleVector,
} }
/// right straight right route /// Route with a start Circle, a tangent straight and a end Circle
pub fn rsr(end: Vector) -> Result<RouteCSC, ()> { impl RouteCSC {
let mut route_csc = RouteCSC { /// right straight right route
start: CircleVector { pub fn rsr(end: Vector) -> Result<Self, ()> {
center: Point::new(end.magnitude, 0.0), let mut route_csc = RouteCSC {
radius: end.magnitude, start: CircleVector {
angle: Angle::zero(), center: Point::new(end.magnitude, 0.0),
}, radius: end.magnitude,
tangent: Vector { angle: Angle::zero(),
origin: Point::zero(), },
angle: Angle::zero(), tangent: Vector {
magnitude: 0.0, origin: Point::zero(),
}, angle: Angle::zero(),
end: CircleVector { magnitude: 0.0,
center: Point::zero(), },
radius: end.magnitude, end: CircleVector {
angle: Angle::zero(), center: Point::zero(),
}, radius: end.magnitude,
}; 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 // get the center point by adding the end vector to the end point
// the angle has to be counter clockwise though (thats why we use the inverse end.angle) // this works because the argument is the angle in positive y direction
route_csc.end.center = end.origin // not positive x direction so we dont have to rotate it here anymore
+ Rotation::new(end.angle) // the angle has to be counter clockwise though (thats why we use the inverse end.angle)
.inverse() route_csc.end.center = end.origin
.transform_vector(Vector2D::new(end.magnitude, 0.0)); + Rotation::new(end.angle)
.inverse()
// get the tangent pitch which is the same as the pitch between the two .transform_vector(Vector2D::new(end.magnitude, 0.0));
// circle centers since our circles have the same radius
route_csc.tangent.angle = Angle::radians( // get the tangent pitch which is the same as the pitch between the two
((route_csc.end.center.y - route_csc.start.center.y) // circle centers since our circles have the same radius
/ (route_csc.end.center.x - route_csc.start.center.x)) route_csc.tangent.angle = Angle::radians(
.atan(), ((route_csc.end.center.y - route_csc.start.center.y)
); / (route_csc.end.center.x - route_csc.start.center.x))
.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 the end circle center x value is smaller than the
if route_csc.end.center.x < route_csc.start.center.x { // start circle center x value
route_csc.tangent.angle += Angle::pi(); // the angle would be 180° rotated so to prevent that:
if route_csc.end.center.x < route_csc.start.center.x {
route_csc.tangent.angle += Angle::pi();
}
// get the tangent magnitude this, again, is the same as the distance
// between the two circle centers since our circles have the same radius
route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x).powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2))
.sqrt();
// get the angle of the start circle
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 = route_csc.start.center
+ Rotation::new(Angle::pi() - end.angle)
.transform_vector(Vector2D::new(route_csc.start.radius, 0.0));
// 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).positive();
Ok(route_csc)
} }
// get the tangent magnitude this, again, is the same as the distance /// left straight left route
// between the two circle centers since our circles have the same radius pub fn lsl(end: Vector) -> Result<Self, ()> {
route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x).powi(2) let mut route_csc = RouteCSC {
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2)) start: CircleVector {
.sqrt(); center: Point::new(-end.magnitude, 0.0),
radius: end.magnitude,
// get the angle of the start circle angle: Angle::zero(),
route_csc.start.angle = (Angle::frac_pi_2() - route_csc.tangent.angle).positive(); },
tangent: Vector {
// get the tangent origin by moving the vector from the start circle center origin: Point::zero(),
// 90° to it's own direction and the magnitude of the circle radius angle: Angle::zero(),
route_csc.tangent.origin = route_csc.start.center magnitude: 0.0,
+ Rotation::new(Angle::pi() - end.angle) },
.transform_vector(Vector2D::new(route_csc.start.radius, 0.0)); end: CircleVector {
center: Point::zero(),
// get the angle of the start circle radius: end.magnitude,
// the angle where we start from the tangent equals the one we finish angle: Angle::zero(),
// so we can use that in here },
route_csc.end.angle = (end.angle - route_csc.start.angle).positive(); };
Ok(route_csc) // get the center point by adding the end vector to the end 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)
/// left straight left route // and again we have to use the counter clockwise direction
pub fn lsl(end: Vector) -> Result<RouteCSC, ()> { route_csc.end.center = end.origin
let mut route_csc = RouteCSC { + Rotation::new(Angle::pi() - end.angle)
start: CircleVector { .transform_vector(Vector2D::new(end.magnitude, 0.0));
center: Point::new(-end.magnitude, 0.0),
radius: end.magnitude, // get the tangent pitch which is the same as the pitch between the two
angle: Angle::zero(), // circle centers since our circles have the same radius
}, route_csc.tangent.angle = Angle::radians(
tangent: Vector { ((route_csc.end.center.y - route_csc.start.center.y)
origin: Point::zero(), / (route_csc.end.center.x - route_csc.start.center.x))
angle: Angle::zero(), .atan(),
magnitude: 0.0, )
}, .positive();
end: CircleVector {
center: Point::zero(), // if the end circle center x value is smaller than the
radius: end.magnitude, // start circle center x value
angle: Angle::zero(), // the angle would be π rotated so to prevent that:
}, if route_csc.end.center.x < route_csc.start.center.x {
}; route_csc.tangent.angle = (route_csc.tangent.angle + Angle::pi()).positive();
}
// get the center point by adding the end vector to the end point
// we have to rotate the vector π (π/2 because the given angle is from the y axis // get the tangent magnitude this, again, is the same as the distance
// and π/2 more to not get the tangent but the vector to the center point) // between the two circle centers since our circles have the same radius
// and again we have to use the counter clockwise direction route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x)
route_csc.end.center = end.origin
+ Rotation::new(Angle::pi() - end.angle)
.transform_vector(Vector2D::new(end.magnitude, 0.0));
// 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 = Angle::radians(
((route_csc.end.center.y - route_csc.start.center.y)
/ (route_csc.end.center.x - route_csc.start.center.x))
.atan(),
)
.positive();
// if the end circle center x value is smaller than the
// start circle center x value
// the angle would be π rotated so to prevent that:
if route_csc.end.center.x < route_csc.start.center.x {
route_csc.tangent.angle = (route_csc.tangent.angle + Angle::pi()).positive();
}
// get the tangent magnitude this, again, is the same as the distance
// between the two circle centers since our circles have the same radius
route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x)
.abs()
.powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y)
.abs() .abs()
.powi(2)) .powi(2)
.sqrt(); + (route_csc.end.center.y - route_csc.start.center.y)
.abs()
// get the angle of the start circle .powi(2))
route_csc.start.angle = (route_csc.tangent.angle - Angle::frac_pi_2()).positive(); .sqrt();
// get the tangent origin by moving the vector from the start circle center // get the angle of the start circle
// 90° to it's own direction and the magnitude of the circle radius route_csc.start.angle = (route_csc.tangent.angle - Angle::frac_pi_2()).positive();
route_csc.tangent.origin = route_csc.start.center
+ Rotation::new(route_csc.start.angle) // get the tangent origin by moving the vector from the start circle center
.transform_vector(Vector2D::new(route_csc.start.radius, 0.0)); // 90° to it's own direction and the magnitude of the circle radius
route_csc.tangent.origin = route_csc.start.center
// get the angle of the start circle + Rotation::new(route_csc.start.angle)
// the angle where we start from the tangent equals the one we finish .transform_vector(Vector2D::new(route_csc.start.radius, 0.0));
// so we can use that in here
route_csc.end.angle = (end.angle - route_csc.start.angle).positive(); // get the angle of the start circle
// the angle where we start from the tangent equals the one we finish
Ok(route_csc) // so we can use that in here
} route_csc.end.angle = (end.angle - route_csc.start.angle).positive();
/// right straight left route Ok(route_csc)
pub fn rsl(end: Vector) -> Result<RouteCSC, ()> {
let mut route_csc = RouteCSC {
start: CircleVector {
center: Point::new(end.magnitude, 0.0),
radius: end.magnitude,
angle: Angle::zero(),
},
tangent: Vector {
origin: Point::zero(),
angle: Angle::zero(),
magnitude: 0.0,
},
end: CircleVector {
center: Point::zero(),
radius: end.magnitude,
angle: Angle::zero(),
},
};
// get the center point by adding the end vector to the end 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.center = end.origin
+ Rotation::new(Angle::pi() - end.angle)
.transform_vector(Vector2D::new(end.magnitude, 0.0));
// check if inside tangent can even be constructed
if ((route_csc.end.center.x - route_csc.start.center.x).powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2))
.sqrt()
< 2.0 * end.magnitude
{
return Err(());
}
// get the tangent length via some simple trigonometry
route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x).powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2)
- (2.0 * end.magnitude).powi(2))
.sqrt();
// tangent middle is the same as the middle of the straight from the center of the start
let tangent_middle = route_csc.end.center.lerp(route_csc.start.center, 0.5);
// 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))
.atan()
- (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 π rotated so to prevent that:
if route_csc.end.center.x < route_csc.start.center.x {
route_csc.tangent.angle += Angle::pi();
} }
// get the angle of the start circle /// right straight left route
route_csc.start.angle = (Angle::frac_pi_2() - route_csc.tangent.angle).positive(); pub fn rsl(end: Vector) -> Result<Self, ()> {
let mut route_csc = RouteCSC {
// get the tangent origin by moving the vector from the start circle center start: CircleVector {
// along its right angle vector center: Point::new(end.magnitude, 0.0),
route_csc.tangent.origin = route_csc.start.center radius: end.magnitude,
+ Rotation::new(Angle::pi() - route_csc.start.angle) angle: Angle::zero(),
.transform_vector(Vector2D::new(route_csc.start.radius, 0.0)); },
tangent: Vector {
// get the angle of the end circle origin: Point::zero(),
route_csc.end.angle = ((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive(); angle: Angle::zero(),
magnitude: 0.0,
Ok(route_csc) },
} end: CircleVector {
center: Point::zero(),
/// left straight right route radius: end.magnitude,
pub fn lsr(end: Vector) -> Result<RouteCSC, ()> { angle: Angle::zero(),
let mut route_csc = RouteCSC { },
start: CircleVector { };
center: Point::new(-end.magnitude, 0.0),
radius: end.magnitude, // get the center point by adding the end vector to the end point
angle: Angle::zero(), // 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)
tangent: Vector { // and again we have to use the counter clockwise direction
origin: Point::zero(), route_csc.end.center = end.origin
angle: Angle::zero(), + Rotation::new(Angle::pi() - end.angle)
magnitude: 0.0, .transform_vector(Vector2D::new(end.magnitude, 0.0));
},
end: CircleVector { // check if inside tangent can even be constructed
center: Point::zero(), if ((route_csc.end.center.x - route_csc.start.center.x).powi(2)
radius: end.magnitude, + (route_csc.end.center.y - route_csc.start.center.y).powi(2))
angle: Angle::zero(), .sqrt()
}, < 2.0 * end.magnitude
}; {
return Err(());
// 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 // get the tangent length via some simple trigonometry
// the angle has to be counter clockwise though (thats why 2π - end.angle) route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x).powi(2)
route_csc.end.center = end.origin + (route_csc.end.center.y - route_csc.start.center.y).powi(2)
+ Rotation::new(end.angle) - (2.0 * end.magnitude).powi(2))
.inverse() .sqrt();
.transform_vector(Vector2D::new(end.magnitude, 0.0));
// tangent middle is the same as the middle of the straight from the center of the start
// check if inside tangent can even be constructed let tangent_middle = route_csc.end.center.lerp(route_csc.start.center, 0.5);
if ((route_csc.end.center.x - route_csc.start.center.x).powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2)) // get the tangent angle
.sqrt() route_csc.tangent.angle = Angle::radians(
< 2.0 * end.magnitude ((route_csc.end.center.y - tangent_middle.y)
{ / (route_csc.end.center.x - tangent_middle.x))
return Err(()); .atan()
- (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 π rotated so to prevent that:
if route_csc.end.center.x < route_csc.start.center.x {
route_csc.tangent.angle += Angle::pi();
}
// get the angle of the start circle
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 = route_csc.start.center
+ Rotation::new(Angle::pi() - 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 tangent length via some simple trigonometry /// left straight right route
route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x).powi(2) pub fn lsr(end: Vector) -> Result<Self, ()> {
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2) let mut route_csc = RouteCSC {
- (2.0 * end.magnitude).powi(2)) start: CircleVector {
.sqrt(); center: Point::new(-end.magnitude, 0.0),
radius: end.magnitude,
// tangent middle is the same as the middle of the straight from the center of the start angle: Angle::zero(),
let tangent_middle = route_csc.end.center.lerp(route_csc.start.center, 0.5); },
tangent: Vector {
// get the tangent angle origin: Point::zero(),
route_csc.tangent.angle = Angle::radians( angle: Angle::zero(),
((route_csc.end.center.y - tangent_middle.y) / (route_csc.end.center.x - tangent_middle.x)) magnitude: 0.0,
.atan() },
+ (2.0 * end.magnitude / route_csc.tangent.magnitude).atan(), end: CircleVector {
); center: Point::zero(),
radius: end.magnitude,
// if the end circle center x value is smaller than the angle: Angle::zero(),
// start circle center x value },
// the angle would be 180° rotated so to prevent that: };
if route_csc.end.center.x < route_csc.start.center.x {
route_csc.tangent.angle += Angle::pi(); // 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 2π - end.angle)
route_csc.end.center = end.origin
+ Rotation::new(end.angle)
.inverse()
.transform_vector(Vector2D::new(end.magnitude, 0.0));
// check if inside tangent can even be constructed
if ((route_csc.end.center.x - route_csc.start.center.x).powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2))
.sqrt()
< 2.0 * end.magnitude
{
return Err(());
}
// get the tangent length via some simple trigonometry
route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x).powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2)
- (2.0 * end.magnitude).powi(2))
.sqrt();
// tangent middle is the same as the middle of the straight from the center of the start
let tangent_middle = route_csc.end.center.lerp(route_csc.start.center, 0.5);
// 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))
.atan()
+ (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.center.x < route_csc.start.center.x {
route_csc.tangent.angle += Angle::pi();
}
// 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 // get the angle of the start circle
route_csc.start.angle = (route_csc.tangent.angle - Angle::frac_pi_2()).positive(); route_csc.start.angle = (route_csc.tangent.angle - Angle::frac_pi_2()).positive();

Write Preview
Loading…
Cancel
Save