//LSR
        void Get_LSR_Length(Vector3 startCircle, Vector3 goalCircle, List <OneDubinsPath> finalPathList)
        {
            //Find both tangent positions
            Vector3 startTangent = Vector3.zero;
            Vector3 goalTangent  = Vector3.zero;

            dubinsMath.RSLorLSR(startCircle, goalCircle, true, out startTangent, out goalTangent);

            //Calculate lengths
            float length1 = dubinsMath.GetArcLength(startCircle, startPos, startTangent, true);

            float length2 = (startTangent - goalTangent).magnitude;

            float length3 = dubinsMath.GetArcLength(goalCircle, goalTangent, goalObj.position, 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.path2Turning = false;

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

            //Add the path to the collection of all paths
            finalPathList.Add(pathData);
        }
        //Find the coordinates of the entire path from the 2 tangents
        void GetTotalPath(OneDubinsPath pathData, bool isReversing)
        {
            //Store the waypoints of the final path here
            List <Node> finalPath = new List <Node>();

            //Start position of the car
            Vector3 currentPos = startPos;
            //Start heading of the car
            float theta = startHeading * Mathf.Deg2Rad;

            //First
            dubinsMath.AddCoordinatesToPath(
                ref currentPos,
                ref theta,
                finalPath,
                pathData.length1,
                true,
                pathData.path1TurningRight,
                isReversing);

            //Second
            dubinsMath.AddCoordinatesToPath(
                ref currentPos,
                ref theta,
                finalPath,
                pathData.length2,
                pathData.path2Turning,
                pathData.path2TurningRight,
                isReversing);

            //Third
            dubinsMath.AddCoordinatesToPath(
                ref currentPos,
                ref theta,
                finalPath,
                pathData.length3,
                true,
                pathData.path3TurningRight,
                isReversing);


            //Add the final goal coordinate of the real car and not the coordinateb we ended up with
            //Because of numerical methods they may be different
            Node newNode = new Node();

            newNode.carPos  = new Vector3(goalObj.position.x, currentPos.y, goalObj.position.z);
            newNode.heading = theta;

            if (isReversing)
            {
                newNode.isReversing = true;
            }

            finalPath.Add(newNode);

            //Save the final path in the path data
            pathData.pathCoordinates = finalPath;
        }
Пример #3
0
        //Find the shortest path from the set of curves and display it - Forward
        void FindShortestDubinPathForward()
        {
            //Get the current position and heading of the car
            //In Hybrid A* we can't use the car, but the current position and heading in search tree
            Vector3 startPos = startPosObj.position;

            float startHeading = startPosObj.eulerAngles.y;

            OneDubinsPath shortestPath = dubinsPathGenerator.GetShortestDubinPath(startPos, startHeading);

            //If we found a path
            if (shortestPath != null)
            {
                //Display the shortest path with a special line
                DisplayPath(shortestPath, lineFinalPath);
            }
        }
Пример #4
0
        //Display a path with a line renderer
        void DisplayPath(OneDubinsPath pathData, LineRenderer lineRenderer)
        {
            //Activate the line renderer
            lineRenderer.gameObject.SetActive(true);

            //The coordinates of the final path
            List <Node> finalPath = pathData.pathCoordinates;

            //Display the final line
            //lineRenderer.SetVertexCount(finalPath.Count);
            lineRenderer.positionCount = finalPath.Count;

            for (int i = 0; i < finalPath.Count; i++)
            {
                lineRenderer.SetPosition(i, finalPath[i].carPos);
            }
        }
        //LRL
        void Get_LRL_Length(Vector3 startCircle, Vector3 goalCircle, bool isLRL, List <OneDubinsPath> finalPathList)
        {
            //Find both tangent positions
            Vector3 startTangent = Vector3.zero;
            Vector3 goalTangent  = Vector3.zero;

            //Center of the 3rd circle
            Vector3 middleCircle = Vector3.zero;

            //Calculate the positions of the 3 circles
            dubinsMath.GetRLRorLRLTangents(
                startCircle,
                goalCircle,
                isLRL,
                out startTangent,
                out goalTangent,
                out middleCircle);

            //Calculate the total length of this path
            float length1 = dubinsMath.GetArcLength(startCircle, startPos, startTangent, true);

            float length2 = dubinsMath.GetArcLength(middleCircle, startTangent, goalTangent, false);

            float length3 = dubinsMath.GetArcLength(goalCircle, goalTangent, goalObj.position, 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.path2Turning = true;

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

            //Add the path to the collection of all paths
            finalPathList.Add(pathData);
        }
Пример #6
0
 //Display balls where the tangent coordinates are
 void DisplayTangentBalls(OneDubinsPath shortestPath)
 {
     debugSpheres[0].transform.position = shortestPath.tangent1;
     debugSpheres[1].transform.position = shortestPath.tangent2;
 }