Example #1
0
int expand_node(priority_queue * open_set, lifo * closed_set)
{
    search_node * successor;
    search_node * expanded_node = open_set->head->node;
    
    if (is_goal(expanded_node))
        return 1;
    
    lifo_add_entry(closed_set, expanded_node);
    pq_pop_head(open_set);
    
    for (int k = 0; k < 4; k++)
    {        
        switch (k)
        {
            case up:
                if (expand_node_up(expanded_node, &successor) < 0)
                    continue;
                break;
            case down:
                if (expand_node_down(expanded_node, &successor) < 0)
                    continue;
                break;
            case left:
                if (expand_node_left(expanded_node, &successor) < 0) 
                    continue;
                break;
            case right:
                if (expand_node_right(expanded_node, &successor) < 0)
                    continue;
                break;
        }
        
        #if DEBUG
        sprintf(str, "Checking if node with id %llu is already in history\n", successor->key);
        god_mode_debug(str);
        #endif
        
        if(node_in_closed_set(closed_set, successor) || node_in_open_set(open_set, successor))
        {
            free(successor);
            #if DEBUG
            god_mode_debug("Node is already in history\n");
            #endif
        }
        else
        {
            #if DEBUG
            god_mode_debug("Node is not in history, it will be added to open_set\n");
            #endif
            pq_add_entry(open_set, successor, f(successor));
        }
    }
    
    return 0;
}
Example #2
0
 virtual Cost search() {
   Q = Queue();
   MC.clear();
   init();
   
   while ( ! Q.empty() ) {
     Node node = Q.top();
     Q.pop();
     
     if ( is_goal(node) )
       return node.first;
     
     find_next_node(node);
   }
   
   return get_none_cost();
 }
Example #3
0
//function to find goal from a given start point
void findGoal(int start_x, int start_y, int goal_x, int goal_y){
	int lowestx;
	int lowesty;

	init_map();

	createopen();
	insert_by_priority(node_map[start_y][start_x]);

	while(true){
		get_lowest_f_cost(lowestx, lowesty);
		add_to_closed(lowestx, lowesty);
		if(is_goal(lowestx, lowesty,goal_x,goal_y)){
			displayTextLine(2, "found path");
			wait1Msec(100);
			//get_path();
			get_distance(start_x, start_y, goal_x, goal_y);
			return;
		}
		//get neighbours
		get_neighbours(lowestx, lowesty);
		for(int i=0; i<CONNECTIVITY;i++){
			if((current_neighbours[i].val==OBST)||(current_neighbours[i].onclosed==true)){}
			else if((current_neighbours[i].g>(get_g_cost(current_neighbours[i].x,current_neighbours[i].y,lowestx, lowesty)))||
				current_neighbours[i].onopen==false){
				set_cost(current_neighbours[i].x,current_neighbours[i].y,lowestx, lowesty);
				current_neighbours[i].parentx=lowestx;
				current_neighbours[i].parenty=lowesty;
				memcpy(&node_map[current_neighbours[i].y][current_neighbours[i].x],&current_neighbours[i],sizeof(node_map[current_neighbours[i].y][current_neighbours[i].x]));
				if(current_neighbours[i].onopen==false){
					insert_by_priority(current_neighbours[i]);
				}
			}
		}
	}

}
Example #4
0
void MazePlugin::syscall(State *s, int status, int syscallNo, int valueOfa0) {
   Q_UNUSED(status);

   switch(syscallNo) {
      case S_MAZE_INIT:
         s->setRegister(v0, init_maze(s));
         break;
      case S_MAZE_DEFEAT:
         defeat();
         break;
      case S_MAZE_VICTORY:
         victory();
         break;
      case S_MAZE_IS_GOAL:
         s->setRegister(v0, is_goal(s, valueOfa0));
         break;
      case S_MAZE_NEIGHBOR:
         {
            int results[4];
            get_neighbors(s, valueOfa0, results);
            unsigned int addr = s->getRegister(a1);

            for(int i = 0; i < 4; ++i)
               s->setMemoryWord(addr + (i << 2), results[i]);
         }
         break;
      case S_MAZE_DRAW_ARROW:
         draw_arrow(s, status, valueOfa0, s->getRegister(a1));
         break;
      case S_MAZE_IS_SEARCHED:
         s->setRegister(v0, is_searched(s, valueOfa0));
         break;
      default:
         break;
   }
}
Example #5
0
int main( int argc, char **argv )
{
    // VARIABLES FOR INPUT
    char str[ MAX_LINE_LENGTH +1 ] ;
    ssize_t nchars; 
    state_t state; // state_t is defined by the PSVN API. It is the type used for individual states.

    PriorityQueue<state_t> open;
    //state_map_t *map = new_state_map();//******
    // VARIABLES FOR ITERATING THROUGH state's SUCCESSORS
    state_t child;
    ruleid_iterator_t iter; // ruleid_terator_t is the type defined by the PSVN API successor/predecessor iterators.
    int ruleid ; // an iterator returns a number identifying a rule
    int64_t totalNodes;

    // READ A LINE OF INPUT FROM stdin
    printf("Please enter a state followed by ENTER: ");
    if ( fgets(str, sizeof str, stdin) == NULL ) {
        printf("Error: empty input line.\n");
        return 0; 
    }

    // CONVERT THE STRING TO A STATE
    nchars = read_state( str, &state );
    if (nchars <= 0) {
        printf("Error: invalid state entered.\n");
        return 0; 
    }

    printf("The state you entered is: ");
    print_state( stdout, &state );
    printf("\n");

    List StateList = List(); 
   // state_map_add( map, &state, 0 );//******dont know if its a must
    open.Add(0,0,state);
    StateList.add(&state);
    StateList.change_color(&state,1);// Gray
   // StateList.change_distance(&state,0);

    totalNodes = 0;
    int d = 0;
    /* Search */
    while ( !open.Empty() ) {

        // g.n
        d = open.CurrentPriority();
printf("D: %d\n",d);
        state = open.Top();
        open.Pop();

        totalNodes++;

        /* DDD */
        if (StateList.get_color(&state)==0 || (StateList.get_distance(&state)>(d-1))){
            StateList.change_distance(&state,d);
            if (is_goal(&state)==1){
                //PRINT STUFF
                printf("Estado: ");
                print_state( stdout, &state);
                printf(" Estados Generados: %"PRId64" , costo: %d",totalNodes,StateList.get_distance(&state));
                return 1;//SUCCES
            }

            /* expand node */
            init_fwd_iter(&iter, &state);
            while((ruleid = next_ruleid(&iter)) >= 0){
                apply_fwd_rule(ruleid,&state,&child);//'child' is a succesor state_t of 'state'
                StateList.add(&child);
                StateList.change_color(&child, 1);//Gray
                const int child_d = d + get_fwd_rule_cost(ruleid);
                StateList.change_distance( &child , child_d );
                open.Add( child_d , child_d , child );
            }
            StateList.change_color(&state,2); //Black
        }

    }
    printf("FAIL!");
    return 2; //FAIL
}
Example #6
0
void MSP3D::reducedGraph(){
	m_graph.clear();
	m_nodes.clear();
	m_start_index=-1;
	m_end_index=-1;
	octomap::OcTree::tree_iterator it_end=m_tree.end_tree();
	bool skip=false;
	int depth=0;
	for(octomap::OcTree::tree_iterator it=m_tree.begin_tree();it!=it_end;++it){
		if(skip){
			if(it.getDepth()<=depth){
				skip=false;
			}
		}
		if(!skip){
			if((it.getCoordinate()-m_current_coord).norm()>m_alpha*it.getSize()  || it.isLeaf()){
				if(!inPath(it.getCoordinate(),it.getSize())){
					m_nodes.push_back(std::pair<octomap::point3d,double>(it.getCoordinate(),it.getSize()));
				}
				skip=true;
				depth=it.getDepth();
			}
		}
	}
	int l=m_nodes.size();

//	std::cout<< "number of nodes: " << l << std::endl;
	for(int i=0;i<l;++i){
		// !!!!!!!!!!!!!  if not in path?
//		std::cout<< "node " << i << ":" << m_nodes[i].first <<std::endl;
		m_graph.add_vertex(i);
		if(is_start(m_nodes[i])){
//			std::cout<<"start: "<< m_nodes[i].first <<std::endl;
			if(m_start_index!=-1){
				std::cout << "2 start nodes, fail" << std::endl;
				exit(1);
			}
			m_start_index=i;
		}
		if(is_goal(m_nodes[i])){
//			std::cout<<"end: "<< m_nodes[i].first <<std::endl;
			if(m_end_index!=-1){
				std::cout << "2 end nodes, fail" << std::endl;
				exit(1);
			}
			m_end_index=i;
		}
	}
	if(m_start_index==-1){
		std::cout << "0 start node, fail" << std::endl;
	}
	if(m_end_index==-1){
		std::cout << "0 end node, fail" << std::endl;
	}
	for(int i=0;i<l;++i){
		for(int j=i+1;j<l;++j){
				if(neighboor(m_nodes[i],m_nodes[j])){
//					std::cout<< "neighboor:" << i << "," << j <<std::endl;
//					std::cout<< "cost:" << cost(i,j) <<std::endl;
					m_graph.add_edge(i,j,cost(i,j));
					m_graph.add_edge(j,i,cost(j,i));
				}
		}
	}
}
Example #7
0
int main( int argc, char **argv )
{
// VARIABLES FOR INPUT
    char str[ MAX_LINE_LENGTH +1 ] ;
    ssize_t nchars; 
    state_t state; // state_t is defined by the PSVN API. It is the type used for individual states.

// VARIABLES FOR ITERATING THROUGH state's SUCCESSORS
    state_t child;
    ruleid_iterator_t iter; // ruleid_terator_t is the type defined by the PSVN API successor/predecessor iterators.
    int ruleid ; // an iterator returns a number identifying a rule
    int childCount;

// READ A LINE OF INPUT FROM stdin
    printf("Please enter a state followed by ENTER: ");
    if ( fgets(str, sizeof str, stdin) == NULL ) {
	printf("Error: empty input line.\n");
	return 0; 
    }

// CONVERT THE STRING TO A STATE
    nchars = read_state( str, &state );
    if (nchars <= 0) {
	printf("Error: invalid state entered.\n");
	return 0; 
    }

    printf("The state you entered is: ");
    print_state( stdout, &state );
    printf("\n");

// LOOP THOUGH THE CHILDREN ONE BY ONE
    childCount = 0;
    init_fwd_iter( &iter, &state );  // initialize the child iterator 
      while( ( ruleid = next_ruleid( &iter ) ) >= 0 ) {
//    while( -1 >= 0 ) {
	apply_fwd_rule( ruleid, &state, &child );
	++childCount;
    	printf("child %d. ",childCount);
	print_state( stdout, &child );
    	//printf("  %s (cost %d)\n",get_fwd_rule_label(ruleid),get_fwd_rule_cost(ruleid));
    	printf("  %s (cost %d), goal=%d\n",get_fwd_rule_label(ruleid),get_fwd_rule_cost(ruleid),is_goal(&child));
    } // end while... no more children
    if (childCount == 0) {
	printf("Your state has no children.\n");
    }

    return 0;
} // end main