コード例 #1
0
        //LRL - Find both tangent positions and the position of the 3rd circle
        private void Get_LRL_Length()
        {
            vec2 startTangent = new vec2(0, 0);
            vec2 goalTangent  = new vec2(0, 0);
            vec2 middleCircle = new vec2(0, 0);

            DubinsMath.GetRLRorLRLTangents(
                startLeftCircle,
                goalLeftCircle,
                true,
                out startTangent,
                out goalTangent,
                out middleCircle);

            //Calculate the total length of this path
            double length1 = DubinsMath.GetArcLength(startLeftCircle, startPos, startTangent, true);
            double length2 = DubinsMath.GetArcLength(middleCircle, startTangent, goalTangent, false);
            double length3 = DubinsMath.GetArcLength(goalLeftCircle, goalTangent, goalPos, true);

            //Save the data
            OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.LRL);

            //We also need this data to simplify when generating the final path
            pathData.segment2Turning = true;

            //LRL
            pathData.SetIfTurningRight(false, true, false);

            //Add the path to the collection of all paths
            pathDataList.Add(pathData);
        }
コード例 #2
0
        //LSR
        private void Get_LSR_Length()
        {
            //Find both tangent positions
            vec2 startTangent = new vec2(0, 0);
            vec2 goalTangent  = new vec2(0, 0);

            DubinsMath.RSLorLSR(startLeftCircle, goalRightCircle, true, out startTangent, out goalTangent);

            //Calculate lengths
            double length1 = DubinsMath.GetArcLength(startLeftCircle, startPos, startTangent, true);
            double length2 = (startTangent - goalTangent).GetLength();
            double length3 = DubinsMath.GetArcLength(goalRightCircle, goalTangent, goalPos, false);

            //Save the data
            OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.LSR);

            //We also need this data to simplify when generating the final path
            pathData.segment2Turning = false;

            //LSR
            pathData.SetIfTurningRight(false, false, true);

            //Add the path to the collection of all paths
            pathDataList.Add(pathData);
        }
コード例 #3
0
ファイル: CDubins.cs プロジェクト: hagre/AgOpenGPS
        //RLR - Find both tangent positions and the position of the 3rd circle
        private void Get_RLR_Length()
        {
            DubinsMath.GetRLRorLRLTangents(
                startRightCircle,
                goalRightCircle,
                false,
                out vec2 startTangent,
                out vec2 goalTangent,
                out vec2 middleCircle);

            //Calculate lengths
            double length1 = DubinsMath.GetArcLength(startRightCircle, startPos, startTangent, false);
            double length2 = DubinsMath.GetArcLength(middleCircle, startTangent, goalTangent, true);
            double length3 = DubinsMath.GetArcLength(goalRightCircle, goalTangent, goalPos, false);

            //Save the data
            OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.RLR)
            {
                //We also need this data to simplify when generating the final path
                segment2Turning = true
            };

            //RLR
            pathData.SetIfTurningRight(true, false, true);

            //Add the path to the collection of all paths
            pathDataList.Add(pathData);
        }
コード例 #4
0
        //Find the coordinates of the entire path from the 2 tangents and length of each segment
        private void GetTotalPath(OneDubinsPath pathData)
        {
            //Store the waypoints of the final path here
            List <vec2> finalPath = new List <vec2>();

            //Start position of the car
            vec2 currentPos = startPos;
            //Start heading of the car
            double theta = startHeading;

            //We always have to add the first position manually
            finalPath.Add(currentPos);

            //How many line segments can we fit into this part of the path

            //First
            int segments = (int)Math.Floor(pathData.length1 / CDubins.driveDistance);

            DubinsMath.AddCoordinatesToPath(
                ref currentPos,
                ref theta,
                finalPath,
                segments,
                true,
                pathData.segment1TurningRight);

            //Second
            segments = (int)Math.Floor(pathData.length2 / CDubins.driveDistance);

            DubinsMath.AddCoordinatesToPath(
                ref currentPos,
                ref theta,
                finalPath,
                segments,
                pathData.segment2Turning,
                pathData.segment2TurningRight);

            //Third
            segments = (int)Math.Floor(pathData.length3 / CDubins.driveDistance);

            DubinsMath.AddCoordinatesToPath(
                ref currentPos,
                ref theta,
                finalPath,
                segments,
                true,
                pathData.segment3TurningRight);

            //Add the final goal coordinate
            finalPath.Add(new vec2(goalPos.easting, goalPos.northing));

            //Save the final path in the path data
            pathData.pathCoordinates = finalPath;
        }
コード例 #5
0
        //Position the left and right circles that are to the left/right of the target and the car
        private void PositionLeftRightCircles()
        {
            //Goal pos
            goalRightCircle = DubinsMath.GetRightCircleCenterPos(goalPos, goalHeading);

            goalLeftCircle = DubinsMath.GetLeftCircleCenterPos(goalPos, goalHeading);

            //Start pos
            startRightCircle = DubinsMath.GetRightCircleCenterPos(startPos, startHeading);

            startLeftCircle = DubinsMath.GetLeftCircleCenterPos(startPos, startHeading);
        }