예제 #1
0
        /// <summary>
        /// Using Greedy BFS algorithm finds the path from start node to the end node.
        /// The "greedy" part uses Euclidian distance as the priority to explore
        /// </summary>
        /// <param name="start">The first node to start the search</param>
        /// <param name="end">The goal node that want to be found from start</param>
        /// <returns>Returns the path as an array of INodes. If there is not an available path, the array will be empty</returns>
        public INode[] CalculatePath(INode start, INode end)
        {
            PathfinderNode startNode = PathfinderNode.GetPathfinderNodeOrCreateNew(nodes, start);

            frontier.Insert(startNode);
            exploration.Add(startNode, startNode);

            while (frontier.Count > 0)
            {
                PathfinderNode exploringNode = frontier.Extract();

                if (exploringNode.Node.Equals(end))
                {
                    break;
                }

                for (int i = 0; i < exploringNode.Node.Neighbors.Length; i++)
                {
                    INode neighbor = exploringNode.Node.Neighbors[i];

                    if (neighbor.IsVisitable)
                    {
                        PathfinderNode neighborNode = PathfinderNode.GetPathfinderNodeOrCreateNew(nodes, neighbor);

                        if (!exploration.ContainsKey(neighborNode)) //If is the first time to be explored
                        {
                            neighborNode.HeuristicValue = Heuristic.SquaredEuclidianDistance(neighbor, end);
                            frontier.Insert(neighborNode);
                            exploration.Add(neighborNode, exploringNode); //Add: neighborNode explored from exploringNode
                        }
                    }
                }
            }


            //Construct path
            if (nodes.ContainsKey(end)) //If the end node was added by one of its neighbors
            {
                //Track the path from end to start
                PathfinderNode nextNode = nodes[end]; //Starts from end node

                while (nextNode != startNode)
                {
                    path.Add(nextNode.Node);
                    if (exploration.ContainsKey(nextNode)) //If the node was visited from other node
                    {
                        nextNode = exploration[nextNode];  //that "came from" node is the next in the path
                    }
                    else  //if the node doesn't exist
                    {
                        break; //stop
                    }
                }
                path.Reverse();
            }

            return(path.ToArray());
        }
예제 #2
0
        /// <summary>
        /// Using Dijkstra algorithm to find the path from start node to the end node.
        /// It uses the weight of the nodes to find the best path
        /// </summary>
        /// <param name="start">The first node to start the search</param>
        /// <param name="end">The goal node that want to be found from start</param>
        /// <returns>Returns the path as an array of INodes. If there is not an available path, the array will be empty</returns>
        public INode[] CalculatePath(INode start, INode end)
        {
            PathfinderNode startNode = PathfinderNode.GetPathfinderNodeOrCreateNew(nodes, start);

            frontier.Insert(startNode);
            exploration.Add(startNode, startNode);

            while (frontier.Count > 0)
            {
                PathfinderNode exploringNode = frontier.Extract();

                if (exploringNode.Node.Equals(end))
                {
                    break;
                }

                for (int i = 0; i < exploringNode.Node.Neighbors.Length; i++)
                {
                    INode neighbor = exploringNode.Node.Neighbors[i];

                    if (neighbor.IsVisitable)
                    {
                        PathfinderNode neighborNode = PathfinderNode.GetPathfinderNodeOrCreateNew(nodes, neighbor);

                        float cost = exploringNode.Cost + neighbor.Weight;

                        if (!exploration.ContainsKey(neighborNode)) //If is the first time to be explored
                        {
                            neighborNode.Cost = cost;
                            frontier.Insert(neighborNode);
                            exploration.Add(neighborNode, exploringNode); //Add: neighborNode explored from exploringNode
                        }
                        else //Already explored
                        { //Compares if this path cost less
                            if (cost < exploration[neighborNode].Cost)
                            {
                                //Change the previous connection, so that neighbor now came from exploringNode
                                neighborNode.Cost         = cost;
                                exploration[neighborNode] = exploringNode;
                            }
                        }
                    }
                }
            }


            //Construct path
            if (nodes.ContainsKey(end)) //If the end node was added by one of its neighbors
            {
                //Track the path from end to start
                PathfinderNode nextNode = nodes[end]; //Starts from end node

                while (nextNode != startNode)
                {
                    path.Add(nextNode.Node);
                    if (exploration.ContainsKey(nextNode)) //If the node was visited from other node
                    {
                        nextNode = exploration[nextNode];  //that "came from" node is the next in the path
                    }
                    else  //if the node doesn't exist
                    {
                        break; //stop
                    }
                }
                path.Reverse();
            }

            return(path.ToArray());
        }