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

Remove giant mut, rename structs

Browse Source 
Renamed Vector and CircleVector to StraightPath and CirclePath to be less confusing with the euclid stuff
Use local variables instead of giant mut at the beginning of the function
Use independant arguments
master
Roman Kretschmer 6 years ago
parent 39d6ec6694
commit a68159b997
Signed by: gnxlxnxx
GPG Key ID: E4EAB482427FA3A0
1 changed files with 246 additions and 302 deletions
Show Stats Download Patch File Download Diff File

548
src/lib.rs
Unescape View File

@ -25,43 +25,33 @@ pub enum Error {
/// Vector with origin, angle and magnitude /// Vector with origin, angle and magnitude
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub struct Vector { pub struct StraightPath {
pub origin: Point, pub origin: Point,
pub angle: Angle, pub vector: Vector,
pub magnitude: f64,
} }
impl Vector { impl StraightPath {
/// approximate equality to other Vector /// approximate equality to other Vector
pub fn approx_eq(&self, other: Self) -> bool { pub fn approx_eq(&self, other: Self) -> bool {
if !ApproxEq::approx_eq(&self.origin, &other.origin) { ApproxEq::approx_eq(&self.vector, &other.vector)
false && ApproxEq::approx_eq(&self.origin, &other.origin)
} else if !ApproxEq::approx_eq(&self.magnitude, &other.magnitude) {
false
} else if !(ApproxEq::approx_eq(&self.angle, &other.angle)
|| ApproxEq::approx_eq(&self.angle.signed(), &other.angle.signed()))
{
false
} else {
true
}
} }
} }
/// Circle vector (Circle + Angle) /// Circle vector (Circle + Angle)
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub struct CircleVector { pub struct CirclePath {
pub center: Point, pub center: Point,
pub radius: f64, pub radius: f64,
pub angle: Angle, pub angle: Angle,
} }
impl CircleVector { impl CirclePath {
///get the length of the circle vector ///get the length of the circle vector
pub fn get_length(&self) -> f64 { pub fn get_length(&self) -> f64 {
self.angle.radians * self.radius self.angle.radians * self.radius
} }
/// approximate equality to other CircleVector /// approximate equality to other CirclePath
pub fn approx_eq(&self, other: Self) -> bool { pub fn approx_eq(&self, other: Self) -> bool {
if !ApproxEq::approx_eq(&self.center, &other.center) { if !ApproxEq::approx_eq(&self.center, &other.center) {
false false
@ -80,17 +70,17 @@ impl CircleVector {
/// Route with a start Circle, a tangent straight and a end Circle /// Route with a start Circle, a tangent straight and a end Circle
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub struct RouteCSC { pub struct RouteCSC {
pub start: CircleVector, pub start: CirclePath,
pub tangent: Vector, pub tangent: StraightPath,
pub end: CircleVector, pub end: CirclePath,
} }
/// Route with 3 Circles /// Route with 3 Circles
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
pub struct RouteCCC { pub struct RouteCCC {
pub start: CircleVector, pub start: CirclePath,
pub middle: CircleVector, pub middle: CirclePath,
pub end: CircleVector, pub end: CirclePath,
} }
#[derive(Debug, Copy, Clone)] #[derive(Debug, Copy, Clone)]
@ -102,311 +92,285 @@ pub enum Path {
/// Route with a start Circle, a tangent straight and a end Circle /// Route with a start Circle, a tangent straight and a end Circle
impl RouteCSC { impl RouteCSC {
/// right straight right route /// right straight right route
pub fn rsr(end: Vector) -> Result<Self, Error> { pub fn rsr(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
let mut route_csc = RouteCSC { let start_center = Point::new(radius, 0.0);
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 // 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 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.center = end.origin let end_center = end_point
+ Rotation::new(end.angle) + Rotation::new(end_angle)
.inverse() .inverse()
.transform_vector(Vector2D::new(end.magnitude, 0.0)); .transform_vector(Vector::new(radius, 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( let mut tangent_angle = Angle::radians(
((route_csc.end.center.y - route_csc.start.center.y) ((end_center.y - start_center.y) / (end_center.x - start_center.x)).atan(),
/ (route_csc.end.center.x - route_csc.start.center.x))
.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 rotated by π so to prevent that: // the angle would be rotated by π so to prevent that:
if route_csc.end.center.x < route_csc.start.center.x { if end_center.x < start_center.x {
route_csc.tangent.angle += Angle::pi(); 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.end.center.x - route_csc.start.center.x).powi(2) let tangent_magnitude = ((end_center.x - start_center.x).powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2)) + (end_center.y - start_center.y).powi(2))
.sqrt(); .sqrt();
// get the angle of the start circle // get the angle of the start circle
route_csc.start.angle = (Angle::frac_pi_2() - route_csc.tangent.angle).positive(); let start_angle = (Angle::frac_pi_2() - tangent_angle).positive();
// 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
// π/2 to it's own direction and the magnitude of the circle radius // π/2 to it's own direction and the magnitude of the circle radius
route_csc.tangent.origin = route_csc.start.center let tangent_origin = start_center
+ Rotation::new(Angle::pi() - end.angle) + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(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
// so we can use that in here // so we can use that in here
route_csc.end.angle = (end.angle - route_csc.start.angle).positive(); let end_angle = (end_angle - start_angle).positive();
Ok(route_csc)
}
/// left straight left route Ok(Self {
pub fn lsl(end: Vector) -> Result<Self, Error> { start: CirclePath {
let mut route_csc = RouteCSC { center: start_center,
start: CircleVector { radius: radius,
center: Point::new(-end.magnitude, 0.0), angle: start_angle,
radius: end.magnitude,
angle: Angle::zero(),
}, },
tangent: Vector { tangent: StraightPath {
origin: Point::zero(), origin: tangent_origin,
angle: Angle::zero(), vector: Vector::from_angle_and_length(tangent_angle, tangent_magnitude),
magnitude: 0.0,
}, },
end: CircleVector { end: CirclePath {
center: Point::zero(), center: end_center,
radius: end.magnitude, radius: radius,
angle: Angle::zero(), angle: end_angle,
}, },
}; })
}
/// left straight left route
pub fn lsl(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
let start_center = Point::new(-radius, 0.0);
// 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
// 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.center = end.origin let end_center = end_point
+ Rotation::new(Angle::pi() - end.angle) + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(radius, 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( let mut tangent_angle = Angle::radians(
((route_csc.end.center.y - route_csc.start.center.y) ((end_center.y - start_center.y) / (end_center.x - start_center.x)).atan(),
/ (route_csc.end.center.x - route_csc.start.center.x))
.atan(),
) )
.positive(); .positive();
// 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.center.x < route_csc.start.center.x { if end_center.x < start_center.x {
route_csc.tangent.angle = (route_csc.tangent.angle + Angle::pi()).positive(); tangent_angle = (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.center.x - route_csc.start.center.x) let tangent_magnitude = ((end_center.x - start_center.x).abs().powi(2)
.abs() + (end_center.y - start_center.y).abs().powi(2))
.powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y)
.abs()
.powi(2))
.sqrt(); .sqrt();
// 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(); let start_angle = (tangent_angle - Angle::frac_pi_2()).positive();
// 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
// π/2 to it's own direction and the magnitude of the circle radius // π/2 to it's own direction and the magnitude of the circle radius
route_csc.tangent.origin = route_csc.start.center let tangent_origin =
+ Rotation::new(route_csc.start.angle) start_center + Rotation::new(start_angle).transform_vector(Vector::new(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
// so we can use that in here // so we can use that in here
route_csc.end.angle = (end.angle - route_csc.start.angle).positive(); let end_angle = (end_angle - start_angle).positive();
Ok(route_csc)
}
/// right straight left route Ok(Self {
pub fn rsl(end: Vector) -> Result<Self, Error> { start: CirclePath {
let mut route_csc = RouteCSC { center: start_center,
start: CircleVector { radius: radius,
center: Point::new(end.magnitude, 0.0), angle: start_angle,
radius: end.magnitude,
angle: Angle::zero(),
}, },
tangent: Vector { tangent: StraightPath {
origin: Point::zero(), origin: tangent_origin,
angle: Angle::zero(), vector: Vector::from_angle_and_length(tangent_angle, tangent_magnitude),
magnitude: 0.0,
}, },
end: CircleVector { end: CirclePath {
center: Point::zero(), center: end_center,
radius: end.magnitude, radius: radius,
angle: Angle::zero(), angle: end_angle,
}, },
}; })
}
/// right straight left route
pub fn rsl(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
let start_center = Point::new(radius, 0.0);
// 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
// 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.center = end.origin let end_center = end_point
+ Rotation::new(Angle::pi() - end.angle) + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(radius, 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.center.x - route_csc.start.center.x).powi(2) if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2))
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2)) .sqrt()
.sqrt() < 2.0 * radius
< 2.0 * end.magnitude
{ {
return Err(Error::CirclesTooClose); return Err(Error::CirclesTooClose);
} }
// get the tangent length via some simple trigonometry // get the tangent length via some simple trigonometry
route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x).powi(2) let tangent_magnitude = ((end_center.x - start_center.x).powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2) + (end_center.y - start_center.y).powi(2)
- (2.0 * end.magnitude).powi(2)) - (2.0 * radius).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.end.center.lerp(route_csc.start.center, 0.5); let tangent_middle = end_center.lerp(start_center, 0.5);
// get the tangent angle // get the tangent angle
route_csc.tangent.angle = Angle::radians( let mut tangent_angle = Angle::radians(
((route_csc.end.center.y - tangent_middle.y) ((end_center.y - tangent_middle.y) / (end_center.x - tangent_middle.x)).atan()
/ (route_csc.end.center.x - tangent_middle.x)) - (2.0 * radius / tangent_magnitude).atan(),
.atan()
- (2.0 * end.magnitude / route_csc.tangent.magnitude).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 π rotated so to prevent that: // the angle would be π rotated so to prevent that:
if route_csc.end.center.x < route_csc.start.center.x { if end_center.x < start_center.x {
route_csc.tangent.angle += Angle::pi(); tangent_angle += Angle::pi();
} }
// get the angle of the start circle // get the angle of the start circle
route_csc.start.angle = (Angle::frac_pi_2() - route_csc.tangent.angle).positive(); let start_angle = (Angle::frac_pi_2() - tangent_angle).positive();
// 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.center let tangent_origin = start_center
+ Rotation::new(Angle::pi() - route_csc.start.angle) + Rotation::new(Angle::pi() - start_angle).transform_vector(Vector::new(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 = let end_angle = ((Angle::frac_pi_2() - end_angle) - tangent_angle).positive();
((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive();
Ok(route_csc) Ok(Self {
} start: CirclePath {
center: start_center,
/// left straight right route radius: radius,
pub fn lsr(end: Vector) -> Result<Self, Error> { angle: start_angle,
let mut route_csc = RouteCSC {
start: CircleVector {
center: Point::new(-end.magnitude, 0.0),
radius: end.magnitude,
angle: Angle::zero(),
}, },
tangent: Vector { tangent: StraightPath {
origin: Point::zero(), origin: tangent_origin,
angle: Angle::zero(), vector: Vector::from_angle_and_length(tangent_angle, tangent_magnitude),
magnitude: 0.0,
}, },
end: CircleVector { end: CirclePath {
center: Point::zero(), center: end_center,
radius: end.magnitude, radius: radius,
angle: Angle::zero(), angle: end_angle,
}, },
}; })
}
/// left straight right route
pub fn lsr(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
let start_center = Point::new(-radius, 0.0);
// 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 2π - end.angle)
route_csc.end.center = end.origin let end_center = end_point
+ Rotation::new(end.angle) + Rotation::new(end_angle)
.inverse() .inverse()
.transform_vector(Vector2D::new(end.magnitude, 0.0)); .transform_vector(Vector::new(radius, 0.0));
// check if inside tangent can even be constructed // check if inside tangent can even be constructed
if ((route_csc.end.center.x - route_csc.start.center.x).powi(2) if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2))
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2)) .sqrt()
.sqrt() < 2.0 * radius
< 2.0 * end.magnitude
{ {
return Err(Error::CirclesTooClose); return Err(Error::CirclesTooClose);
} }
// get the tangent length via some simple trigonometry // get the tangent length via some simple trigonometry
route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x).powi(2) let tangent_magnitude = ((end_center.x - start_center.x).powi(2)
+ (route_csc.end.center.y - route_csc.start.center.y).powi(2) + (end_center.y - start_center.y).powi(2)
- (2.0 * end.magnitude).powi(2)) - (2.0 * radius).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.end.center.lerp(route_csc.start.center, 0.5); let tangent_middle = end_center.lerp(start_center, 0.5);
// get the tangent angle // get the tangent angle
route_csc.tangent.angle = Angle::radians( let mut tangent_angle = Angle::radians(
((route_csc.end.center.y - tangent_middle.y) ((end_center.y - tangent_middle.y) / (end_center.x - tangent_middle.x)).atan()
/ (route_csc.end.center.x - tangent_middle.x)) + (2.0 * radius / tangent_magnitude).atan(),
.atan()
+ (2.0 * end.magnitude / route_csc.tangent.magnitude).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 rotated by π so to prevent that: // the angle would rotated by π so to prevent that:
if route_csc.end.center.x < route_csc.start.center.x { if end_center.x < start_center.x {
route_csc.tangent.angle += Angle::pi(); tangent_angle += Angle::pi();
} }
// 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(); let start_angle = (tangent_angle - Angle::frac_pi_2()).positive();
// 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
// π/2 to it's own direction and the magnitude of the circle radius // π/2 to it's own direction and the magnitude of the circle radius
route_csc.tangent.origin = route_csc.start.center let tangent_origin =
+ Rotation::new(route_csc.start.angle) start_center + Rotation::new(start_angle).transform_vector(Vector::new(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 = let end_angle = ((Angle::frac_pi_2() - end_angle) - tangent_angle).positive();
((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive();
Ok(route_csc) Ok(Self {
start: CirclePath {
center: start_center,
radius: radius,
angle: start_angle,
},
tangent: StraightPath {
origin: tangent_origin,
vector: Vector::from_angle_and_length(tangent_angle, tangent_magnitude),
},
end: CirclePath {
center: end_center,
radius: radius,
angle: end_angle,
},
})
} }
/// get the length of the path /// get the length of the path
pub fn get_length(&self) -> f64 { pub fn get_length(&self) -> f64 {
self.start.get_length() + self.tangent.magnitude + self.end.get_length() self.start.get_length() + self.tangent.vector.length() + self.end.get_length()
} }
/// get the shortest circle straight circle route /// get the shortest circle straight circle route
pub fn get_shortest(end: Vector) -> Result<Self, Error> { pub fn get_shortest(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
let mut route_csc; let mut route_csc;
let route_rsr = Self::rsr(end).unwrap(); let route_rsr = Self::rsr(radius, end_point, end_angle).unwrap();
let route_lsl = Self::rsr(end).unwrap(); let route_lsl = Self::rsr(radius, end_point, end_angle).unwrap();
let route_lsr = Self::rsr(end); let route_lsr = Self::rsr(radius, end_point, end_angle);
let route_rsl = Self::rsr(end); let route_rsl = Self::rsr(radius, end_point, end_angle);
route_csc = route_rsr; route_csc = route_rsr;
if route_lsl.get_length() < route_csc.get_length() { if route_lsl.get_length() < route_csc.get_length() {
@ -430,181 +394,161 @@ impl RouteCSC {
/// Route with 3 Circles /// Route with 3 Circles
impl RouteCCC { impl RouteCCC {
/// right left right route (not working yet) /// right left right route (not working yet)
pub fn rlr(end: Vector) -> Result<Self, Error> { pub fn rlr(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
let mut route_ccc = RouteCCC { let start_center = Point::new(radius, 0.0);
start: CircleVector {
center: Point::new(end.magnitude, 0.0),
radius: end.magnitude,
angle: Angle::zero(),
},
middle: CircleVector {
center: Point::zero(),
radius: end.magnitude,
angle: Angle::zero(),
},
end: CircleVector {
center: Point::zero(),
radius: end.magnitude,
angle: Angle::zero(),
},
};
// 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 we use the inverse end.angle) // the angle has to be counter clockwise though (thats why we use the inverse end.angle)
route_ccc.end.center = end.origin let end_center = end_point
+ Rotation::new(end.angle) + Rotation::new(end_angle)
.inverse() .inverse()
.transform_vector(Vector2D::new(end.magnitude, 0.0)); .transform_vector(Vector::new(radius, 0.0));
// check if path can be constructed or if the circles are too far apart // check if path can be constructed or if the circles are too far apart
if ((route_ccc.end.center.x - route_ccc.start.center.x).powi(2) if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2))
+ (route_ccc.end.center.y - route_ccc.start.center.y).powi(2)) .sqrt()
.sqrt() > (4.0 * radius)
> (4.0 * end.magnitude)
{ {
return Err(Error::CirclesTooFarApart); return Err(Error::CirclesTooFarApart);
} }
let vector_start_center_middle_center: Vector2D; let vector_start_center_middle_center: Vector;
route_ccc.middle.center = {
let vector_start_center_end_center = Vector2D::new(
route_ccc.end.center.x - route_ccc.start.center.x,
route_ccc.end.center.y - route_ccc.start.center.y,
);
let vector_start_center_end_center_magnitude = let middle_center = {
(vector_start_center_end_center.x.powi(2) let vector_start_center_end_center =
+ vector_start_center_end_center.y.powi(2)) Vector::new(end_center.x - start_center.x, end_center.y - start_center.y);
.sqrt();
let vector_start_center_middle_center_angle = let vector_start_center_middle_center_angle =
vector_start_center_end_center.angle_from_x_axis().radians vector_start_center_end_center.angle_from_x_axis().radians
+ (vector_start_center_end_center_magnitude / (4.0 * end.magnitude)).acos(); + (vector_start_center_end_center.length() / (4.0 * radius)).acos();
vector_start_center_middle_center = Vector2D::new( vector_start_center_middle_center = Vector::new(
(2.0 * end.magnitude) * vector_start_center_middle_center_angle.cos(), (2.0 * radius) * vector_start_center_middle_center_angle.cos(),
(2.0 * end.magnitude) * vector_start_center_middle_center_angle.sin(), (2.0 * radius) * vector_start_center_middle_center_angle.sin(),
); );
Point::new( Point::new(
route_ccc.start.center.x + vector_start_center_middle_center.x, start_center.x + vector_start_center_middle_center.x,
route_ccc.start.center.y + vector_start_center_middle_center.y, start_center.y + vector_start_center_middle_center.y,
) )
}; };
let vector_middle_center_end_center = Vector2D::new( let vector_middle_center_end_center = Vector::new(
route_ccc.end.center.x - route_ccc.middle.center.x, end_center.x - middle_center.x,
route_ccc.end.center.y - route_ccc.middle.center.y, end_center.y - middle_center.y,
); );
route_ccc.start.angle = let start_angle =
(Angle::pi() - vector_start_center_middle_center.angle_from_x_axis()).positive(); (Angle::pi() - vector_start_center_middle_center.angle_from_x_axis()).positive();
route_ccc.middle.angle = Rotation::new(Angle::pi()) let middle_angle = Rotation::new(Angle::pi())
.transform_vector(vector_start_center_middle_center) .transform_vector(vector_start_center_middle_center)
.angle_to(vector_middle_center_end_center) .angle_to(vector_middle_center_end_center)
.positive(); .positive();
route_ccc.end.angle = Rotation::new(Angle::pi()) let end_angle = Rotation::new(Angle::pi())
.transform_vector(vector_middle_center_end_center) .transform_vector(vector_middle_center_end_center)
.angle_to(Vector2D::new( .angle_to(Vector::new(
end.origin.x - route_ccc.end.center.x, end_point.x - end_center.x,
end.origin.y - route_ccc.end.center.y, end_point.y - end_center.y,
)) ))
.positive(); .positive();
Ok(route_ccc) Ok(Self {
} start: CirclePath {
center: start_center,
/// left right left route (not working yet) radius: radius,
pub fn lrl(end: Vector) -> Result<Self, Error> { angle: start_angle,
let mut route_ccc = RouteCCC {
start: CircleVector {
center: Point::new(-end.magnitude, 0.0),
radius: end.magnitude,
angle: Angle::zero(),
}, },
middle: CircleVector { middle: CirclePath {
center: Point::zero(), center: middle_center,
radius: end.magnitude, radius: radius,
angle: Angle::zero(), angle: middle_angle,
}, },
end: CircleVector { end: CirclePath {
center: Point::zero(), center: end_center,
radius: end.magnitude, radius: radius,
angle: Angle::zero(), angle: end_angle,
}, },
}; })
}
/// left right left route (not working yet)
pub fn lrl(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
let start_center = Point::new(-radius, 0.0);
// 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
// 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_ccc.end.center = end.origin let end_center = end_point
+ Rotation::new(Angle::pi() - end.angle) + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(radius, 0.0));
.transform_vector(Vector2D::new(end.magnitude, 0.0));
// check if path can be constructed or if the circles are too far apart // check if path can be constructed or if the circles are too far apart
if ((route_ccc.end.center.x - route_ccc.start.center.x).powi(2) if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2))
+ (route_ccc.end.center.y - route_ccc.start.center.y).powi(2)) .sqrt()
.sqrt() > (4.0 * radius)
> (4.0 * end.magnitude)
{ {
return Err(Error::CirclesTooFarApart); return Err(Error::CirclesTooFarApart);
} }
let vector_start_center_middle_center: Vector2D; let vector_start_center_middle_center: Vector;
route_ccc.middle.center = { let middle_center = {
let vector_start_center_end_center = Vector2D::new( let vector_start_center_end_center =
route_ccc.end.center.x - route_ccc.start.center.x, Vector::new(end_center.x - start_center.x, end_center.y - start_center.y);
route_ccc.end.center.y - route_ccc.start.center.y,
);
let vector_start_center_end_center_magnitude =
(vector_start_center_end_center.x.powi(2)
+ vector_start_center_end_center.y.powi(2))
.sqrt();
let vector_start_center_middle_center_angle = let vector_start_center_middle_center_angle =
vector_start_center_end_center.angle_from_x_axis().radians vector_start_center_end_center.angle_from_x_axis().radians
- (vector_start_center_end_center_magnitude / (4.0 * end.magnitude)).acos(); - (vector_start_center_end_center.length() / (4.0 * radius)).acos();
vector_start_center_middle_center = Vector2D::new( vector_start_center_middle_center = Vector::new(
(2.0 * end.magnitude) * vector_start_center_middle_center_angle.cos(), (2.0 * radius) * vector_start_center_middle_center_angle.cos(),
(2.0 * end.magnitude) * vector_start_center_middle_center_angle.sin(), (2.0 * radius) * vector_start_center_middle_center_angle.sin(),
); );
Point::new( Point::new(
route_ccc.start.center.x + vector_start_center_middle_center.x, start_center.x + vector_start_center_middle_center.x,
route_ccc.start.center.y + vector_start_center_middle_center.y, start_center.y + vector_start_center_middle_center.y,
) )
}; };
let vector_middle_center_end_center = Vector2D::new( let vector_middle_center_end_center = Vector::new(
route_ccc.end.center.x - route_ccc.middle.center.x, end_center.x - middle_center.x,
route_ccc.end.center.y - route_ccc.middle.center.y, end_center.y - middle_center.y,
); );
route_ccc.start.angle = (vector_start_center_middle_center.angle_from_x_axis()).positive(); let start_angle = (vector_start_center_middle_center.angle_from_x_axis()).positive();
route_ccc.middle.angle = vector_middle_center_end_center let middle_angle = vector_middle_center_end_center
.angle_to( .angle_to(
Rotation::new(Angle::pi()).transform_vector(vector_start_center_middle_center), Rotation::new(Angle::pi()).transform_vector(vector_start_center_middle_center),
) )
.positive(); .positive();
route_ccc.end.angle = Vector2D::new( let end_angle = Vector::new(end_point.x - end_center.x, end_point.y - end_center.y)
end.origin.x - route_ccc.end.center.x, .angle_to(Rotation::new(Angle::pi()).transform_vector(vector_middle_center_end_center))
end.origin.y - route_ccc.end.center.y, .positive();
)
.angle_to(Rotation::new(Angle::pi()).transform_vector(vector_middle_center_end_center))
.positive();
Ok(route_ccc) Ok(Self {
start: CirclePath {
center: start_center,
radius: radius,
angle: start_angle,
},
middle: CirclePath {
center: middle_center,
radius: radius,
angle: middle_angle,
},
end: CirclePath {
center: end_center,
radius: radius,
angle: end_angle,
},
})
} }
/// get the length of the path /// get the length of the path
@ -613,9 +557,9 @@ impl RouteCCC {
} }
/// get the shortest circle circle circle route /// get the shortest circle circle circle route
pub fn get_shortest(end: Vector) -> Result<Self, Error> { pub fn get_shortest(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
let route_rlr = Self::rlr(end); let route_rlr = Self::rlr(radius, end_point, end_angle);
let route_lrl = Self::lrl(end); let route_lrl = Self::lrl(radius, end_point, end_angle);
if let Ok(route_rlr) = route_rlr { if let Ok(route_rlr) = route_rlr {
if let Ok(route_lrl) = route_lrl { if let Ok(route_lrl) = route_lrl {
@ -636,9 +580,9 @@ impl RouteCCC {
} }
/// get the shortest path /// get the shortest path
pub fn get_shortest(end: Vector) -> Path { pub fn get_shortest(radius: f64, end_point: Point, end_angle: Angle) -> Path {
let route_csc = RouteCSC::get_shortest(end).unwrap(); let route_csc = RouteCSC::get_shortest(radius, end_point, end_angle).unwrap();
let route_ccc = RouteCCC::get_shortest(end); let route_ccc = RouteCCC::get_shortest(radius, end_point, end_angle);
if let Ok(route_ccc) = route_ccc { if let Ok(route_ccc) = route_ccc {
if route_ccc.get_length() < route_csc.get_length() { if route_ccc.get_length() < route_csc.get_length() {
Path::CCC(route_ccc) Path::CCC(route_ccc)

Write Preview
Loading…
Cancel
Save