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;
}
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();
}
//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],¤t_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]);
}
}
}
}
}
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;
}
}
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
}
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));
}
}
}
}
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
