Esempio n. 1
0
 public void updatekey(DStatLiteNode thiscell)
 {
     if (thiscell.g < thiscell.rhs)
     {
         #if TIEBREAKING
             thiscell.key[0] = thiscell.g + thiscell.h + keymodifier;
             thiscell.key[1] = thiscell.g + thiscell.h + keymodifier;
             thiscell.key[2] = thiscell.g;
         #else
                 thiscell.key[0] = thiscell.g + thiscell.h + keymodifier;
             thiscell.key[1] = thiscell.g;
         #endif
     }
     else
     {
         #if TIEBREAKING
         thiscell.key[0] = thiscell.rhs + thiscell.h + keymodifier;
         thiscell.key[1] = thiscell.rhs + thiscell.h + keymodifier + 1;
         thiscell.key[2] = thiscell.h + keymodifier;
         #else
                 thiscell.key[0] = thiscell.rhs + thiscell.h + keymodifier;
                 thiscell.key[1] = thiscell.rhs;
         #endif
     }
 }
Esempio n. 2
0
        public void updatemaze(DStatLiteNode robot)
        {
            int d1, d2;
            DStatLiteNode tmpcell;

            for (d1 = 0; d1 < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; ++d1)
            {
	            if (!robot.move[d1].GetMazeCell().blocked)
	            {
	                tmpcell = robot.move[d1];
	                initializecell(tmpcell);
	                for (d2 = 0; d2 < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; ++d2)
		            if (!tmpcell.move[d2].Equals(null))
		            {
		                tmpcell.move[d2] = null;
		                tmpcell.succ[d2].move[reverse[d2]] = null;
		                initializecell(tmpcell.succ[d2]);
		                if (tmpcell.succ[d2] != start && tmpcell.succ[d2].SearchTree == tmpcell)
			            updaterhs(tmpcell.succ[d2]);
		            }
	                if (tmpcell != start)
	                {
		                tmpcell.rhs = LARGE;
		                updatecell(tmpcell);
	                }
	            }
            }
        }
Esempio n. 3
0
        private bool Mark_path(Maze maze)
        {
            if (this.start.g == 2147483647) return false;

            Object localObject = this.start;
            while (localObject != this.goal)
            {
                int i = ((DStatLiteNode)localObject).g;
                DStatLiteNode localCell = null;

                for (int j = 0; j < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; j++)
                {
                    int k = ((DStatLiteNode)localObject).GetMazeCell().X + Maze.delta_x[j];
                    int m = ((DStatLiteNode)localObject).GetMazeCell().Y + Maze.delta_y[j];

                    //if ((0 > k) || (k >= this.size_x) || (0 > m) || (m >= this.size_y) || (this.cells[k][m].type_robot_vision == 1))
                    if ((0 > k) || (k >= SimulationForm.MAZE_W) || (0 > m) || (m >= SimulationForm.MAZE_H) || (graph[k, m].type_robot_vision == 1))//(maze.cells[k, m].blocked))
                    {
                        continue;
                    }
                    if (this.graph[k, m].g < i)
                    {
                        i = this.graph[k, m].g;
                        localCell = this.graph[k, m];
                    }
                }

                ((DStatLiteNode)localObject).type_robot_vision = 2;
                ((DStatLiteNode)localObject).GetMazeCell().SetPathFlag();
                localObject = localCell;
            }
            return true;
        }
Esempio n. 4
0
        public void updatecell(DStatLiteNode thiscell)
        {
            if (thiscell.g < thiscell.rhs)
            {
                #if TIEBREAKING
	                thiscell.key[0] = thiscell.g + thiscell.h + keymodifier;
	                thiscell.key[1] = thiscell.g + thiscell.h + keymodifier;
	                thiscell.key[2] = thiscell.g;
                #else
	                thiscell.key[0] = thiscell.g + thiscell.h + keymodifier;
	                thiscell.key[1] = thiscell.g;
                #endif
	                open_list.Insert(thiscell);
            }
            else if (thiscell.g > thiscell.rhs)
            {
                #if TIEBREAKING
                    thiscell.key[0] = thiscell.rhs + thiscell.h + keymodifier;
                    thiscell.key[1] = thiscell.rhs + thiscell.h + keymodifier + 1;
                    thiscell.key[2] = thiscell.h + keymodifier;
                #else
	                thiscell.key[0] = thiscell.rhs + H(thiscell) + keymodifier;
	                thiscell.key[1] = thiscell.rhs;
                #endif
	                open_list.Insert(thiscell);
            }
            else
	            open_list.Delete(thiscell);
        }
Esempio n. 5
0
File: Key.cs Progetto: milad-d/pfa
        public Key(DStatLiteNode paramCell)
        {
            this.x = Math.Min(paramCell.g, paramCell.rhs);
            this.y = this.x;
            this.x = (this.x != 2147483647 ? this.x + paramCell.h : this.x);

            this.cell = paramCell;
        }
Esempio n. 6
0
        	/* Public: */
        public DStarLite(Maze maze, bool mark_path, bool step_by_step,
            TieBreakingStrategy tie_breaking_strategy,
            Heuristic heuristic, int neighborhood)
        {
            h = maze.GetH();
            w = maze.GetW();
            open_list = new BinaryHeap(w * h);

            graph = new DStatLiteNode[h, w];
            for (int y = 0; y < h; y++)
            {
                for (int x = 0; x < w; x++)
                {
                    graph[y, x] = new DStatLiteNode(maze.GetMazeCell(x, y), tie_breaking_strategy);
                    graph[y, x].h = heuristic.DistanceToGoal(graph[y, x].GetMazeCell(), maze.GetGoal());

                    if (maze.cells[y, x].GetCost() == 0x7F)
                    {
                        graph[y, x].type_robot_vision = 1;
                        //graph[y, x].real_type = 1;
                    }
                    else
                    {
                        graph[y, x].type_robot_vision = 0;
                        //graph[y, x].real_type = 0;
                    }

                    graph[y, x].g = (graph[y, x].rhs = 2147483647);
                    graph[y, x].iteration = mazeiteration;
                    graph[y, x].parent = null;
                }
            }

            has_solution = false;
            this.mark_path = mark_path;
            this.step_by_step = step_by_step;
            this.neighborhood = neighborhood;
            goal = graph[maze.GetGoal().Y, maze.GetGoal().X];
            start = graph[maze.GetStart().Y, maze.GetStart().X];

            start.parent = null;
            start.g = start.GetMazeCell().GetCost();
            start.f = start.g + start.h;
            open_list.Insert(start);

            //------------------------------
            this.cde = new Key.Key_comparator();
            this.u = new TreeSet(this.cde);
            this.goal.rhs = 0;
            //this.iteration += 1;
            this.u.clear();
            this.u.add(new Key(this.goal));
            this.goal.real_type = (this.goal.type_robot_vision = 0);
            this.start.real_type = (this.start.type_robot_vision = 0);

        }
Esempio n. 7
0
 public void initializecell(DStatLiteNode thiscell)
 {
     if (thiscell.generated != mazeiteration)
     {
         thiscell.g = LARGE;
         thiscell.rhs = LARGE;
         thiscell.SearchTree = new DStatLiteNode();//new List<DStatLiteNode>();
         thiscell.generated = mazeiteration;
     }
 }
Esempio n. 8
0
        private void Update_cell(DStatLiteNode paramCell, int Iteration, Maze maze)
        {
            DStatLiteNode localCell = null;

            System.Console.WriteLine("Update_cell: " + paramCell);

            Key localKey = new Key(paramCell);

            paramCell.iteration = Iteration;

            if (paramCell != this.goal)
            {
                paramCell.rhs = 2147483647;

                for (int i = 0; i < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; i++)
                {
                    int j = paramCell.GetMazeCell().X + Maze.delta_x[i];
                    int k = paramCell.GetMazeCell().Y + Maze.delta_y[i];

                    if ((0 > j) || (j >= SimulationForm.MAZE_W) || (0 > k) || (k >= SimulationForm.MAZE_H) || (maze.cells[j, k].blocked))
                    {
                        continue;
                    }
                    int m;
                    if (this.graph[j, k].g == 2147483647)
                    {
                        m = 2147483647;
                    }
                    else
                    {
                        m = this.graph[j, k].g + 1;
                    }
                    if (paramCell.rhs > m)
                    {
                        paramCell.rhs = m;
                        localCell     = this.graph[j, k];
                    }
                }
            }

            //open_list.Delete(localCell);
            this.u.remove(localKey);
            paramCell.parent = localCell;

            System.Console.WriteLine("New Parente: " + localCell);

            if (paramCell.g != paramCell.rhs)
            {
                open_list.Insert(paramCell);
                this.u.add(new Key(paramCell));
            }
        }
Esempio n. 9
0
        public DStatLiteNode(Cell maze_cell, TieBreakingStrategy tie_breaking_strategy)
        {
            closed                     = false;
            this.maze_cell             = maze_cell;
            this.tie_breaking_strategy = tie_breaking_strategy;

            //----------------------------------------------------
            this.parent            = null;
            this.maze_cell.X       = maze_cell.X;
            this.maze_cell.Y       = maze_cell.Y;
            this.h                 = 0;
            this.used              = false;
            this.type_robot_vision = (this.real_type = 0);
            this.g                 = (this.rhs = 2147483647);
            this.iteration         = 0;
        }
Esempio n. 10
0
        public void updaterhs(DStatLiteNode thiscell)
        {
            int d;

            thiscell.rhs = LARGE;
            thiscell.SearchTree = new DStatLiteNode();//new List<DStatLiteNode>();
            for (d = 0; d < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; ++d)
            {
	            //if (thiscell.move[d] && thiscell.move[d].generated == mazeiteration && thiscell.rhs > thiscell.move[d].g + 1)
                if (thiscell.move[d].generated == mazeiteration && thiscell.rhs > thiscell.move[d].g + 1)
                {
	                thiscell.rhs = thiscell.move[d].g + 1;
	                thiscell.SearchTree = thiscell.move[d];
	            }
            }
            updatecell(thiscell);
        }
Esempio n. 11
0
        public bool computeshortestpath()
        {
            DStatLiteNode tmpcell1, tmpcell2;
            goaltmpcell = new DStatLiteNode();

            int x, d;
            if (goal.g < goal.rhs)
            {
                goaltmpcell.key[0] = goal.g + keymodifier;
                goaltmpcell.key[1] = goal.g + keymodifier;
                goaltmpcell.key[2] = goal.g;
            }
            else
            {
                goaltmpcell.key[0] = goal.rhs + keymodifier;
                goaltmpcell.key[1] = goal.rhs + keymodifier + 1;
                goaltmpcell.key[2] = keymodifier;
            }
            while ((goal.rhs > goal.g || open_list.Pop().LessThanForHeap(goaltmpcell)))
            {
                tmpcell1 = (DStatLiteNode)open_list.Pop();
                oldtmpcell.key[0] = tmpcell1.key[0];
                oldtmpcell.key[1] = tmpcell1.key[1];
                oldtmpcell.key[2] = tmpcell1.key[2];
                updatekey(tmpcell1);
                if (oldtmpcell.LessThanForHeap(tmpcell1))
                    updatecell(tmpcell1);
                else if (tmpcell1.g > tmpcell1.rhs)
                {
                    tmpcell1.g = tmpcell1.rhs;
                    open_list.Delete(tmpcell1);
                    for (d = 0; d < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; ++d)
                    {
                        if (!tmpcell1.move[d].Equals(null))
                        {
                            tmpcell2 = tmpcell1.move[d];
                            initializecell(tmpcell2);
                            if (tmpcell2 != start && tmpcell2.rhs > tmpcell1.g + 1)
                            {
                                tmpcell2.rhs = tmpcell1.g + 1;
                                tmpcell2.SearchTree = tmpcell1;
                                updatecell(tmpcell2);
                            }
                        }
                    }
                }
                else
                {
                    tmpcell1.g = LARGE;
                    updatecell(tmpcell1);
                    for (d = 0; d < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; ++d)
                    {
                        if (!tmpcell1.move[d].Equals(null))
                        {
                            tmpcell2 = tmpcell1.move[d];
                            initializecell(tmpcell2);
                            if (tmpcell2 != start && tmpcell2.SearchTree == tmpcell1)
                                updaterhs(tmpcell2);
                        }
                    }
                }
                if (goal.g < goal.rhs)
                {
                    goaltmpcell.key[0] = goal.g + keymodifier;
                    goaltmpcell.key[1] = goal.g + keymodifier;
                    goaltmpcell.key[2] = goal.g;
                }
                else
                {
                    goaltmpcell.key[0] = goal.rhs + keymodifier;
                    goaltmpcell.key[1] = goal.rhs + keymodifier + 1;
                    goaltmpcell.key[2] = keymodifier;
                }
            }
            return (goal.rhs == LARGE);
        }
Esempio n. 12
0
        public void Calculate_path(bool paramBoolean, int Iteration, Maze maze)
        {
            //Clear_path();
            //with heap : DStatLiteNode node;
            while (!Execution_end())
            {
                //with heap : node = (DStatLiteNode)open_list.Pop();
                //with heap :  node.closed = true;
                Key localKey = (Key)this.u.first();
                this.u.remove(localKey);
                DStatLiteNode node = localKey.cell;

                node.iteration = Iteration;
                int i;
                int j;
                int k;
                if (node.g > node.rhs)
                {
                    node.g = node.rhs;

                    if (node != this.start)
                    {
                        for (i = 0; i < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; i++)
                        {
                            j = node.GetMazeCell().X + Maze.delta_x[i];
                            k = node.GetMazeCell().Y + Maze.delta_y[i];

                            //if ((0 > j) || (j >= this.size_x) || (0 > k) || (k >= this.size_y) || (this.cells[j][k].type_robot_vision == 1) || (node.g + 1 >= this.cells[j][k].rhs))
                            if ((0 > j) || (j >= SimulationForm.MAZE_W) || (0 > k) || (k >= SimulationForm.MAZE_H) || (maze.cells[j,k].blocked) || (node.g + 1 >= this.graph[j,k].rhs))
                            {
                                continue;
                            }
                            Update_cell(this.graph[j, k], Iteration, maze);
                        }
                    }
                }
                else
                {
                    node.g = 2147483647;

                    Update_cell(node, Iteration, maze);

                    for (i = 0; i < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; i++)
                    {
                        j = node.GetMazeCell().X + Maze.delta_x[i];
                        k = node.GetMazeCell().Y + Maze.delta_y[i];
                        
                        //if ((0 > j) || (j >= this.size_x) || (0 > k) || (k >= this.size_y) || (this.cells[j][k].type_robot_vision == 1))
                        if ((0 > j) || (j >= SimulationForm.MAZE_W) || (0 > k) || (k >= SimulationForm.MAZE_H) || (maze.cells[j, k].blocked))
                        {
                            continue;
                        }
                        Update_cell(this.graph[j,k], Iteration, maze);
                    }

                }

                if (paramBoolean)
                    break;
            }
            if (Execution_end()) 
                Mark_path(maze);
        }
Esempio n. 13
0
	    public void Solve()
        {
		    DStatLiteNode node;
		    while(!HasExecutionFinished())
            {
			    node = (DStatLiteNode)open_list.Pop();
			    node.closed = true;

			    if(node == goal)
                {
				    DStatLiteNode node_child = null;
				    path_cost = node.g;
				    has_solution = true;

				    if(mark_path)
                    {
					    node.GetMazeCell().SetPathFlag();
					    node_child = node;
					    node = node.parent;

					    do
                        {
						    node.GetMazeCell().SetNextMazeCell(node_child.GetMazeCell());
						    node.GetMazeCell().SetPathFlag();
						    node_child = node;
						    node = node.parent;
					    }while(node != null);
				    }
				    break;
			    }

			    for(int i = 0 ; i < neighborhood ; i++)
                {
				    int x , y;
				    x = node.GetMazeCell().X + Maze.delta_x[i];
				    y = node.GetMazeCell().Y + Maze.delta_y[i];
				    if(0 <= x && x < w && 0 <= y && y < h)
                    {
					    DStatLiteNode child = graph[y, x];
					    if(child.GetMazeCell().IsBlocked() || child.closed) continue;
					    int cost = child.GetMazeCell().GetCost();

					    if(open_list.Has(child))
                        {
						    if(child.g > node.g + cost)
                            {
							    child.parent = node;
							    child.g = node.g + cost;
							    child.f = child.g + child.h;
							    open_list.Insert(child);
						    }
					    }
                        else
                        {
						    child.parent = node;
						    child.g = node.g + cost;
						    child.f = child.g + child.h;
						    open_list.Insert(child);
					    }
				    }
			    }
			    if(step_by_step) break;
		    }
        }