diff --git a/src/lib.rs b/src/lib.rs index a8d4344..1f6355e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -21,41 +21,34 @@ pub struct Vector { pub magnitude: f64, } -/// Circle -pub struct Circle { +/// Circle vector (Circle + Angle) +pub struct CircleVector { pub center: Point, pub radius: f64, -} - -/// Circle route with a circle and a angle for how long to drive on this circle -pub struct CircleRoute { - pub circle: Circle, pub angle: Angle, } /// Route with a start Circle, a tangent straight and a end Circle (eg. rsl, rsr, lsr, lsl) pub struct RouteCSC { - pub start: CircleRoute, + pub start: CircleVector, pub tangent: Vector, - pub end: CircleRoute, + pub end: CircleVector, } /// Route with 3 Circles (eg. rlr, lrl) (not yet implemented) #[allow(unused)] pub struct RouteCCC { - pub start: Circle, - pub middle: Circle, - pub end: Circle, + pub start: CircleVector, + pub middle: CircleVector, + pub end: CircleVector, } /// right straight right route pub fn rsr(end: Vector) -> Result { let mut route_csc = RouteCSC { - start: CircleRoute { - circle: Circle { - center: Point::new(end.magnitude, 0.0), - radius: end.magnitude, - }, + start: CircleVector { + center: Point::new(end.magnitude, 0.0), + radius: end.magnitude, angle: Angle::zero(), }, tangent: Vector { @@ -63,11 +56,9 @@ pub fn rsr(end: Vector) -> Result { angle: Angle::zero(), magnitude: 0.0, }, - end: CircleRoute { - circle: Circle { - center: Point::zero(), - radius: end.magnitude, - }, + end: CircleVector { + center: Point::zero(), + radius: end.magnitude, angle: Angle::zero(), }, }; @@ -76,7 +67,7 @@ pub fn rsr(end: Vector) -> Result { // 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.circle.center = end.origin + route_csc.end.center = end.origin + Rotation::new(end.angle) .inverse() .transform_vector(Vector2D::new(end.magnitude, 0.0)); @@ -84,33 +75,32 @@ pub fn rsr(end: Vector) -> Result { // 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.circle.center.y - route_csc.start.circle.center.y) - / (route_csc.end.circle.center.x - route_csc.start.circle.center.x)) + ((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.circle.center.x < route_csc.start.circle.center.x { + if route_csc.end.center.x < route_csc.start.center.x { route_csc.tangent.angle += Angle::pi(); } // 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.circle.center.x - route_csc.start.circle.center.x).powi(2) - + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2)) - .sqrt(); + 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.circle.center + route_csc.tangent.origin = route_csc.start.center + Rotation::new(Angle::pi() - end.angle) - .transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0)); + .transform_vector(Vector2D::new(route_csc.start.radius, 0.0)); // get the angle of the start circle // the angle where we start from the tangent equals the one we finish @@ -123,11 +113,9 @@ pub fn rsr(end: Vector) -> Result { /// left straight left route pub fn lsl(end: Vector) -> Result { let mut route_csc = RouteCSC { - start: CircleRoute { - circle: Circle { - center: Point::new(-end.magnitude, 0.0), - radius: end.magnitude, - }, + start: CircleVector { + center: Point::new(-end.magnitude, 0.0), + radius: end.magnitude, angle: Angle::zero(), }, tangent: Vector { @@ -135,11 +123,9 @@ pub fn lsl(end: Vector) -> Result { angle: Angle::zero(), magnitude: 0.0, }, - end: CircleRoute { - circle: Circle { - center: Point::zero(), - radius: end.magnitude, - }, + end: CircleVector { + center: Point::zero(), + radius: end.magnitude, angle: Angle::zero(), }, }; @@ -148,15 +134,15 @@ pub fn lsl(end: Vector) -> Result { // 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.circle.center = end.origin + 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.circle.center.y - route_csc.start.circle.center.y) - / (route_csc.end.circle.center.x - route_csc.start.circle.center.x)) + ((route_csc.end.center.y - route_csc.start.center.y) + / (route_csc.end.center.x - route_csc.start.center.x)) .atan(), ) .positive(); @@ -164,17 +150,16 @@ pub fn lsl(end: Vector) -> Result { // 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.circle.center.x < route_csc.start.circle.center.x { + if route_csc.end.center.x < route_csc.start.center.x { route_csc.tangent.angle = (route_csc.tangent.angle + Angle::pi()).positive(); } // 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.circle.center.x - - route_csc.start.circle.center.x) + route_csc.tangent.magnitude = ((route_csc.end.center.x - route_csc.start.center.x) .abs() .powi(2) - + (route_csc.end.circle.center.y - route_csc.start.circle.center.y) + + (route_csc.end.center.y - route_csc.start.center.y) .abs() .powi(2)) .sqrt(); @@ -184,9 +169,9 @@ pub fn lsl(end: Vector) -> Result { // 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.circle.center + route_csc.tangent.origin = route_csc.start.center + Rotation::new(route_csc.start.angle) - .transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0)); + .transform_vector(Vector2D::new(route_csc.start.radius, 0.0)); // get the angle of the start circle // the angle where we start from the tangent equals the one we finish @@ -199,11 +184,9 @@ pub fn lsl(end: Vector) -> Result { /// right straight left route pub fn rsl(end: Vector) -> Result { let mut route_csc = RouteCSC { - start: CircleRoute { - circle: Circle { - center: Point::new(end.magnitude, 0.0), - radius: end.magnitude, - }, + start: CircleVector { + center: Point::new(end.magnitude, 0.0), + radius: end.magnitude, angle: Angle::zero(), }, tangent: Vector { @@ -211,11 +194,9 @@ pub fn rsl(end: Vector) -> Result { angle: Angle::zero(), magnitude: 0.0, }, - end: CircleRoute { - circle: Circle { - center: Point::zero(), - radius: end.magnitude, - }, + end: CircleVector { + center: Point::zero(), + radius: end.magnitude, angle: Angle::zero(), }, }; @@ -224,13 +205,13 @@ pub fn rsl(end: Vector) -> Result { // 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.circle.center = end.origin + 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.circle.center.x - route_csc.start.circle.center.x).powi(2) - + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2)) + 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 { @@ -238,23 +219,17 @@ pub fn rsl(end: Vector) -> Result { } // get the tangent length via some simple trigonometry - route_csc.tangent.magnitude = - ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2) - + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2) - - (2.0 * end.magnitude).powi(2)) - .sqrt(); + 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 - .circle - .center - .lerp(route_csc.start.circle.center, 0.5); + 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.circle.center.y - tangent_middle.y) - / (route_csc.end.circle.center.x - tangent_middle.x)) + ((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(), ); @@ -262,7 +237,7 @@ pub fn rsl(end: Vector) -> Result { // 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.circle.center.x < route_csc.start.circle.center.x { + if route_csc.end.center.x < route_csc.start.center.x { route_csc.tangent.angle += Angle::pi(); } @@ -271,9 +246,9 @@ pub fn rsl(end: Vector) -> Result { // 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.circle.center + route_csc.tangent.origin = route_csc.start.center + Rotation::new(Angle::pi() - route_csc.start.angle) - .transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0)); + .transform_vector(Vector2D::new(route_csc.start.radius, 0.0)); // get the angle of the end circle route_csc.end.angle = ((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive(); @@ -284,11 +259,9 @@ pub fn rsl(end: Vector) -> Result { /// left straight right route pub fn lsr(end: Vector) -> Result { let mut route_csc = RouteCSC { - start: CircleRoute { - circle: Circle { - center: Point::new(-end.magnitude, 0.0), - radius: end.magnitude, - }, + start: CircleVector { + center: Point::new(-end.magnitude, 0.0), + radius: end.magnitude, angle: Angle::zero(), }, tangent: Vector { @@ -296,11 +269,9 @@ pub fn lsr(end: Vector) -> Result { angle: Angle::zero(), magnitude: 0.0, }, - end: CircleRoute { - circle: Circle { - center: Point::zero(), - radius: end.magnitude, - }, + end: CircleVector { + center: Point::zero(), + radius: end.magnitude, angle: Angle::zero(), }, }; @@ -309,14 +280,14 @@ pub fn lsr(end: Vector) -> Result { // 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.circle.center = end.origin + 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.circle.center.x - route_csc.start.circle.center.x).powi(2) - + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2)) + 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 { @@ -324,23 +295,17 @@ pub fn lsr(end: Vector) -> Result { } // get the tangent length via some simple trigonometry - route_csc.tangent.magnitude = - ((route_csc.end.circle.center.x - route_csc.start.circle.center.x).powi(2) - + (route_csc.end.circle.center.y - route_csc.start.circle.center.y).powi(2) - - (2.0 * end.magnitude).powi(2)) - .sqrt(); + 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 - .circle - .center - .lerp(route_csc.start.circle.center, 0.5); + 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.circle.center.y - tangent_middle.y) - / (route_csc.end.circle.center.x - tangent_middle.x)) + ((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(), ); @@ -348,7 +313,7 @@ pub fn lsr(end: Vector) -> Result { // 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.circle.center.x < route_csc.start.circle.center.x { + if route_csc.end.center.x < route_csc.start.center.x { route_csc.tangent.angle += Angle::pi(); } @@ -357,9 +322,9 @@ pub fn lsr(end: Vector) -> Result { // 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.circle.center + route_csc.tangent.origin = route_csc.start.center + Rotation::new(route_csc.start.angle) - .transform_vector(Vector2D::new(route_csc.start.circle.radius, 0.0)); + .transform_vector(Vector2D::new(route_csc.start.radius, 0.0)); // get the angle of the end circle route_csc.end.angle = ((Angle::frac_pi_2() - end.angle) - route_csc.tangent.angle).positive();