Exemple #1
0
        //
        // Length calculations for the different paths
        //

        //Basic idea (from "Optimal paths for a car that goes both forwards and backwards"):

        //In each formula the objective is to move from (0, 0, 0) to (x, y, phi)

        //We write (r, theta) = R(x, y) for the polar transform:
        // r * cos(theta) = x
        // r * sin(theta) = y
        // r >= 0
        // -pi <= theta < pi
        //So R(x, y) means that we translate x and y to polar coordinates from the cartesian
        //https://en.wikipedia.org/wiki/Polar_coordinate_system

        //We write phi = M(theta) if phi ≡ theta mod(2*pi) and -pi <= phi < pi, so M(theta) means modulus 2*pi

        //L is the overall length. We say L = ∞ if there's no solution

        //t, u, v are the unknown segment lengths
        //A Reeds-Shepp path consists of 3-5 segments, but only 3 segments have unpredetermined lengths
        //The other segments have each a length of a curve where we drive a length of pi/2
        //or the same length as another segment in the same path
        //The unit of length for a straight segment is whatever we want. For a circular arc it is in radians.



        //8.1: CSC, same turn
        public static float Lf_Sf_Lf(RSCar goal, out PathSegmentLengths path)
        {
            float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);

            float x   = goal.pos.x;
            float y   = goal.pos.z;
            float phi = goal.HeadingInRad;


            //Calculations
            //On page 24 in the Reeds-Shepp report, it says:
            //(u, t) = R(x - sin(phi), y - 1 + cos(phi))
            //v = M(phi - t)
            //L = |t| + |u| + |v|
            //This path can't be optimal if t or v is outside [0, pi]
            R(x - Mathf.Sin(phi), y - 1f + Mathf.Cos(phi), out u, out t);

            v = M(phi - t);


            //Debug.Log(t + " " + u + " " + v);

            //Dont need to check u because its straight
            if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(v))
            {
                return(float.PositiveInfinity);
            }

            path.t = t; path.u = u; path.v = v;

            float totalLength = t + u + v;

            return(totalLength);
        }
        //Loop through all paths and find the shortest one (if one can be found)
        private static float FindShortestPathLength(RSCar carEndMod, out PathSegmentLengths bestPathLengths, out PathWords bestWord)
        {
            //How many paths are we going to check
            int numPathWords = Enum.GetNames(typeof(PathWords)).Length;

            //Better than using float.MaxValue because we can use float.IsPositiveInfinity(bestPathLength) to test if its infinity
            float shortestPathLength = float.PositiveInfinity;

            bestWord = 0;

            //Will keep track of the length of the best path
            //Some Reeds-Shepp segments have 5 lengths, but 2 of those are known, so we only need 3 to find the shortest path
            bestPathLengths = new PathSegmentLengths(0f, 0f, 0f);

            //Loop through all paths that are enums to find the shortest
            for (int w = 0; w < numPathWords; w++)
            {
                PathWords word = (PathWords)w;

                PathSegmentLengths pathSegmentLengths;

                float pathLength = PathLengthMath.GetLength(carEndMod, word, out pathSegmentLengths);

                if (pathLength < shortestPathLength)
                {
                    shortestPathLength = pathLength;
                    bestWord           = word;
                    bestPathLengths    = pathSegmentLengths;
                }
            }

            return(shortestPathLength);
        }
        //Find the shortest Reeds-Shepp path and generate waypoints to follow that path
        private static List <RSCar> FindShortestPath(
            RSCar carStart, RSCar carEnd, RSCar carEndMod, float wpDistance, float turningRadius, bool generateOneWp)
        {
            PathSegmentLengths bestPathLengths;
            PathWords          bestWord;

            float shortestPathLength = FindShortestPathLength(carEndMod, out bestPathLengths, out bestWord);

            //No path could be found
            if (float.IsPositiveInfinity(shortestPathLength))
            {
                Debug.Log("Cant find a Reeds-Shepp path");

                return(null);
            }
            //else
            //{
            //    Debug.Log(bestWord);
            //}


            //Calculate the waypoints to complete this path
            //Use the car's original start position because we no longer need the normalized version
            List <RSCar> shortestPath = AddWaypoints(bestWord, bestPathLengths, carStart, carEnd, wpDistance, turningRadius, generateOneWp);


            return(shortestPath);
        }
        //Same as above but we just want the shortest distance
        public static float GetShortestDistance(Vector3 startPos, float startHeading, Vector3 endPos, float endHeading, float turningRadius)
        {
            RSCar carStart = new RSCar(startPos, startHeading);
            RSCar carEnd   = new RSCar(endPos, endHeading);

            //The formulas assume we move from(0, 0, 0) to(x, y, theta), and that the turning radius is 1
            //This means we have to move and rotate the goal's position and heading as if the start's position and heading had been at (0,0,0)
            RSCar carEndMod = NormalizeGoalCar(carStart, carEnd, turningRadius);

            PathSegmentLengths bestPathLengths;
            PathWords          bestWord;

            float shortestPathLength = FindShortestPathLength(carEndMod, out bestPathLengths, out bestWord);

            //No path could be found
            if (float.IsPositiveInfinity(shortestPathLength))
            {
                Debug.Log("Cant find a Reeds-Shepp path");

                return(shortestPathLength);
            }

            //Convert back to the actual diistance by using the turning radius
            shortestPathLength *= turningRadius;

            return(shortestPathLength);
        }
Exemple #5
0
        //Copy data from car to this car
        public RSCar(RSCar car)
        {
            this.pos     = car.pos;
            this.heading = car.heading;

            this.gear     = car.gear;
            this.steering = car.steering;
        }
Exemple #6
0
        //8.9: C|C(pi/2)SC, same turn
        public static float Lf_Rbpi2_Sb_Lb(RSCar goal, out PathSegmentLengths path)
        {
            float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);

            float x   = goal.pos.x;
            float y   = goal.pos.z;
            float phi = goal.HeadingInRad;


            //Calculations
            //Uses a modified formula adapted from the c_c2sca function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
            float xi  = x - Mathf.Sin(phi);
            float eta = y - 1f + Mathf.Cos(phi);

            float u1, theta;

            R(xi, eta, out u1, out theta);

            float u1Squared = u1 * u1;

            if (u1Squared < 4f)
            {
                return(float.PositiveInfinity);
            }

            //This is the part thats different from the original report:
            float va1 = 1.25f - ((u1 * u1) / 16f);

            if (va1 < 0f || va1 > 1f)
            {
                return(float.PositiveInfinity);
            }

            u = Mathf.Sqrt(u1Squared - 4f) - 2f;

            if (u < 0f)
            {
                return(float.PositiveInfinity);
            }

            float alpha = Mathf.Atan2(2f, u + 2f);

            t = M(HALF_PI + theta + alpha);
            v = M(t + HALF_PI - phi);

            //Check all 2 curves (pi/2 is always a valid curve)
            if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(v))
            {
                return(float.PositiveInfinity);
            }

            path.t = t; path.u = u; path.v = v;

            float totalLength = t + HALF_PI + u + v;

            return(totalLength);
        }
Exemple #7
0
        //8.8: C|CuCu|C
        public static float Lf_Rub_Lub_Rf(RSCar goal, out PathSegmentLengths path)
        {
            float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);

            float x   = goal.pos.x;
            float y   = goal.pos.z;
            float phi = goal.HeadingInRad;


            //Calculations
            //Uses a modified formula adapted from the c_cucu_c function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
            float xi  = x + Mathf.Sin(phi);
            float eta = y - 1f - Mathf.Cos(phi);

            float u1, theta;

            R(xi, eta, out u1, out theta);

            if (u1 > 6f)
            {
                return(float.PositiveInfinity);
            }

            //This is the part thats different from the original report:
            float va1 = 1.25f - ((u1 * u1) / 16f);

            if (va1 < 0f || va1 > 1f)
            {
                return(float.PositiveInfinity);
            }

            u = Mathf.Acos(va1);

            float va2 = Mathf.Sin(u);

            float alpha = Mathf.Asin((2f * va2) / u1);

            t = M(HALF_PI + theta + alpha);
            v = M(t - phi);

            //Check all 3 curves
            if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(u) || IsCurveSegmentInvalid(v))
            {
                return(float.PositiveInfinity);
            }

            path.t = t; path.u = u; path.v = v;

            float totalLength = t + u + u + v;

            return(totalLength);
        }
Exemple #8
0
        //8.7: CCu|CuC
        public static float Lf_Ruf_Lub_Rb(RSCar goal, out PathSegmentLengths path)
        {
            float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);

            float x   = goal.pos.x;
            float y   = goal.pos.z;
            float phi = goal.HeadingInRad;


            //Calculations
            //Uses a modified formula adapted from the ccu_cuc function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
            float xi  = x + Mathf.Sin(phi);
            float eta = y - 1f - Mathf.Cos(phi);

            float u1, theta;

            R(xi, eta, out u1, out theta);

            if (u1 > 4f)
            {
                return(float.PositiveInfinity);
            }

            if (u1 > 2f)
            {
                float alpha = Mathf.Acos((u1 / 4f) - 0.5f);

                t = M(HALF_PI + theta - alpha);
                u = M(PI - alpha);
                v = M(phi - t + 2f * u);
            }
            else
            {
                float alpha = Mathf.Acos((u1 / 4f) + 0.5f);

                t = M(HALF_PI + theta + alpha);
                u = M(alpha);
                v = M(phi - t + 2f * u);
            }

            //Check all 3 curves
            if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(u) || IsCurveSegmentInvalid(v))
            {
                return(float.PositiveInfinity);
            }

            path.t = t; path.u = u; path.v = v;

            float totalLength = t + u + u + v;

            return(totalLength);
        }
Exemple #9
0
        //8.4: CC|C
        public static float Lf_Rf_Lb(RSCar goal, out PathSegmentLengths path)
        {
            float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);

            float x   = goal.pos.x;
            float y   = goal.pos.z;
            float phi = goal.HeadingInRad;


            //Calculations
            //Uses a modified formula adapted from the cc_c function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
            float xi  = x - Mathf.Sin(phi);
            float eta = y - 1f + Mathf.Cos(phi);

            float u1, theta;

            R(xi, eta, out u1, out theta);

            if (u1 > 4f)
            {
                return(float.PositiveInfinity);
            }

            u = Mathf.Acos((8f - (u1 * u1)) / 8f);

            float va = Mathf.Sin(u);

            float alpha = Mathf.Asin(2f * va / u1);

            t = M(HALF_PI - alpha + theta);

            v = M(t - u - phi);

            //Check all 3 curves
            if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(u) || IsCurveSegmentInvalid(v))
            {
                return(float.PositiveInfinity);
            }

            path.t = t; path.u = u; path.v = v;

            float totalLength = t + u + v;

            return(totalLength);
        }
Exemple #10
0
        //8.2: CSC, different turn
        public static float Lf_Sf_Rf(RSCar goal, out PathSegmentLengths path)
        {
            float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);

            float x   = goal.pos.x;
            float y   = goal.pos.z;
            float phi = goal.HeadingInRad;


            //Calculations
            float u1, t1;

            R(x + Mathf.Sin(phi), y - 1f - Mathf.Cos(phi), out u1, out t1);

            if (u1 * u1 < 4f)
            {
                return(float.PositiveInfinity);
            }

            u = Mathf.Sqrt((u1 * u1) - 4f);

            float T, theta;

            R(u, 2f, out T, out theta);

            t = M(t1 + theta);

            v = M(t - phi);


            //Debug.Log(t + " " + u + " " + v);

            if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(v))
            {
                return(float.PositiveInfinity);
            }

            path.t = t; path.u = u; path.v = v;

            float totalLength = t + u + v;

            return(totalLength);
        }
        //The formulas assume we move from(0, 0, 0) to (x, y, theta), and that the turning radius is 1
        //This means we have to move and rotate the goal's position and heading as if the start's position and heading had been at (0,0,0)
        //But we are using Unity, so the rotation of the start car has to be along the x-axis and not z-axis which is Unity's zero-rotation
        public static RSCar NormalizeGoalCar(RSCar carStart, RSCar carEnd, float turningRadius)
        {
            //Change the position and rotation of the goal car
            Vector3 posDiff = carEnd.pos - carStart.pos;

            //Turning radius is 1
            posDiff /= turningRadius;

            //Unitys coordinate is not the same as the one used in the pdf so we have to make som strange translations
            float headingDiff = PathLengthMath.WrapAngleInRadians((2f * Mathf.PI) - (carEnd.HeadingInRad - carStart.HeadingInRad));

            //Rotate the vector between the cars
            //Add 90 degrees because of unitys coordinate system
            Vector3 newEndPos = Quaternion.Euler(0f, -carStart.HeadingInDegrees + 90f, 0f) * posDiff;

            RSCar carEndMod = new RSCar(newEndPos, headingDiff);

            return(carEndMod);
        }
Exemple #12
0
        //8.4: C|CC
        public static float Lf_Rb_Lb(RSCar goal, out PathSegmentLengths path)
        {
            float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);

            float x   = goal.pos.x;
            float y   = goal.pos.z;
            float phi = goal.HeadingInRad;


            //Calculations
            //Uses a modified formula adapted from the c_cc function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
            float xi  = x - Mathf.Sin(phi);
            float eta = y - 1f + Mathf.Cos(phi);

            float u1, theta;

            R(xi, eta, out u1, out theta);

            if (u1 > 4f)
            {
                return(float.PositiveInfinity);
            }

            float alpha = Mathf.Acos(u1 / 4f);

            t = M(HALF_PI + alpha + theta);
            u = M(PI - 2f * alpha);
            //This part is the only thing thats different from 8.3
            v = M(t + u - phi);

            //Check all 3 curves
            if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(u) || IsCurveSegmentInvalid(v))
            {
                return(float.PositiveInfinity);
            }

            path.t = t; path.u = u; path.v = v;

            float totalLength = t + u + v;

            return(totalLength);
        }
        /// <summary>
        /// Generate the shortest Reeds-Shepp path
        /// </summary>
        /// <param name="startPos">The position of the car where the path starts</param>
        /// <param name="startHeading">The heading of the car where the path starts [rad]</param>
        /// <param name="endPos">The position of the car where the path ends</param>
        /// <param name="endHeading">The heading of the car where the path ends [rad]</param>
        /// <param name="turningRadius">The truning radius of the car [m]</param>
        /// <param name="wpDistance">The distance between the waypoints that make up the final path</param>
        /// <param name="generateOneWp">Should we generate just 1 waypoint and not all</param>
        /// <returns></returns>
        public static List <RSCar> GetShortestPath(Vector3 startPos, float startHeading, Vector3 endPos, float endHeading, float turningRadius, float wpDistance, bool generateOneWp)
        {
            RSCar carStart = new RSCar(startPos, startHeading);
            RSCar carEnd   = new RSCar(endPos, endHeading);

            //The formulas assume we move from(0, 0, 0) to(x, y, theta), and that the turning radius is 1
            //This means we have to move and rotate the goal's position and heading as if the start's position and heading had been at (0,0,0)
            RSCar carEndMod = NormalizeGoalCar(carStart, carEnd, turningRadius);


            //int startTime = Environment.TickCount;

            List <RSCar> shortestPath = FindShortestPath(carStart, carEnd, carEndMod, wpDistance, turningRadius, generateOneWp);

            //float timeInSeconds = (float)(Environment.TickCount - startTime) / 1000f;

            //Debug.Log("Ticks to generate <b>Reeds-Shepp:</b> " + (Environment.TickCount - startTime));

            return(shortestPath);
        }
Exemple #14
0
        //8.10 (reversed): CSC(pi/2)|C, different turn
        public static float Lf_Sf_Lfpi2_Rb(RSCar goal, out PathSegmentLengths path)
        {
            float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);

            float x   = goal.pos.x;
            float y   = goal.pos.z;
            float phi = goal.HeadingInRad;


            //Calculations
            //Uses a modified formula adapted from the csc2_cb function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
            float xi  = x + Mathf.Sin(phi);
            float eta = y - 1f - Mathf.Cos(phi);

            float u1, theta;

            R(xi, eta, out u1, out theta);

            if (u1 < 2f)
            {
                return(float.PositiveInfinity);
            }

            //This is the part thats different from the original report:
            t = M(theta);
            u = u1 - 2f;
            v = M(-t - HALF_PI + phi);


            //Check all 2 curves
            if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(v))
            {
                return(float.PositiveInfinity);
            }

            path.t = t; path.u = u; path.v = v;

            float totalLength = t + u + HALF_PI + v;

            return(totalLength);
        }
        //Add waypoints to a given path
        private static List <RSCar> AddWaypoints(
            PathWords word, PathSegmentLengths pathSegmentLengths, RSCar carStart, RSCar carEnd, float wpDistance, float turningRadius, bool generateOneWp)
        {
            //Find the car settings we need to drive through the path
            List <SegmentSettings> pathSettings = PathSettings.GetSettings(word, pathSegmentLengths);

            if (pathSettings == null)
            {
                Debug.Log("Cant find settings for a path");

                return(null);
            }


            //Generate the waypoints

            //Data used when generating the path
            //The pos and heading we will move along the path
            Vector3 pos     = carStart.pos;
            float   heading = carStart.HeadingInRad;
            //The distance between each step we take when generating the path, the smaller the better, but is also slower
            float stepDistance = 0.05f;
            //To generate waypoints with a certain distance between them we need to know how far we have driven since the last wp
            float driveDistance = 0f;


            //The waypoints
            List <RSCar> waypoints = new List <RSCar>();

            //Add the first wp
            waypoints.Add(new RSCar(pos, heading, pathSettings[0].gear, pathSettings[0].steering));


            //Loop through all path 3-5 path segments
            for (int i = 0; i < pathSettings.Count; i++)
            {
                SegmentSettings segmentSettings = pathSettings[i];

                //How many steps will we take to generate this segment
                //Will always be at least 2 no matter the stepDistance
                int n = (int)Math.Ceiling((segmentSettings.length * turningRadius) / stepDistance);

                //How far will we move each step?
                float stepLength = (segmentSettings.length * turningRadius) / n;

                //Change stuff depending on in which direction we are moving
                float steeringWheelPos = 1f;

                if (segmentSettings.steering == RSCar.Steering.Left)
                {
                    steeringWheelPos = -1f;
                }

                //Invert steering if we are reversing
                if (segmentSettings.gear == RSCar.Gear.Back)
                {
                    steeringWheelPos *= -1f;
                }


                //Drive through this segment in small steps
                for (int j = 0; j < n; j++)
                {
                    //Update position
                    float dx = stepLength * Mathf.Sin(heading);
                    float dz = stepLength * Mathf.Cos(heading);

                    if (segmentSettings.gear == RSCar.Gear.Back)
                    {
                        dx = -dx;
                        dz = -dz;
                    }

                    pos = new Vector3(pos.x + dx, pos.y, pos.z + dz);


                    //Update heading if we are turning
                    if (segmentSettings.steering != RSCar.Steering.Straight)
                    {
                        heading = heading + (stepLength / turningRadius) * steeringWheelPos;
                    }


                    //Should we generate a new wp?
                    driveDistance += stepLength;

                    if (driveDistance > wpDistance)
                    {
                        waypoints.Add(new RSCar(pos, heading, segmentSettings.gear, segmentSettings.steering));

                        driveDistance = driveDistance - wpDistance;

                        if (generateOneWp)
                        {
                            return(waypoints);
                        }
                    }
                }

                //We also need to add the last pos of this segment as waypoint or the path will not be the same
                //if we for example are ignoring the waypoint where we change direction
                waypoints.Add(new RSCar(pos, heading, segmentSettings.gear, segmentSettings.steering));
            }


            //Move the last wp pos to the position of the goal car
            //When we generate waypoints, the accuracy depends on the stepDistance, so is not always hitting the goal exactly
            waypoints[waypoints.Count - 1].pos = carEnd.pos;


            return(waypoints);
        }
Exemple #16
0
        //Could maybe optimize sin(phi) and cos(phi) because they are always the same

        public static float GetLength(RSCar car, PathWords word, out PathSegmentLengths pathLengths)
        {
            switch (word)
            {
            //8.1: CSC, same turn
            case PathWords.Lf_Sf_Lf: return(Lf_Sf_Lf(car, out pathLengths));

            case PathWords.Lb_Sb_Lb: return(Lf_Sf_Lf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Sf_Rf: return(Lf_Sf_Lf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Sb_Rb: return(Lf_Sf_Lf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.2: CSC, different turn
            case PathWords.Lf_Sf_Rf: return(Lf_Sf_Rf(car, out pathLengths));

            case PathWords.Lb_Sb_Rb: return(Lf_Sf_Rf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Sf_Lf: return(Lf_Sf_Rf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Sb_Lb: return(Lf_Sf_Rf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.3: C|C|C
            case PathWords.Lf_Rb_Lf: return(Lf_Rb_Lf(car, out pathLengths));

            case PathWords.Lb_Rf_Lb: return(Lf_Rb_Lf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Lb_Rf: return(Lf_Rb_Lf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Lf_Rb: return(Lf_Rb_Lf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.4: C|CC
            case PathWords.Lf_Rb_Lb: return(Lf_Rb_Lb(car, out pathLengths));

            case PathWords.Lb_Rf_Lf: return(Lf_Rb_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Lb_Rb: return(Lf_Rb_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Lf_Rf: return(Lf_Rb_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.4: CC|C
            case PathWords.Lf_Rf_Lb: return(Lf_Rf_Lb(car, out pathLengths));

            case PathWords.Lb_Rb_Lf: return(Lf_Rf_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Lf_Rb: return(Lf_Rf_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Lb_Rf: return(Lf_Rf_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.7: CCu|CuC
            case PathWords.Lf_Ruf_Lub_Rb: return(Lf_Ruf_Lub_Rb(car, out pathLengths));

            case PathWords.Lb_Rub_Luf_Rf: return(Lf_Ruf_Lub_Rb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Luf_Rub_Lb: return(Lf_Ruf_Lub_Rb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Lub_Ruf_Lf: return(Lf_Ruf_Lub_Rb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.8: C|CuCu|C
            case PathWords.Lf_Rub_Lub_Rf: return(Lf_Rub_Lub_Rf(car, out pathLengths));

            case PathWords.Lb_Ruf_Luf_Rb: return(Lf_Rub_Lub_Rf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Lub_Rub_Lf: return(Lf_Rub_Lub_Rf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Luf_Ruf_Lb: return(Lf_Rub_Lub_Rf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.9: C|C(pi/2)SC, same turn
            case PathWords.Lf_Rbpi2_Sb_Lb: return(Lf_Rbpi2_Sb_Lb(car, out pathLengths));

            case PathWords.Lb_Rfpi2_Sf_Lf: return(Lf_Rbpi2_Sb_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Lbpi2_Sb_Rb: return(Lf_Rbpi2_Sb_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Lfpi2_Sf_Rf: return(Lf_Rbpi2_Sb_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.10: C|C(pi/2)SC, different turn
            case PathWords.Lf_Rbpi2_Sb_Rb: return(Lf_Rbpi2_Sb_Rb(car, out pathLengths));

            case PathWords.Lb_Rfpi2_Sf_Rf: return(Lf_Rbpi2_Sb_Rb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Lbpi2_Sb_Lb: return(Lf_Rbpi2_Sb_Rb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Lfpi2_Sf_Lf: return(Lf_Rbpi2_Sb_Rb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.9(reversed): CSC(pi / 2) | C, same turn
            case PathWords.Lf_Sf_Rfpi2_Lb: return(Lf_Sf_Rfpi2_Lb(car, out pathLengths));

            case PathWords.Lb_Sb_Rbpi2_Lf: return(Lf_Sf_Rfpi2_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Sf_Lfpi2_Rb: return(Lf_Sf_Rfpi2_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Sb_Lbpi2_Rf: return(Lf_Sf_Rfpi2_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.10 (reversed): CSC(pi/2)|C, different turn
            case PathWords.Lf_Sf_Lfpi2_Rb: return(Lf_Sf_Lfpi2_Rb(car, out pathLengths));

            case PathWords.Lb_Sb_Lbpi2_Rf: return(Lf_Sf_Lfpi2_Rb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Sf_Rfpi2_Lb: return(Lf_Sf_Lfpi2_Rb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Sb_Rbpi2_Lf: return(Lf_Sf_Lfpi2_Rb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));


            //8.11: C | C(pi / 2)SC(pi / 2) | C
            case PathWords.Lf_Rbpi2_Sb_Lbpi2_Rf: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car, out pathLengths));

            case PathWords.Lb_Rfpi2_Sf_Lfpi2_Rb: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rf_Lbpi2_Sb_Rbpi2_Lf: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));

            case PathWords.Rb_Lfpi2_Sf_Rfpi2_Lb: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
            }

            pathLengths = new PathSegmentLengths(0f, 0f, 0f);

            return(float.PositiveInfinity);
        }
Exemple #17
0
        //Change car data
        public RSCar ChangeData(float newXPos, float newZPos, float newHeading)
        {
            RSCar carCopy = new RSCar(new Vector3(newXPos, pos.y, newZPos), newHeading);

            return(carCopy);
        }