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,
}
/// right straight right route
pub fn rsr(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
// 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 we use the inverse end.angle)
route_csc.end.center = end.origin
+ Rotation::new(end.angle)
.inverse()
.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(),
);
// 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();
/// Route with a start Circle, a tangent straight and a end Circle
impl RouteCSC {
/// right straight right route
pub fn rsr(end: Vector) -> Result<Self, ()> {
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
// 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 we use the inverse end.angle)
route_csc.end.center = end.origin
+ Rotation::new(end.angle)
.inverse()
.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(),
);
// 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 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
// 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)
}
/// left straight left route
pub fn lsl(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));
// 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)
/// left straight left route
pub fn lsl(end: Vector) -> Result<Self, ()> {
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));
// 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))
.sqrt();
// 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 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)
}
/// right straight left route
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();
.powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y)
.abs()
.powi(2))
.sqrt();
// 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 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 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)
}
/// left straight right route
pub fn lsr(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
// 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(());
/// right straight left route
pub fn rsl(end: Vector) -> Result<Self, ()> {
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
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
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();
/// left straight right route
pub fn lsr(end: Vector) -> Result<Self, ()> {
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
// 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
route_csc.start.angle = (route_csc.tangent.angle - Angle::frac_pi_2()).positive();

Write Preview
Loading…
Cancel
Save