/// <summary>
        /// Method used to smooth a received path, using a string pulling technique
        /// it returns a new path, where the path positions are selected in order to provide a smoother path
        /// </summary>
        /// <param name="data"></param>
        /// <param name="globalPath"></param>
        /// <returns></returns>
        public static GlobalPath SmoothPath(KinematicData data, GlobalPath globalPath)
        {
            NavMeshEdge edge;
            var lookAhead = 3;
            Vector3 lookAheadTarget;

            var smoothedPath = new GlobalPath
            {
                IsPartial = globalPath.IsPartial
            };

            //we will string pull from the begginning to the end
            var endPosition = globalPath.PathPositions.Last();

            var currentPosition = data.position;

            for (int i = 0; i < globalPath.PathNodes.Count; i++)
            {
                edge = globalPath.PathNodes[i] as NavMeshEdge;
                if (edge != null)
                {

                    if ((i + lookAhead) < globalPath.PathNodes.Count)
                    {
                        lookAheadTarget = globalPath.PathNodes[i + lookAhead].LocalPosition;
                    }
                    else
                    {
                        lookAheadTarget = endPosition;
                    }

                    if (!lookAheadTarget.Equals(currentPosition))
                    {
                        var closestPointInEdge = MathHelper.ClosestPointInLineSegment2ToLineSegment1(currentPosition, lookAheadTarget, edge.PointOne, edge.PointTwo, lookAheadTarget);
                        smoothedPath.PathPositions.Add(closestPointInEdge);
                        currentPosition = closestPointInEdge;
                    }
                }
            }

            smoothedPath.PathPositions.Add(endPosition);

            return smoothedPath;
        }
Exemple #2
0
 public override Path GetPath(GlobalPath path)
 {
     return path;
 }
Exemple #3
0
        protected GlobalPath CalculateSolution(NodeRecord node, bool partial)
        {
            var path = new GlobalPath
            {
                IsPartial = partial
            };
            var currentNode = node;

            path.PathPositions.Add(this.GoalPosition);

            //I need to remove the first Node and the last Node because they correspond to the dummy first and last Polygons that were created by the initialization.
            //And for instance I don't want to be forced to go to the center of the initial polygon before starting to move towards my destination.

            //skip the last node, but only if the solution is not partial (if the solution is partial, the last node does not correspond to the dummy goal polygon)
            if (!partial && currentNode.parent != null)
            {
                currentNode = currentNode.parent;
            }

            while (currentNode.parent != null)
            {
                path.PathNodes.Add(currentNode.node); //we need to reverse the list because this operator add elements to the end of the list
                path.PathPositions.Add(currentNode.node.LocalPosition);

                if (currentNode.parent.parent == null) break; //this skips the first node
                currentNode = currentNode.parent;
            }

            path.PathNodes.Reverse();
            path.PathPositions.Reverse();
            return path;
        }
Exemple #4
0
        public virtual bool Search(out GlobalPath solution, bool returnPartialSolution = false)
        {
            var startTime = Time.realtimeSinceStartup;
            var processedNodes = 0;
            int count;

            while (processedNodes < this.NodesPerSearch)
            {
                count = this.Open.CountOpen();
                if (count == 0)
                {

                    solution = null;
                    this.InProgress = false;
                    this.CleanUp();
                    this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;
                    return true;
                }

                if (count > this.MaxOpenNodes)
                {
                    this.MaxOpenNodes = count;
                }

                var bestNode = this.Open.GetBestAndRemove();

                //goal node found, return the shortest Path
                if (bestNode.node == this.GoalNode)
                {
                    solution = this.CalculateSolution(bestNode, false);
                    this.InProgress = false;
                    this.CleanUp();
                    this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;
                    return true;
                }

                this.Closed.AddToClosed(bestNode);
                processedNodes++;
                this.TotalProcessedNodes++;

                var outConnections = bestNode.node.OutEdgeCount;
                for (int i = 0; i < outConnections; i++)
                {
                    var childNode = GenerateChildNodeRecord(bestNode, bestNode.node.EdgeOut(i));
                    var closedSearch = this.Closed.SearchInClosed(childNode);
                    if (closedSearch != null) continue;

                    var openSearch = this.Open.SearchInOpen(childNode);
                    if (openSearch != null)
                    {
                        if (childNode.fValue <= openSearch.fValue)
                        {
                            this.Open.Replace(openSearch, childNode);
                        }
                    }
                    else
                    {
                        this.Open.AddToOpen(childNode);
                    }
                }
            }

            this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;

            //this is very unlikely but it might happen that we process all nodes alowed in this cycle but there are no more nodes to process
            if (this.Open.CountOpen() == 0)
            {
                solution = null;
                this.InProgress = false;
                this.CleanUp();
                return true;
            }

            //if the caller wants create a partial Path to reach the current best node so far
            if (returnPartialSolution)
            {
                var bestNodeSoFar = this.Open.PeekBest();
                solution = this.CalculateSolution(bestNodeSoFar, true);
            }
            else
            {
                solution = null;
            }
            return false;
        }
Exemple #5
0
 public override Path GetPath(GlobalPath path)
 {
     return path.LocalPaths[0];
 }
Exemple #6
0
 public abstract Path GetPath(GlobalPath path);
        public override bool Search(out GlobalPath solution, bool returnPartialSolution = false)
        {
            var startTime = Time.realtimeSinceStartup;
            var processedNodes = 0;
            int count;

            while (processedNodes < this.NodesPerSearch)
            {
                count = this.Open.CountOpen();
                if (count == 0)
                {

                    solution = null;
                    this.InProgress = false;
                    this.CleanUp();
                    this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;
                    return true;
                }

                if (count > this.MaxOpenNodes)
                {
                    this.MaxOpenNodes = count;
                }

                var bestNode = this.NodeRecordArray.GetBestAndRemove();

                //goal node found, return the shortest Path
                if (bestNode.node == this.GoalNode)
                {
                    solution = this.CalculateSolution(bestNode, false);
                    this.InProgress = false;
                    this.CleanUp();
                    this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;
                    return true;
                }

                this.NodeRecordArray.AddToClosed(bestNode);

                processedNodes++;
                this.TotalProcessedNodes++;

                //put your code here

                //or if you would like, you can change just these lines of code this in the original A* Pathfinding Class,
                //create a ProcessChildNode method in the base class with the code from the previous A* algorithm.
                //if you do this, then you don't need to implement this search method method. Don't forget to override the ProcessChildMethod if you do this
                var outConnections = bestNode.node.OutEdgeCount;
                for (int i = 0; i < outConnections; i++)
                {
                    this.ProcessChildNode(bestNode,bestNode.node.EdgeOut(i));
                }
            }

            this.TotalProcessingTime += Time.realtimeSinceStartup - startTime;

            //this is very unlikely but it might happen that we process all nodes alowed in this cycle but there are no more nodes to process
            if (this.Open.CountOpen() == 0)
            {
                solution = null;
                this.InProgress = false;
                this.CleanUp();
                return true;
            }

            //if the caller wants create a partial Path to reach the current best node so far
            if (returnPartialSolution)
            {
                var bestNodeSoFar = this.Open.PeekBest();
                solution = this.CalculateSolution(bestNodeSoFar, true);
            }
            else
            {
                solution = null;
            }
            return false;
        }