Exemplo n.º 1
0
        private List <GridWorldSpace> RepeatedAStar()
        {
            int timeSteps = 0;

            GridWorld currWorld = new GridWorld(world.Rows, world.Rows,
                                                world.Width, world.Height);

            currWorld.SetStart(0, 0);
            currWorld.SetGoal(world.Rows - 1, world.Rows - 1);

            int  size     = (world.Rows * world.Rows) + 1;
            Node currNode = world.GetSpaceByCoord(world.GetStartCoord()).TreePointer;
            Node goalNode = world.GetSpaceByCoord(world.GetGoalCoord()).TreePointer;
            List <GridWorldSpace> repeatedPath = new List <GridWorldSpace>();
            List <GridWorldSpace> tmpPath      = new List <GridWorldSpace>();
            CoordHeap             openList     = new CoordHeap();

            while (currNode.key != goalNode.key)
            {
                repeatedPath.Add(currNode.key);
                currNode.key.G = 0;
                currNode.key.S = timeSteps;
                goalNode.key.G = size;
                goalNode.key.S = timeSteps;
                currNode.key.F = currNode.key.G + currNode.key.H;

                openList.Insert(currNode.key);
                tmpPath = GetPath(currWorld);

                currNode = openList.GetLeastF().TreePointer;
                timeSteps++;
            }

            cost = timeSteps;
            return(repeatedPath);
        }
Exemplo n.º 2
0
        private List <GridWorldSpace> GetPath(GridWorld currWorld)
        {
            int timeSteps = 0;

            CoordHeap             openList   = new CoordHeap();
            List <GridWorldSpace> closedList = new List <GridWorldSpace>();
            List <Node>           backTrack  = new List <Node>();
            GridWorldSpace        currSpace  = currWorld.GetSpaceByCoord(currWorld.GetStartCoord());
            GridWorldSpace        goalSpace  = currWorld.GetSpaceByCoord(currWorld.GetGoalCoord());

            Node starting = new Node(currSpace);

            backTrack.Add(starting);
            openList.Insert(currSpace);
            ComputeValues(currSpace, timeSteps);

            while (!openList.isEmpty() && currSpace != goalSpace)
            {
                CardinalDir[] cardinals = { CardinalDir.UP,
                                            CardinalDir.DOWN,
                                            CardinalDir.LEFT,
                                            CardinalDir.RIGHT };

                int isBlocked = 0;

                for (int i = 0; i < cardinals.Length; i++)
                {
                    if (currWorld.IsTraversible(currSpace, cardinals[i]))
                    {
                        GridWorldSpace tmpSpace = GetAdjacentSpace(currSpace, cardinals[i]);

                        bool tmpbool = tmpSpace.isTraversible();

                        leastSpace = backTrack.First();
                        for (int it = 0; it < backTrack.Count; it++)
                        {
                            if (leastSpace.key.F > backTrack[it].key.F)
                            {
                                leastSpace.key = backTrack[it].key;
                            }
                        }

                        Node parent1 = leastSpace;
                        ComputeValues(tmpSpace, timeSteps);
                        if (tmpbool && !openList.Contains(tmpSpace) && !closedList.Contains(tmpSpace))
                        {
                            ComputeValues(tmpSpace, timeSteps);
                            openList.Insert(tmpSpace);

                            //Initialized a Node Object for tmpSpace
                            child = new Node(tmpSpace);

                            TT.Insert(parent1, child);
                            backTrack.Add(child);

                            if (child.key == goalSpace)
                            {
                                break;
                            }
                        }

                        if (tmpbool && openList.Contains(tmpSpace) && (tmpSpace.F < openList.SpaceF(tmpSpace.Id)))
                        {
                            child = new Node(tmpSpace);

                            openList.RemoveSpecific(tmpSpace.Id); //Remove anything with this ID
                            openList.Insert(tmpSpace);

                            for (int x = 0; x < backTrack.Count; x++)
                            {
                                if (tmpSpace.Id == backTrack[x].key.Id)
                                {
                                    backTrack.Remove(child); //Remove anything with this ID
                                }
                            }

                            backTrack.Add(child);
                            TT.Insert(parent1, child);

                            if (child.key == goalSpace)
                            {
                                break;
                            }
                        }

                        else
                        {
                            isBlocked++;
                        }
                    }
                    else
                    {
                        isBlocked++;
                    }

                    if (isBlocked == 4)
                    {
                        currSpace.Blocked = true;
                    }
                }

                openList.Remove(currSpace);
                closedList.Add(currSpace);
                if (!openList.isEmpty())
                {
                    currSpace = openList.GetLeastF();
                }
                backTrack.Remove(leastSpace);

                timeSteppedPaths[timeSteps] = new List <GridWorldSpace>();
                foreach (GridWorldSpace tmp in closedList)
                {
                    timeSteppedPaths[timeSteps].Add(tmp);
                }

                timeStepStrings[timeSteps] = "";
                Debug.WriteLine("");
                Debug.WriteLine("Timestep: " + timeSteps);
                Debug.WriteLine("Current Node: " + currSpace.Id);
                Debug.WriteLine("Closed List:");
                foreach (GridWorldSpace tmp in closedList)
                {
                    Debug.Write(tmp.Id);
                    timeStepStrings[timeSteps] += tmp.Id;
                    timeStepStrings[timeSteps] += "->";
                    Debug.Write("->");
                }
                Debug.WriteLine("");
                openList.PrintHeap();

                timeSteps++;
            }

            if (currSpace.isGoal())
            {
                timeStepStrings[timeSteps] += timeStepStrings[timeSteps - 1];
                timeStepStrings[timeSteps] += currSpace.Id;

                timeSteppedPaths[timeSteps] = new List <GridWorldSpace>();
                foreach (GridWorldSpace tmp in closedList)
                {
                    timeSteppedPaths[timeSteps].Add(tmp);
                }

                solvable = true;
            }
            else
            {
                timeSteps = timeSteps - 1;
                solvable  = false;
            }

            List <GridWorldSpace> finalPath = new List <GridWorldSpace>();

            //track parent pointers all the way up to Start
            Node curr = child; //Go to Goal, where child = goal

            while (curr != null)
            {
                finalPath.Add(curr.key);
                curr = curr.parent;
            }

            finalPath.Reverse();
            Debug.WriteLine("Final list: ");
            foreach (GridWorldSpace tmp in finalPath)
            {
                Debug.Write(tmp.Id);
                path_string += tmp.Id;
                if (tmp.isGoal())
                {
                    Debug.Write("(GOAL)");
                    path_string += "(GOAL)";
                }
                else
                {
                    Debug.Write("->");
                    path_string += "->";
                }
            }

            cost = timeSteps;
            return(finalPath);
        }