Update Cargo.toml

- start of the rsr vector was wrong because I took the wrong angle
- shortest path was always rsr because I didn't replace them properly in
  the first place

Cargo.toml

@@ -9,13 +9,14 @@ keywords = ["pathfinder", "dubinspath"]
 categories = ["algorithms"]
 readme = "README.md"
 license = "Apache-2.0/MIT"
-authors = ["Roman Kretschmer <gnxlxnxx@web.de>"]
+authors = ["Roman Kretschmer <roman@kretschmer.email>"]
 edition = "2018"
 
 
 
 [dependencies]
 thiserror = "1.0"
-euclid = "0.20.14"
+euclid = "0.21.0"
+num-traits = { version = "0.2.10", default-features = false }
 
 [dev-dependencies]

src/lib.rs

@@ -4,7 +4,7 @@
 //!
 //! The arguments to get a path are
 //!
-//!   - radius: the minimum radius you can drive or respectively the radius you want to drive (f64)
+//!   - radius: the minimum radius you can drive or respectively the radius you want to drive (T)
 //!   - end_point: the point you want to end up at (Point)
 //!   - end_angle: the angle you want to have in the end (Angle)
 //!
@@ -30,13 +30,25 @@
 //!
 //!
 
-use euclid::{approxeq::ApproxEq, Point2D, Rotation2D, UnknownUnit};
+use std::{
+    cmp::PartialOrd,
+    convert,
+    ops::{Add, Mul, Rem, Sub},
+};
+
+use convert::From;
+pub use euclid::Angle;
+use euclid::{approxeq::ApproxEq, Point2D, Rotation2D, Trig, UnknownUnit, Vector2D};
+use num_traits::{
+    self,
+    float::{Float, FloatConst},
+    Zero,
+};
 use thiserror::Error;
 
-pub type Angle = euclid::Angle<f64>;
-pub type Point = Point2D<f64, UnknownUnit>;
-pub type Vector = euclid::Vector2D<f64, UnknownUnit>;
-type Rotation = Rotation2D<f64, UnknownUnit, UnknownUnit>;
+pub type Point<T> = Point2D<T, UnknownUnit>;
+pub type Vector<T> = Vector2D<T, UnknownUnit>;
+type Rotation<T> = Rotation2D<T, UnknownUnit, UnknownUnit>;
 
 #[derive(Debug, Error)]
 pub enum Error {
@@ -48,12 +60,12 @@ pub enum Error {
 
 /// Vector with origin, angle and magnitude
 #[derive(Debug, Copy, Clone)]
-pub struct StraightPath {
-    pub origin: Point,
-    pub vector: Vector,
+pub struct StraightPath<T> {
+    pub origin: Point<T>,
+    pub vector: Vector<T>,
 }
 
-impl StraightPath {
+impl<T: ApproxEq<T>> StraightPath<T> {
     /// approximate equality to other Vector
     pub fn approx_eq(&self, other: Self) -> bool {
         ApproxEq::approx_eq(&self.vector, &other.vector)
@@ -63,15 +75,37 @@ impl StraightPath {
 
 /// Circle vector (Circle + Angle)
 #[derive(Debug, Copy, Clone)]
-pub struct CirclePath {
-    pub center: Point,
-    pub radius: f64,
-    pub angle: Angle,
+pub struct CirclePath<T>
+where
+    T: Mul<T, Output = T>
+        + ApproxEq<T>
+        + Rem<Output = T>
+        + Sub<Output = T>
+        + Add<Output = T>
+        + Zero
+        + FloatConst
+        + PartialOrd
+        + Copy,
+{
+    pub center: Point<T>,
+    pub radius: T,
+    pub angle: Angle<T>,
 }
 
-impl CirclePath {
+impl<T> CirclePath<T>
+where
+    T: Mul<T, Output = T>
+        + ApproxEq<T>
+        + Rem<Output = T>
+        + Sub<Output = T>
+        + Add<Output = T>
+        + Zero
+        + FloatConst
+        + PartialOrd
+        + Copy,
+{
     ///get the length of the circle vector
-    pub fn get_length(&self) -> f64 {
+    pub fn get_length(&self) -> T {
         self.angle.radians * self.radius
     }
     /// approximate equality to other CirclePath
@@ -92,31 +126,78 @@ impl CirclePath {
 
 /// Route with a start Circle, a tangent straight and a end Circle
 #[derive(Debug, Copy, Clone)]
-pub struct RouteCSC {
-    pub start: CirclePath,
-    pub tangent: StraightPath,
-    pub end: CirclePath,
+pub struct RouteCSC<T>
+where
+    T: Mul<T, Output = T>
+        + Mul
+        + ApproxEq<T>
+        + Rem<Output = T>
+        + Sub<Output = T>
+        + Add<Output = T>
+        + Zero
+        + FloatConst
+        + PartialOrd
+        + Copy,
+{
+    pub start: CirclePath<T>,
+    pub tangent: StraightPath<T>,
+    pub end: CirclePath<T>,
 }
 
 /// Route with 3 Circles
 #[derive(Debug, Copy, Clone)]
-pub struct RouteCCC {
-    pub start: CirclePath,
-    pub middle: CirclePath,
-    pub end: CirclePath,
+pub struct RouteCCC<T>
+where
+    T: Mul<T, Output = T>
+        + Mul
+        + ApproxEq<T>
+        + Rem<Output = T>
+        + Sub<Output = T>
+        + Add<Output = T>
+        + Zero
+        + FloatConst
+        + PartialOrd
+        + Copy,
+{
+    pub start: CirclePath<T>,
+    pub middle: CirclePath<T>,
+    pub end: CirclePath<T>,
 }
 
 #[derive(Debug, Copy, Clone)]
-pub enum Path {
-    CSC(RouteCSC),
-    CCC(RouteCCC),
+pub enum Path<T>
+where
+    T: Mul<T, Output = T>
+        + Mul
+        + ApproxEq<T>
+        + Rem<Output = T>
+        + Sub<Output = T>
+        + Add<Output = T>
+        + Zero
+        + FloatConst
+        + PartialOrd
+        + Copy,
+{
+    CSC(RouteCSC<T>),
+    CCC(RouteCCC<T>),
 }
 
 /// Route with a start Circle, a tangent straight and a end Circle
-impl RouteCSC {
+impl<T> RouteCSC<T>
+where
+    T: Add
+        + Mul
+        + Mul<f64, Output = T>
+        + FloatConst
+        + Float
+        + PartialOrd
+        + From<f64>
+        + ApproxEq<T>
+        + Trig,
+{
     /// right straight right route
-    pub fn rsr(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
-        let start_center = Point::new(radius, 0.0);
+    pub fn rsr(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Result<Self, Error> {
+        let start_center = Point::new(radius, 0.0.into());
 
         // 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
@@ -125,7 +206,7 @@ impl RouteCSC {
         let end_center = end_point
             + Rotation::new(end_angle)
                 .inverse()
-                .transform_vector(Vector::new(radius, 0.0));
+                .transform_vector(Vector::new(radius, 0.0.into()));
 
         // get the tangent pitch which is the same as the pitch between the two
         // circle centers since our circles have the same radius
@@ -137,7 +218,7 @@ impl RouteCSC {
         // start circle center x value
         // the angle would be rotated by π so to prevent that:
         if end_center.x < start_center.x {
-            tangent_angle += Angle::pi();
+            tangent_angle = tangent_angle + Angle::pi();
         }
 
         // get the tangent magnitude this, again, is the same as the distance
@@ -152,7 +233,8 @@ impl RouteCSC {
         // 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
         let tangent_origin = start_center
-            + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(radius, 0.0));
+            + Rotation::new(Angle::pi() - start_angle)
+                .transform_vector(Vector::new(radius, 0.0.into()));
 
         // get the angle of the start circle
         // the angle where we start from the tangent equals the one we finish
@@ -162,7 +244,7 @@ impl RouteCSC {
         Ok(Self {
             start: CirclePath {
                 center: start_center,
-                radius: radius,
+                radius,
                 angle: start_angle,
             },
             tangent: StraightPath {
@@ -171,22 +253,23 @@ impl RouteCSC {
             },
             end: CirclePath {
                 center: end_center,
-                radius: radius,
+                radius,
                 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);
+    pub fn lsl(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Result<Self, Error> {
+        let start_center = Point::new(-radius, 0.0.into());
 
         // 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
         let end_center = end_point
-            + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(radius, 0.0));
+            + Rotation::new(Angle::pi() - end_angle)
+                .transform_vector(Vector::new(radius, 0.0.into()));
 
         // get the tangent pitch which is the same as the pitch between the two
         // circle centers since our circles have the same radius
@@ -213,8 +296,8 @@ impl RouteCSC {
 
         // 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
-        let tangent_origin =
-            start_center + Rotation::new(start_angle).transform_vector(Vector::new(radius, 0.0));
+        let tangent_origin = start_center
+            + Rotation::new(start_angle).transform_vector(Vector::new(radius, 0.0.into()));
 
         // get the angle of the start circle
         // the angle where we start from the tangent equals the one we finish
@@ -224,7 +307,7 @@ impl RouteCSC {
         Ok(Self {
             start: CirclePath {
                 center: start_center,
-                radius: radius,
+                radius,
                 angle: start_angle,
             },
             tangent: StraightPath {
@@ -233,27 +316,28 @@ impl RouteCSC {
             },
             end: CirclePath {
                 center: end_center,
-                radius: radius,
+                radius,
                 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);
+    pub fn rsl(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Result<Self, Error> {
+        let start_center = Point::new(radius, 0.0.into());
 
         // 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
         let end_center = end_point
-            + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(radius, 0.0));
+            + Rotation::new(Angle::pi() - end_angle)
+                .transform_vector(Vector::new(radius, 0.0.into()));
 
         // check if inside tangent can even be constructed
         if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2))
             .sqrt()
-            < 2.0 * radius
+            < radius * 2.0
         {
             return Err(Error::CirclesTooClose);
         }
@@ -261,23 +345,23 @@ impl RouteCSC {
         // get the tangent length via some simple trigonometry
         let tangent_magnitude = ((end_center.x - start_center.x).powi(2)
             + (end_center.y - start_center.y).powi(2)
-            - (2.0 * radius).powi(2))
+            - (radius * 2.0).powi(2))
         .sqrt();
 
         // tangent middle is the same as the middle of the straight from the center of the start
-        let tangent_middle = end_center.lerp(start_center, 0.5);
+        let tangent_middle = end_center.lerp(start_center, 0.5.into());
 
         // get the tangent angle
         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(),
+                - (radius * 2.0 / 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 end_center.x < start_center.x {
-            tangent_angle += Angle::pi();
+            tangent_angle = tangent_angle + Angle::pi();
         }
 
         // get the angle of the start circle
@@ -286,7 +370,8 @@ impl RouteCSC {
         // get the tangent origin by moving the vector from the start circle center
         // along its right angle vector
         let tangent_origin = start_center
-            + Rotation::new(Angle::pi() - start_angle).transform_vector(Vector::new(radius, 0.0));
+            + Rotation::new(Angle::pi() - start_angle)
+                .transform_vector(Vector::new(radius, 0.0.into()));
 
         // get the angle of the end circle
         let end_angle = ((Angle::frac_pi_2() - end_angle) - tangent_angle).positive();
@@ -294,7 +379,7 @@ impl RouteCSC {
         Ok(Self {
             start: CirclePath {
                 center: start_center,
-                radius: radius,
+                radius,
                 angle: start_angle,
             },
             tangent: StraightPath {
@@ -303,15 +388,15 @@ impl RouteCSC {
             },
             end: CirclePath {
                 center: end_center,
-                radius: radius,
+                radius,
                 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);
+    pub fn lsr(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Result<Self, Error> {
+        let start_center = Point::new(-radius, 0.0.into());
 
         // 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
@@ -320,12 +405,12 @@ impl RouteCSC {
         let end_center = end_point
             + Rotation::new(end_angle)
                 .inverse()
-                .transform_vector(Vector::new(radius, 0.0));
+                .transform_vector(Vector::new(radius, 0.0.into()));
 
         // check if inside tangent can even be constructed
         if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2))
             .sqrt()
-            < 2.0 * radius
+            < radius * 2.0
         {
             return Err(Error::CirclesTooClose);
         }
@@ -333,23 +418,23 @@ impl RouteCSC {
         // get the tangent length via some simple trigonometry
         let tangent_magnitude = ((end_center.x - start_center.x).powi(2)
             + (end_center.y - start_center.y).powi(2)
-            - (2.0 * radius).powi(2))
+            - (radius * 2.0).powi(2))
         .sqrt();
 
         // tangent middle is the same as the middle of the straight from the center of the start
-        let tangent_middle = end_center.lerp(start_center, 0.5);
+        let tangent_middle = end_center.lerp(start_center, 0.5.into());
 
         // get the tangent angle
         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(),
+                + (radius * 2.0 / 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 end_center.x < start_center.x {
-            tangent_angle += Angle::pi();
+            tangent_angle = tangent_angle + Angle::pi();
         }
 
         // get the angle of the start circle
@@ -357,8 +442,8 @@ impl RouteCSC {
 
         // 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
-        let tangent_origin =
-            start_center + Rotation::new(start_angle).transform_vector(Vector::new(radius, 0.0));
+        let tangent_origin = start_center
+            + Rotation::new(start_angle).transform_vector(Vector::new(radius, 0.0.into()));
 
         // get the angle of the end circle
         let end_angle = ((Angle::frac_pi_2() - end_angle) - tangent_angle).positive();
@@ -366,7 +451,7 @@ impl RouteCSC {
         Ok(Self {
             start: CirclePath {
                 center: start_center,
-                radius: radius,
+                radius,
                 angle: start_angle,
             },
             tangent: StraightPath {
@@ -375,25 +460,29 @@ impl RouteCSC {
             },
             end: CirclePath {
                 center: end_center,
-                radius: radius,
+                radius,
                 angle: end_angle,
             },
         })
     }
 
     /// get the length of the path
-    pub fn get_length(&self) -> f64 {
+    pub fn get_length(&self) -> T {
         self.start.get_length() + self.tangent.vector.length() + self.end.get_length()
     }
 
     /// get the shortest circle straight circle route
-    pub fn get_shortest(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
+    pub fn get_shortest(
+        radius: T,
+        end_point: Point<T>,
+        end_angle: Angle<T>,
+    ) -> Result<Self, Error> {
         let mut route_csc;
 
         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);
+        let route_lsl = Self::lsl(radius, end_point, end_angle).unwrap();
+        let route_lsr = Self::lsr(radius, end_point, end_angle);
+        let route_rsl = Self::rsl(radius, end_point, end_angle);
 
         route_csc = route_rsr;
         if route_lsl.get_length() < route_csc.get_length() {
@@ -415,10 +504,21 @@ impl RouteCSC {
 }
 
 /// Route with 3 Circles
-impl RouteCCC {
+impl<T> RouteCCC<T>
+where
+    T: Add
+        + Mul
+        + Mul<f64, Output = T>
+        + FloatConst
+        + Float
+        + PartialOrd
+        + From<f64>
+        + ApproxEq<T>
+        + Trig,
+{
     /// right left right route (not working yet)
-    pub fn rlr(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
-        let start_center = Point::new(radius, 0.0);
+    pub fn rlr(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Result<Self, Error> {
+        let start_center = Point::new(radius, 0.0.into());
 
         // 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
@@ -427,17 +527,17 @@ impl RouteCCC {
         let end_center = end_point
             + Rotation::new(end_angle)
                 .inverse()
-                .transform_vector(Vector::new(radius, 0.0));
+                .transform_vector(Vector::new(radius, 0.0.into()));
 
         // check if path can be constructed or if the circles are too far apart
         if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2))
             .sqrt()
-            > (4.0 * radius)
+            > (radius * 4.0)
         {
             return Err(Error::CirclesTooFarApart);
         }
 
-        let vector_start_center_middle_center: Vector;
+        let vector_start_center_middle_center: Vector<T>;
 
         let middle_center = {
             let vector_start_center_end_center =
@@ -445,11 +545,11 @@ impl RouteCCC {
 
             let vector_start_center_middle_center_angle =
                 vector_start_center_end_center.angle_from_x_axis().radians
-                    + (vector_start_center_end_center.length() / (4.0 * radius)).acos();
+                    + (vector_start_center_end_center.length() / (radius * 4.0)).acos();
 
             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(),
+                (radius * 2.0) * Trig::cos(vector_start_center_middle_center_angle),
+                (radius * 2.0) * Trig::sin(vector_start_center_middle_center_angle),
             );
 
             Point::new(
@@ -482,42 +582,43 @@ impl RouteCCC {
         Ok(Self {
             start: CirclePath {
                 center: start_center,
-                radius: radius,
+                radius,
                 angle: start_angle,
             },
             middle: CirclePath {
                 center: middle_center,
-                radius: radius,
+                radius,
                 angle: middle_angle,
             },
             end: CirclePath {
                 center: end_center,
-                radius: radius,
+                radius,
                 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);
+    pub fn lrl(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Result<Self, Error> {
+        let start_center = Point::new(-radius, 0.0.into());
 
         // 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
         let end_center = end_point
-            + Rotation::new(Angle::pi() - end_angle).transform_vector(Vector::new(radius, 0.0));
+            + Rotation::new(Angle::pi() - end_angle)
+                .transform_vector(Vector::new(radius, 0.0.into()));
 
         // check if path can be constructed or if the circles are too far apart
         if ((end_center.x - start_center.x).powi(2) + (end_center.y - start_center.y).powi(2))
             .sqrt()
-            > (4.0 * radius)
+            > (radius * 4.0)
         {
             return Err(Error::CirclesTooFarApart);
         }
 
-        let vector_start_center_middle_center: Vector;
+        let vector_start_center_middle_center: Vector<T>;
 
         let middle_center = {
             let vector_start_center_end_center =
@@ -525,11 +626,11 @@ impl RouteCCC {
 
             let vector_start_center_middle_center_angle =
                 vector_start_center_end_center.angle_from_x_axis().radians
-                    - (vector_start_center_end_center.length() / (4.0 * radius)).acos();
+                    - (vector_start_center_end_center.length() / (radius * 4.0)).acos();
 
             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(),
+                (radius * 2.0) * Trig::cos(vector_start_center_middle_center_angle),
+                (radius * 2.0) * Trig::sin(vector_start_center_middle_center_angle),
             );
 
             Point::new(
@@ -558,29 +659,33 @@ impl RouteCCC {
         Ok(Self {
             start: CirclePath {
                 center: start_center,
-                radius: radius,
+                radius,
                 angle: start_angle,
             },
             middle: CirclePath {
                 center: middle_center,
-                radius: radius,
+                radius,
                 angle: middle_angle,
             },
             end: CirclePath {
                 center: end_center,
-                radius: radius,
+                radius,
                 angle: end_angle,
             },
         })
     }
 
     /// get the length of the path
-    pub fn get_length(&self) -> f64 {
+    pub fn get_length(&self) -> T {
         self.start.get_length() + self.middle.get_length() + self.end.get_length()
     }
 
     /// get the shortest circle circle circle route
-    pub fn get_shortest(radius: f64, end_point: Point, end_angle: Angle) -> Result<Self, Error> {
+    pub fn get_shortest(
+        radius: T,
+        end_point: Point<T>,
+        end_angle: Angle<T>,
+    ) -> Result<Self, Error> {
         let route_rlr = Self::rlr(radius, end_point, end_angle);
         let route_lrl = Self::lrl(radius, end_point, end_angle);
 
@@ -603,7 +708,18 @@ impl RouteCCC {
 }
 
 /// get the shortest path
-pub fn get_shortest(radius: f64, end_point: Point, end_angle: Angle) -> Path {
+pub fn get_shortest<T>(radius: T, end_point: Point<T>, end_angle: Angle<T>) -> Path<T>
+where
+    T: Add
+        + Mul
+        + Mul<f64, Output = T>
+        + FloatConst
+        + Float
+        + PartialOrd
+        + From<f64>
+        + ApproxEq<T>
+        + Trig,
+{
     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 {

tests/unit_test.rs

@@ -35,7 +35,7 @@ mod tests {
         let radius = 0.5;
         let end_point = Point::new(-7.0 * radius, 0.0);
         let end_angle = Angle::zero();
-        RouteCCC::rlr(radius, end_point, end_angle).unwrap();
+        RouteCCC::lrl(radius, end_point, end_angle).unwrap();
     }
 
     #[test]