From a68159b997693bd46da664f2c6ffa0152328c8bd Mon Sep 17 00:00:00 2001 From: gnxlxnxx Date: Tue, 23 Jun 2020 20:23:58 +0200 Subject: [PATCH] Remove giant mut, rename structs 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 --- src/lib.rs | 548 ++++++++++++++++++++++++----------------------------- 1 file changed, 246 insertions(+), 302 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index fd36a91..432b0d1 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -25,43 +25,33 @@ pub enum Error { /// Vector with origin, angle and magnitude #[derive(Debug, Copy, Clone)] -pub struct Vector { +pub struct StraightPath { pub origin: Point, - pub angle: Angle, - pub magnitude: f64, + pub vector: Vector, } -impl Vector { +impl StraightPath { /// approximate equality to other Vector pub fn approx_eq(&self, other: Self) -> bool { - if !ApproxEq::approx_eq(&self.origin, &other.origin) { - false - } 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 - } + ApproxEq::approx_eq(&self.vector, &other.vector) + && ApproxEq::approx_eq(&self.origin, &other.origin) } } /// Circle vector (Circle + Angle) #[derive(Debug, Copy, Clone)] -pub struct CircleVector { +pub struct CirclePath { pub center: Point, pub radius: f64, pub angle: Angle, } -impl CircleVector { +impl CirclePath { ///get the length of the circle vector pub fn get_length(&self) -> f64 { self.angle.radians * self.radius } - /// approximate equality to other CircleVector + /// approximate equality to other CirclePath pub fn approx_eq(&self, other: Self) -> bool { if !ApproxEq::approx_eq(&self.center, &other.center) { false @@ -80,17 +70,17 @@ impl CircleVector { /// Route with a start Circle, a tangent straight and a end Circle #[derive(Debug, Copy, Clone)] pub struct RouteCSC { - pub start: CircleVector, - pub tangent: Vector, - pub end: CircleVector, + pub start: CirclePath, + pub tangent: StraightPath, + pub end: CirclePath, } /// Route with 3 Circles #[derive(Debug, Copy, Clone)] pub struct RouteCCC { - pub start: CircleVector, - pub middle: CircleVector, - pub end: CircleVector, + pub start: CirclePath, + pub middle: CirclePath, + pub end: CirclePath, } #[derive(Debug, Copy, Clone)] @@ -102,311 +92,285 @@ pub enum Path { /// Route with a start Circle, a tangent straight and a end Circle impl RouteCSC { /// right straight right route - pub fn rsr(end: Vector) -> Result { - 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(), - }, - }; + pub fn rsr(radius: f64, end_point: Point, end_angle: Angle) -> Result { + let start_center = Point::new(radius, 0.0); // 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) + let end_center = end_point + + Rotation::new(end_angle) .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 // 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(), + let mut tangent_angle = Angle::radians( + ((end_center.y - start_center.y) / (end_center.x - start_center.x)).atan(), ); // if the end circle center x value is smaller than the // start circle center x value // the angle would be rotated by π so to prevent that: - if route_csc.end.center.x < route_csc.start.center.x { - route_csc.tangent.angle += Angle::pi(); + if end_center.x < start_center.x { + 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)) + let tangent_magnitude = ((end_center.x - start_center.x).powi(2) + + (end_center.y - 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(); + let start_angle = (Angle::frac_pi_2() - tangent_angle).positive(); // 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 - route_csc.tangent.origin = route_csc.start.center - + Rotation::new(Angle::pi() - end.angle) - .transform_vector(Vector2D::new(route_csc.start.radius, 0.0)); + let tangent_origin = start_center + + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(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) - } + let end_angle = (end_angle - start_angle).positive(); - /// left straight left route - pub fn lsl(end: Vector) -> Result { - let mut route_csc = RouteCSC { - start: CircleVector { - center: Point::new(-end.magnitude, 0.0), - radius: end.magnitude, - angle: Angle::zero(), + Ok(Self { + start: CirclePath { + center: start_center, + radius: radius, + angle: start_angle, }, - tangent: Vector { - origin: Point::zero(), - angle: Angle::zero(), - magnitude: 0.0, + tangent: StraightPath { + origin: tangent_origin, + vector: Vector::from_angle_and_length(tangent_angle, tangent_magnitude), }, - end: CircleVector { - center: Point::zero(), - radius: end.magnitude, - angle: Angle::zero(), + end: CirclePath { + center: end_center, + radius: radius, + angle: end_angle, }, - }; + }) + } + + /// left straight left route + pub fn lsl(radius: f64, end_point: Point, end_angle: Angle) -> Result { + let start_center = Point::new(-radius, 0.0); // 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)); + let end_center = end_point + + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(radius, 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(), + let mut tangent_angle = Angle::radians( + ((end_center.y - start_center.y) / (end_center.x - 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(); + if end_center.x < start_center.x { + tangent_angle = (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() - .powi(2)) + let tangent_magnitude = ((end_center.x - start_center.x).abs().powi(2) + + (end_center.y - 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(); + let start_angle = (tangent_angle - Angle::frac_pi_2()).positive(); // 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 - route_csc.tangent.origin = route_csc.start.center - + Rotation::new(route_csc.start.angle) - .transform_vector(Vector2D::new(route_csc.start.radius, 0.0)); + let tangent_origin = + start_center + Rotation::new(start_angle).transform_vector(Vector::new(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) - } + let end_angle = (end_angle - start_angle).positive(); - /// right straight left route - pub fn rsl(end: Vector) -> Result { - let mut route_csc = RouteCSC { - start: CircleVector { - center: Point::new(end.magnitude, 0.0), - radius: end.magnitude, - angle: Angle::zero(), + Ok(Self { + start: CirclePath { + center: start_center, + radius: radius, + angle: start_angle, }, - tangent: Vector { - origin: Point::zero(), - angle: Angle::zero(), - magnitude: 0.0, + tangent: StraightPath { + origin: tangent_origin, + vector: Vector::from_angle_and_length(tangent_angle, tangent_magnitude), }, - end: CircleVector { - center: Point::zero(), - radius: end.magnitude, - angle: Angle::zero(), + end: CirclePath { + center: end_center, + radius: radius, + angle: end_angle, }, - }; + }) + } + + /// right straight left route + pub fn rsl(radius: f64, end_point: Point, end_angle: Angle) -> Result { + let start_center = Point::new(radius, 0.0); // 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)); + let end_center = end_point + + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(radius, 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 + if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2)) + .sqrt() + < 2.0 * radius { return Err(Error::CirclesTooClose); } // 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)) + let tangent_magnitude = ((end_center.x - start_center.x).powi(2) + + (end_center.y - start_center.y).powi(2) + - (2.0 * radius).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); + let tangent_middle = end_center.lerp(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(), + let mut tangent_angle = Angle::radians( + ((end_center.y - tangent_middle.y) / (end_center.x - tangent_middle.x)).atan() + - (2.0 * radius / 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(); + if end_center.x < start_center.x { + tangent_angle += Angle::pi(); } // 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 // 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)); + let tangent_origin = start_center + + Rotation::new(Angle::pi() - start_angle).transform_vector(Vector::new(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(); + let end_angle = ((Angle::frac_pi_2() - end_angle) - tangent_angle).positive(); - Ok(route_csc) - } - - /// left straight right route - pub fn lsr(end: Vector) -> Result { - let mut route_csc = RouteCSC { - start: CircleVector { - center: Point::new(-end.magnitude, 0.0), - radius: end.magnitude, - angle: Angle::zero(), + Ok(Self { + start: CirclePath { + center: start_center, + radius: radius, + angle: start_angle, }, - tangent: Vector { - origin: Point::zero(), - angle: Angle::zero(), - magnitude: 0.0, + tangent: StraightPath { + origin: tangent_origin, + vector: Vector::from_angle_and_length(tangent_angle, tangent_magnitude), }, - end: CircleVector { - center: Point::zero(), - radius: end.magnitude, - angle: Angle::zero(), + end: CirclePath { + center: end_center, + radius: radius, + angle: end_angle, }, - }; + }) + } + + /// left straight right route + pub fn lsr(radius: f64, end_point: Point, end_angle: Angle) -> Result { + let start_center = Point::new(-radius, 0.0); // 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) + let end_center = end_point + + Rotation::new(end_angle) .inverse() - .transform_vector(Vector2D::new(end.magnitude, 0.0)); + .transform_vector(Vector::new(radius, 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 + if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2)) + .sqrt() + < 2.0 * radius { return Err(Error::CirclesTooClose); } // 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)) + let tangent_magnitude = ((end_center.x - start_center.x).powi(2) + + (end_center.y - start_center.y).powi(2) + - (2.0 * radius).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); + let tangent_middle = end_center.lerp(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(), + let mut tangent_angle = Angle::radians( + ((end_center.y - tangent_middle.y) / (end_center.x - tangent_middle.x)).atan() + + (2.0 * radius / tangent_magnitude).atan(), ); // if the end circle center x value is smaller than the // start circle center x value // the angle would rotated by π so to prevent that: - if route_csc.end.center.x < route_csc.start.center.x { - route_csc.tangent.angle += Angle::pi(); + if end_center.x < start_center.x { + tangent_angle += Angle::pi(); } // 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 // π/2 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)); + let tangent_origin = + start_center + Rotation::new(start_angle).transform_vector(Vector::new(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(); + let end_angle = ((Angle::frac_pi_2() - end_angle) - 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 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 - pub fn get_shortest(end: Vector) -> Result { + pub fn get_shortest(radius: f64, end_point: Point, end_angle: Angle) -> Result { let mut route_csc; - let route_rsr = Self::rsr(end).unwrap(); - let route_lsl = Self::rsr(end).unwrap(); - let route_lsr = Self::rsr(end); - let route_rsl = Self::rsr(end); + let route_rsr = Self::rsr(radius, end_point, end_angle).unwrap(); + let route_lsl = Self::rsr(radius, end_point, end_angle).unwrap(); + let route_lsr = Self::rsr(radius, end_point, end_angle); + let route_rsl = Self::rsr(radius, end_point, end_angle); route_csc = route_rsr; if route_lsl.get_length() < route_csc.get_length() { @@ -430,181 +394,161 @@ impl RouteCSC { /// Route with 3 Circles impl RouteCCC { /// right left right route (not working yet) - pub fn rlr(end: Vector) -> Result { - let mut route_ccc = RouteCCC { - 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(), - }, - }; + pub fn rlr(radius: f64, end_point: Point, end_angle: Angle) -> Result { + let start_center = Point::new(radius, 0.0); // 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_ccc.end.center = end.origin - + Rotation::new(end.angle) + let end_center = end_point + + Rotation::new(end_angle) .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 - if ((route_ccc.end.center.x - route_ccc.start.center.x).powi(2) - + (route_ccc.end.center.y - route_ccc.start.center.y).powi(2)) - .sqrt() - > (4.0 * end.magnitude) + if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2)) + .sqrt() + > (4.0 * radius) { return Err(Error::CirclesTooFarApart); } - let vector_start_center_middle_center: Vector2D; - - 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_middle_center: Vector; - 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 middle_center = { + let vector_start_center_end_center = + Vector::new(end_center.x - start_center.x, end_center.y - start_center.y); let vector_start_center_middle_center_angle = 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( - (2.0 * end.magnitude) * vector_start_center_middle_center_angle.cos(), - (2.0 * end.magnitude) * vector_start_center_middle_center_angle.sin(), + vector_start_center_middle_center = Vector::new( + (2.0 * radius) * vector_start_center_middle_center_angle.cos(), + (2.0 * radius) * vector_start_center_middle_center_angle.sin(), ); Point::new( - route_ccc.start.center.x + vector_start_center_middle_center.x, - route_ccc.start.center.y + vector_start_center_middle_center.y, + start_center.x + vector_start_center_middle_center.x, + start_center.y + vector_start_center_middle_center.y, ) }; - let vector_middle_center_end_center = Vector2D::new( - route_ccc.end.center.x - route_ccc.middle.center.x, - route_ccc.end.center.y - route_ccc.middle.center.y, + let vector_middle_center_end_center = Vector::new( + end_center.x - middle_center.x, + 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(); - route_ccc.middle.angle = Rotation::new(Angle::pi()) + let middle_angle = Rotation::new(Angle::pi()) .transform_vector(vector_start_center_middle_center) .angle_to(vector_middle_center_end_center) .positive(); - route_ccc.end.angle = Rotation::new(Angle::pi()) + let end_angle = Rotation::new(Angle::pi()) .transform_vector(vector_middle_center_end_center) - .angle_to(Vector2D::new( - end.origin.x - route_ccc.end.center.x, - end.origin.y - route_ccc.end.center.y, + .angle_to(Vector::new( + end_point.x - end_center.x, + end_point.y - end_center.y, )) .positive(); - Ok(route_ccc) - } - - /// left right left route (not working yet) - pub fn lrl(end: Vector) -> Result { - let mut route_ccc = RouteCCC { - start: CircleVector { - center: Point::new(-end.magnitude, 0.0), - radius: end.magnitude, - angle: Angle::zero(), + Ok(Self { + start: CirclePath { + center: start_center, + radius: radius, + angle: start_angle, }, - middle: CircleVector { - center: Point::zero(), - radius: end.magnitude, - angle: Angle::zero(), + middle: CirclePath { + center: middle_center, + radius: radius, + angle: middle_angle, }, - end: CircleVector { - center: Point::zero(), - radius: end.magnitude, - angle: Angle::zero(), + end: CirclePath { + center: end_center, + radius: radius, + angle: end_angle, }, - }; + }) + } + + /// left right left route (not working yet) + pub fn lrl(radius: f64, end_point: Point, end_angle: Angle) -> Result { + let start_center = Point::new(-radius, 0.0); // 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_ccc.end.center = end.origin - + Rotation::new(Angle::pi() - end.angle) - .transform_vector(Vector2D::new(end.magnitude, 0.0)); + let end_center = end_point + + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(radius, 0.0)); // 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) - + (route_ccc.end.center.y - route_ccc.start.center.y).powi(2)) - .sqrt() - > (4.0 * end.magnitude) + if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2)) + .sqrt() + > (4.0 * radius) { 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 = - (vector_start_center_end_center.x.powi(2) - + vector_start_center_end_center.y.powi(2)) - .sqrt(); + let middle_center = { + let vector_start_center_end_center = + Vector::new(end_center.x - start_center.x, end_center.y - start_center.y); let vector_start_center_middle_center_angle = 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( - (2.0 * end.magnitude) * vector_start_center_middle_center_angle.cos(), - (2.0 * end.magnitude) * vector_start_center_middle_center_angle.sin(), + vector_start_center_middle_center = Vector::new( + (2.0 * radius) * vector_start_center_middle_center_angle.cos(), + (2.0 * radius) * vector_start_center_middle_center_angle.sin(), ); Point::new( - route_ccc.start.center.x + vector_start_center_middle_center.x, - route_ccc.start.center.y + vector_start_center_middle_center.y, + start_center.x + vector_start_center_middle_center.x, + start_center.y + vector_start_center_middle_center.y, ) }; - let vector_middle_center_end_center = Vector2D::new( - route_ccc.end.center.x - route_ccc.middle.center.x, - route_ccc.end.center.y - route_ccc.middle.center.y, + let vector_middle_center_end_center = Vector::new( + end_center.x - middle_center.x, + 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( Rotation::new(Angle::pi()).transform_vector(vector_start_center_middle_center), ) .positive(); - route_ccc.end.angle = Vector2D::new( - end.origin.x - route_ccc.end.center.x, - end.origin.y - route_ccc.end.center.y, - ) - .angle_to(Rotation::new(Angle::pi()).transform_vector(vector_middle_center_end_center)) - .positive(); + let end_angle = Vector::new(end_point.x - end_center.x, end_point.y - end_center.y) + .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 @@ -613,9 +557,9 @@ impl RouteCCC { } /// get the shortest circle circle circle route - pub fn get_shortest(end: Vector) -> Result { - let route_rlr = Self::rlr(end); - let route_lrl = Self::lrl(end); + pub fn get_shortest(radius: f64, end_point: Point, end_angle: Angle) -> Result { + let route_rlr = Self::rlr(radius, end_point, end_angle); + let route_lrl = Self::lrl(radius, end_point, end_angle); if let Ok(route_rlr) = route_rlr { if let Ok(route_lrl) = route_lrl { @@ -636,9 +580,9 @@ impl RouteCCC { } /// get the shortest path -pub fn get_shortest(end: Vector) -> Path { - let route_csc = RouteCSC::get_shortest(end).unwrap(); - let route_ccc = RouteCCC::get_shortest(end); +pub fn get_shortest(radius: f64, end_point: Point, end_angle: Angle) -> Path { + let route_csc = RouteCSC::get_shortest(radius, end_point, end_angle).unwrap(); + let route_ccc = RouteCCC::get_shortest(radius, end_point, end_angle); if let Ok(route_ccc) = route_ccc { if route_ccc.get_length() < route_csc.get_length() { Path::CCC(route_ccc)