Пример #1
0
void Astar::begin(){
  //let's create a new state from the start node using the start node
  //double check the naming it's confusing
  start.create_state(start);
  //push to the front of the openlist list, this should be the openlist
  openlist.push_front(start);
  //let's create a node called current
  node current_node;
  //let's check there is nothing on the open list
  while( !openlist.empty() ){
    //let's print the open list
    print_list(openlist, "Open");
    //store the front of the open list on the current_node
    current_node = openlist.front();
    //delete it from the open list queue
    openlist.pop_front();
    //add current_node to the closed list
    closedlist.push_front(current_node);
    //check if the node just popped actually the goal
    //if it is we just print it here but something else needs to be done.
    if ( isGoal(current_node,goal) ){
      cout << "We have reached the goal" << endl;
      cout << current_node << endl;
      break;
    } else {
      //else let's expand the currnent node
      cout << "Not yet, let's expand the current_node node." << endl;
      cout << current_node << endl;
      expand(current_node);
      openlist.sort();
    }
  }
}
Пример #2
0
/**
 * Test whether the "now" state is valid
 */
int validState(int dx, int dy, State &now, const vector<string> &ground)
{
	// boundary check
	if (now.person.x >= ground.size() || now.person.x < 0)
		return 0;
	if (now.person.y >= ground[now.person.x].size() || now.person.y < 0)
		return 0;
	// is wall?
	if (isWall(ground[now.person.x][now.person.y]))
		return 0;
	// check boxes overlap with person
	int personOverlapBox = 1;
	for (int i = 0; i < now.box.size(); ++i) {
		if (now.box[i] == now.person) {
			personOverlapBox = 2;
			now.box[i].x += dx;
			now.box[i].y += dy;
			if (!isValidBox(now.box[i], now, ground))
				return 0;
		}
	}
	// terminal state?
	for (int i = 0; i < now.box.size(); ++i) {
		if (!isGoal(ground[now.box[i].x][now.box[i].y])) {
			return personOverlapBox;
		}
	}
	return -1;
}
Пример #3
0
void calc4ee_(struct exp ea, enum opr opr, struct exp eb)
{
    float r,a,b;

    a = ea.r;
    b = eb.r;
    switch (opr)
    {
    case op_add:
        r = a + b;
        break;
    case op_sub:
        r = a - b;
        break;
    case op_mul:
        r = a * b;
        break;
    case op_div:
        if (b != 0.0)
            r = (float)a / (float)b;
        else
            r = 0;
        break;
    default:
        r = 0;
        break;
    }

    if (isGoal(r, 24))
    {
        printf("(%s)%c(%s)\t",ea.str,oprStr[opr],eb.str);
    }

}
Пример #4
0
 void print()
 {
     std::cout << toString();
     std::cout << "Dimension "<< dimension()<< '\n';
     std::cout << "is Goal "<< isGoal() << '\n';
     std::cout << "hamming "<< hamming() << '\n';
     std::cout << "manhattan " << manhattan() << '\n';
 }
Пример #5
0
	//更新処理
	void update(const float elapsed_time)
	{
		if (!isGoal())
		{
			const float now_time = m_time * m_t + elapsed_time;//時間を進める
			m_t = std::min(now_time / m_time, 1.f);//進行度を算出
			m_now = m_begin * (1.f - m_t) + m_end * m_t;//現在の値を更新(線形補間)
		}
	}
Пример #6
0
/**
 * get the goal positions
 */
void getGoalPosition(const vector<string> &ground, vector<Position> &goal)
{
	for (int i = 0; i < ground.size(); ++i) {
		for (int j = 0; j < ground[i].size(); ++j) {
			if (isGoal(ground[i][j])) {
				goal.push_back(Position(i, j));
			}
		}
	}
}
Пример #7
0
main() {
    // 0 if no mine, 1 if mine
    int gameOver = 0, winGame = 0;
    int i, j;
    char x[1],y[1];
    int row,col;
   
    for(i = 0; i < SIZE ; i ++){
        for(j = 0; j < SIZE ; j ++){
            boardState[i][j] = 0;
            boardActivated[i][j] = 0;
        }    
    }
    
    initializeBoard();

   /**********************************/
    
    
    while(1){
        printf("Choose index of the tile [row]<space>[col]:");
        fgets(x,2,stdin);
        row = x[0] - '0';
        getchar();
        fgets(y,2,stdin); 
        col = y[0] - '0';
        getchar();
        
        boardActivated[row][col] = 1;
        printBoard();
            
        if(boardState[row][col] == 100){ 
            gameOver = 1;
            break;
        }
        
       // else checkNeighbors(x,y);
        
        if (isGoal(boardState, boardActivated)== 100){ 
            winGame = 1;       
            break;
        }
        
             
    }
    
    if(winGame){
        printf("You win the game!\n");
    }
    
    else if(gameOver){
        printf("BOOOM! \n");
    }
    
}
Пример #8
0
int searchPQS (PriQ q, State s, int size) {
    //returns index in PQ, uses linear search.
    int success = FALSE;
    int i = 0;
    State temp = q->heap[1]->s;
    while (i < q->arraySize && temp != NULL &&
            !isGoal(temp,s,size)) {
        i++;
        if (q->heap[i] != NULL) {
            temp = q->heap[i]->s;
        
            if (isGoal(temp,s,size)) {
                success = TRUE;
            }
        }
    }
    if (success == FALSE) {
        i = NOTHING;
    }
    return i;
}
Пример #9
0
/**
 * judge if the current state is freeze deadlock
 */
int isFreezeDeadlock(State &now, const vector<string> &ground)
{
	vector<int>hfreeze(now.box.size(), -2);
	vector<int>vfreeze(now.box.size(), -2);
	for (int i = 0; i < now.box.size(); ++i) {
		horizontalFreeze(i, hfreeze, vfreeze, now, ground);
		verticalFreeze(i, hfreeze, vfreeze, now, ground);
	}
	for (int i = 0; i < hfreeze.size(); ++i) {
		if (hfreeze[i] == 0 && vfreeze[i] == 0 && !isGoal(ground[now.box[i].x][now.box[i].y])) {
			return 1;
		}
	}
	return 0;
}
Пример #10
0
void calc4enn_(struct exp ea, enum opr opr, int b)
{
    float r,a;

    a = ea.r;
    switch (opr)
    {
    case op_add:
        r = a + b;
        break;
    case op_sub:
        r = a - b;
        break;
    case op_mul:
        r = a * b;
        break;
    case op_div:
        r = (float)a / (float)b;
        break;
    case op_rsub:
        r = b - a;
        break;
    case op_rdiv:
        if (a != 0.0)
            r = b / a;
        break;
    default:
        break;
    }

    if (isGoal(r, 24))
    {
        if (opr == op_rdiv || opr == op_rsub)
            printf("%d%c(%s)\t", b, oprStr[opr], ea.str);
        else
            printf("(%s)%c%d\t", ea.str, oprStr[opr], b);
    }
}
void DataWrapper::publish(const VisionFieldObject* visual_object)
{
    if( isGoal(visual_object->getID()) )
    {
        digitalWrite(18, HIGH);
    }
    else if( visual_object->getID() == BALL )
    {
        digitalWrite(17, HIGH);
    }
    else if( visual_object->getID() == FIELDLINE )
    {
        digitalWrite(23, HIGH);
    }
    else if( visual_object->getID() == OBSTACLE )
    {
        digitalWrite(22, HIGH);
    }

    #if VISION_WRAPPER_VERBOSITY > 0
    visual_object->printLabel(debug);
    debug << std::endl;
    #endif
}
/*
 * Function: aStart
 *
 * @Parameters: Grid_Container grid 	~ grid_container to find neighboors
 *		Point start		~ starting point
 *		Point goal		~ goal point
 *
 * Uses an pseudocode I found of wikipedia as guidance on my implementation.
 *
 * @returns an array of points which contains the path to the goal from the start point
 * 
 *
 */
ArrayList* aStar( Grid_Container *grid, Point start, Point goal) 
{
	int i = 0;

	ArrayList *closed_set 	= NULL;
	PQ *open_set 		= NULL;
	ArrayList *map 		= NULL;
	
	start.g_score = 0;
	start.f_score = start.g_score + heuristic_estimate( start, goal);

	open_set 	= makePQ();
	closed_set 	= makeArrayList();
	map		= makeArrayList();
	
	insert(	open_set, start);
	
	while ( !isEmpty( open_set))
	{
		
		Point current = pop(open_set);
		//printf("Current x: %d y: %d\n", current.x, current.y);
		if ( isGoal( current))
		{	
			
			ArrayList* rtrnList = NULL;
			free_PQ( open_set);
			free_ArrayList(closed_set);
			
			rtrnList = reconstruct_Path(map, current);
			
			free_ArrayList( map);
			return  rtrnList;
		}
		
		add(closed_set, current);
		ArrayList *neighbors 	= NULL; 
		neighbors = getNeighborPoints(current, grid); 
		//printf("Printing neighbors:\n");
		//printArrayList(neighbors);
		for ( i = 0; i < getSize( neighbors); i++)
		{
			Point neighbor = get( neighbors, i);
			
			if ( !contains( closed_set, neighbor)) 
			{

				int tentative_g_score = calculate_g_score(current, neighbor);

				if (!containsPoint( open_set, neighbor) || tentative_g_score < neighbor.g_score)
				{
					neighbor.parent_x = current.x;
					neighbor.parent_y = current.y;
					add( map, current); 
					neighbor.g_score = tentative_g_score;
					
					if (!isGoal(neighbor)) 
					{
						neighbor.f_score = neighbor.g_score + heuristic_estimate( current, neighbor);
					} 
					else 
					{
						neighbor.f_score = 0;
					}
					
					if (!containsPoint( open_set, neighbor))
					{
						insert( open_set, neighbor);
					}
				}
				
			}
			
		}
		free_ArrayList(neighbors);
	}
	return NULL; 
}
Пример #13
0
bool WorldObject::isGoalPost(){
  return (isGoal() && !isGoalCenter());
}
Пример #14
0
	//現在の状態を表示
	void debugPrint(const int flame)
	{
		//printf("%2d: now=%.3f, begin=%.3f, end=%.3f, diff=%.3f, time=%.3f, gradient=%.3f, t=%.3f, remain_t=%.3f, progress_time=%.3f, remain_time=%.3f, isGoal()=%s\n", flame, now(), begin(), end(), difference(), time(), gradient(), t(), remain_t(), progress_time(), remain_time(), isGoal() ? "true" : "false");
		printf("%2d: now=%.3f, begin=%.3f, end=%.3f, time=%.3f, t=%.3f, isGoal()=%s\n", flame, now(), begin(), end(), time(), t(), isGoal() ? "true" : "false");
	}
Пример #15
0
/**
 * @function getShortestPath
 */
bool BFS3D::getShortestPath(std::vector<short unsigned int> start, std::vector<std::vector<int> > &path)
{
    int val = 0, cntr = 0, min_val = INFINITE_COST;
    std::vector<int> state(3,0);
    std::vector<int> next_state(3,0);
    int newx,newy,newz;

    //make sure the while loop eventually stops
    int max_path_length = dimX_*dimY_;

    int dx[DIRECTIONS3D] = { 1,  1,  1,  0,  0,  0, -1, -1, -1,    1,  1,  1,  0,  0, -1, -1, -1,    1,  1,  1,  0,  0,  0, -1, -1, -1};
    int dy[DIRECTIONS3D] = { 1,  0, -1,  1,  0, -1, -1,  0,  1,    1,  0, -1,  1, -1, -1,  0,  1,    1,  0, -1,  1,  0, -1, -1,  0,  1};
    int dz[DIRECTIONS3D] = {-1, -1, -1, -1, -1, -1, -1, -1, -1,    0,  0,  0,  0,  0,  0,  0,  0,    1,  1,  1,  1,  1,  1,  1,  1,  1};

    path.resize(0);
    next_state[0] = start[0];
    next_state[1] = start[1];
    next_state[2] = start[2];

    while( !isGoal(next_state) || cntr > max_path_length )
    {
        state = next_state;
        min_val = INFINITE_COST;

        //iterate through neighbors
        for( int d = 0; d < DIRECTIONS3D; d++ )
        {
            newx = state[0] + dx[d];
            newy = state[1] + dy[d];
            newz = state[2] + dz[d];

            //make sure it is inside the map and has no obstacle
            if( 0 > newx || newx >= dimX_ || 0 > newy || newy >= dimY_ || 0 > newz || newz >= dimZ_ )
                continue;

            val = dist_[xyzToIndex(newx,newy,newz)];

            if( val >= INFINITE_COST )
                continue;

            if ( state[0] != newx && state[1] != newy && state[2] != newz )
                val += cost_sqrt3_move_;
            else if ( (state[1] != newy && state[2] != newz) ||
                      (state[0] != newx && state[2] != newz) ||
                      (state[0] != newx && state[1] != newy) )
                val += cost_sqrt2_move_;
            else
                val += cost_1_move_;

            if( val < min_val )
            {
                min_val = val;
                next_state[0] = newx;
                next_state[1] = newy;
                next_state[2] = newz;
            }
        }
        path.push_back(next_state);

        cntr++;
    }

    //unable to find path
    if(cntr > max_path_length)
    {
        printf("--(!) [BFS3D] Unable to find path to goal. Exiting.\n");
        path.clear();
        return false;
    }

    return true;
}
Пример #16
0
void findWaypoints() {
	int position[3]; // Current position
	int i;
	double path_length = 0; // Total length of the path
	int path_steps = 0; // Total length of the path in steps

	logStart("findWaypoints");

	position[0] = getPosition()[0];
	position[1] = getPosition()[1];
	position[2] = getPosition()[2];

	for (i = 0; i < 1024; i++) {
		int a, b, c;

		double best_value = world[position[0]][position[1]][position[2]]; // So far best weigt == current weight
		int next[3] = { -1, -1, -1 }; // Next position

		for (a = -1; a <= 1; a++) {
			if (position[0] + a >= 0 && position[0] + a < WORLD_SIZE_X) {
				for (b = -1; b <= 1; b++) {
					if (position[1] + b >= 0 && position[1] + b < WORLD_SIZE_Y) {
						for (c = -1; c <= 1; c++) {
							if (position[2] + c >= 0 && position[2] + c < WORLD_SIZE_Z) {
								if (!isObstacle(position[0] + a, position[1] + b, position[2] + c)) {
									double new_value = world[position[0] + a][position[1] + b][position[2] + c];
									/* if (LL_DEBUG || 1)
									 printf(
									 "Surrounding point [%i, %i, %i] has value %2.12f (%e) - best know has value %2.12f (%e)\n",
									 position[0] + a, position[1]
									 + b, position[2] + c,
									 new_value, best_value); */
									if (new_value > best_value) {
										best_value = new_value;
										next[0] = position[0] + a;
										next[1] = position[1] + b;
										next[2] = position[2] + c;
									}
								}
							}
						}
					}
				}
			}
		}

		printf("Next waypoint: [%i, %i, %i] with value %2.12f (%e)\n", next[0], next[1], next[2], best_value,
				best_value);

		// Calculate path length
		int del_0 = position[0] - next[0];
		int del_1 = position[1] - next[1];
		int del_2 = position[2] - next[2];

		del_0 *= del_0;
		del_1 *= del_1;
		del_2 *= del_2;

		if (del_0 + del_1 + del_2 == 1)
			path_length += 1;
		if (del_0 + del_1 + del_2 == 2)
			path_length += 1.414; // SQRT(2)
		if (del_0 + del_1 + del_2 == 3)
			path_length += 1.732; // SQRT(3)

		path_steps++;

		if (isGoal(next[0], next[1], next[2]))
			break;
		else {
			// Set found waypoint as next position
			position[0] = next[0];
			position[1] = next[1];
			position[2] = next[2];
		}

	}

	printf("Total path length: %f\n", path_length);
	printf("Total path length in steps: %i\n", path_steps);

	logEnd("findWaypoints");
}
Пример #17
0
void demo_7_main_core_0() {

	int threads = TOTAL_PROC_NUM;

	// Initialize obstacle map from heightmap
	if (1) {
		logStart("initialize obstacle map");

		// Recode heightmap to global_obstacles
		int a, b, c;

		for (a = 0; a < WORLD_SIZE_X; a++) {
			for (b = 0; b < WORLD_SIZE_Y; b++) {
				int height = getHeightmapValue(a, b);

				// some regions cannot be flown over
				if (height > 174)
					height = 255;

				for (c = 0; c < height; c++) {
					global_obstacles[a][b][c] = 1;
				}
				for (c = height; c < WORLD_SIZE_Z; c++) {
					global_obstacles[a][b][c] = 0;
				}
			}
		}

		logEnd("initialize obstacle map");
	}

	// Set goal
	setGoal(WORLD_SIZE_X - 3, WORLD_SIZE_Y - 3, getHeightmapValue(WORLD_SIZE_X - 3, WORLD_SIZE_Y - 3) + 2);

	// Set current position
	setPosition(3, 3, getHeightmapValue(3, 3) + 2);

	// Init weightmap from obstacle map
	if (1) {
		int a, b, c;

		logStart("initialize weightmap from obstacle map");

		for (a = 0; a < WORLD_SIZE_X; a++) {
			for (b = 0; b < WORLD_SIZE_Y; b++) {
				for (c = 0; c < WORLD_SIZE_Z; c++) {
					if (isGoal(a, b, c)) {
						world[a][b][c] = WEIGHT_SINK;
					} else if (isObstacle(a, b, c)) {
						world[a][b][c] = WEIGHT_OBSTACLE;
					} else {
						world[a][b][c] = WEIGHT_INIT;
					}
				}
			}
		}

		logEnd("initialize weightmap from obstacle map");
	}

	// Propagate weights with selected algorithm
	promote_world(ITERATIONS, threads);

	// Find waypoints to goal
	findWaypoints();

	printf("Iterations: %i\n", ITERATIONS);

	return;
}
Пример #18
0
bool AstarSearch::search()
{
  // -- Start Astar search ----------
  // If the openlist is empty, search failed
  while (!openlist_.empty()) {

    // Terminate the search if the count reaches a certain value
    static int search_count = 0;
    search_count++;
    if (search_count > 300000) {
      ROS_WARN("Exceed time limit");
      search_count = 0;
      return false;
    }

    // Pop minimum cost node from openlist
    SimpleNode sn;
    sn = openlist_.top();
    openlist_.pop();
    nodes_[sn.index_y][sn.index_x][sn.index_theta].status = STATUS::CLOSED;

    // Expand nodes from this node
    AstarNode *current_node = &nodes_[sn.index_y][sn.index_x][sn.index_theta];

    // for each update
    for (const auto &state : state_update_table_[sn.index_theta]) {
      // Next state
      double next_x     = current_node->x + state.shift_x;
      double next_y     = current_node->y + state.shift_y;
      double next_theta = astar::modifyTheta(current_node->theta + state.rotation);
      double move_cost  = state.step;

#if DEBUG
      // Display search process
      geometry_msgs::Pose p;
      p.position.x = next_x;
      p.position.y = next_y;
      tf::quaternionTFToMsg(tf::createQuaternionFromYaw(next_theta), p.orientation);
      p = astar::transformPose(p, map2ogm_);
      debug_pose_array_.poses.push_back(p);
#endif

      // Increase curve cost
      if (state.curve)
        move_cost *= curve_weight_;

      // Increase reverse cost
      if (current_node->back != state.back)
        move_cost *= reverse_weight_;

      // Calculate index of the next state
      SimpleNode next;
      next.index_x     = next_x / map_info_.resolution;
      next.index_y     = next_y / map_info_.resolution;
      next.index_theta = sn.index_theta + state.index_theta;
      // Avoid invalid index
      next.index_theta = (next.index_theta + angle_size_) % angle_size_;

      // Check if the index is valid
      if (isOutOfRange(next.index_x, next.index_y) || detectCollision(next))
        continue;

      AstarNode *next_node = &nodes_[next.index_y][next.index_x][next.index_theta];
      double next_hc       =  nodes_[next.index_y][next.index_x][0].hc;

      // Calculate euclid distance heuristic cost
      if (!use_wavefront_heuristic_)
        next_hc = astar::calcDistance(next_x, next_y, goal_pose_local_.pose.position.x, goal_pose_local_.pose.position.y);

      // GOAL CHECK
      if (isGoal(next_x, next_y, next_theta)) {
        search_count = 0;
        next_node->status = STATUS::OPEN;
        next_node->x      = next_x;
        next_node->y      = next_y;
        next_node->theta  = next_theta;
        next_node->gc     = current_node->gc + move_cost;
        next_node->hc     = next_hc;
        next_node->back   = state.back;
        next_node->parent = current_node;

        setPath(next);
        return true;
      }

      // NONE
      if (next_node->status == STATUS::NONE) {
        next_node->status = STATUS::OPEN;
        next_node->x      = next_x;
        next_node->y      = next_y;
        next_node->theta  = next_theta;
        next_node->gc     = current_node->gc + move_cost;
        next_node->hc     = next_hc;
        next_node->back   = state.back;
        next_node->parent = current_node;

        next.cost = next_node->gc + next_node->hc;
        openlist_.push(next);
        continue;
      }

      // OPEN or CLOSED
      if (next_node->status == STATUS::OPEN || next_node->status == STATUS::CLOSED) {
        if (current_node->gc + move_cost + next_hc < next_node->gc + next_hc) {
          next_node->status = STATUS::OPEN;
          next_node->x      = next_x;
          next_node->y      = next_y;
          next_node->theta  = next_theta;
          next_node->gc     = current_node->gc + move_cost;
          next_node->hc     = next_hc;
          next_node->back   = state.back;
          next_node->parent = current_node;

          next.cost = next_node->gc + next_node->hc;
          openlist_.push(next);
          continue;
        }
      }

      if (search_count == 0)
        break;

    } // state update

  }

  // Failed to find path
  ROS_WARN("Openlist is Empty!");
  return false;
}
Node* AIManager::astar(Vector3f end, Vector3f begin)
{
    bool firstGo = true;
    Node* start = new Node(begin);
    Node* goal = new Node(end);
    Node* current;
    std::vector<Node*> openlist;
    std::vector<Node*> closelist;
    Vector3f goalvf = end;

    openlist.push_back(start);
    std::make_heap(openlist.begin(), openlist.end());

    while(!openlist.empty())
    {
        current = openlist.front();
        std::pop_heap(openlist.begin(), openlist.end());
        openlist.pop_back();
        if(isGoal(current, goal))
        {
            return current;
        }
    //    getSuccessors(current, openlist, closelist, end);
    //If adjacent position is not obstacle and not on closed
    //add adjacent positions to list
    Node* temp = new Node;
    bool valid = true;
    //if(!Overlay::isObstacle(current->_x+STEP, current->_y, current->_z))
    if(!checkObstacle(Vector3f(current->_x+STEP, current->_y, current->_z)))
    {
        temp->_x = current->_x+STEP;
        temp->_z = current->_z; 
        for(unsigned int i = 0; i < closelist.size() && valid; ++i)
        {
            if(temp->isSamePosition(closelist[i]))
                valid = false;
        }
        
        if(valid)
        {
            temp->_g = current->_g+GSCORE;
            temp->_h = calculateHn(Vector3f(float(temp->_x), float(temp->_y), float(temp->_z)), goalvf);
            temp->calculateFn();
            temp->setParent(current);
            openlist.push_back(temp);
            std::push_heap(openlist.begin(), openlist.end());
        }
    }
    //if(!Overlay::isObstacle(current->_x-STEP, current->_y, current->_z))
    if(!checkObstacle(Vector3f(current->_x-STEP, current->_y, current->_z)))
    {
        temp->_x = current->_x-STEP;
        temp->_z = current->_z; 
        for(unsigned int i = 0; i < closelist.size() && valid; ++i)
        {
            if(temp->isSamePosition(closelist[i]))
                valid = false;
        }
        if(valid)
        {
            temp->_g = current->_g+GSCORE;
            temp->_h = calculateHn(Vector3f(float(temp->_x), float(temp->_y), float(temp->_z)), goalvf);
            temp->calculateFn();
            temp->setParent(current);
            openlist.push_back(temp);
            std::push_heap(openlist.begin(), openlist.end());
        }
        valid = true;
    }
    //if(!Overlay::isObstacle(current->_x, current->_y, current->_z+STEP))
    if(!checkObstacle(Vector3f(current->_x, current->_y, current->_z+STEP)))
    {
        temp->_x = current->_x;
        temp->_z = current->_z+STEP; 
        for(unsigned int i = 0; i < closelist.size() && valid; ++i)
        {
            if(temp->isSamePosition(closelist[i]))
                valid = false;
        }
        if(valid)
        {
            temp->_g = current->_g+GSCORE;
            temp->_h = calculateHn(Vector3f(float(temp->_x), float(temp->_y), float(temp->_z)), goalvf);
            temp->calculateFn();
            temp->setParent(current);
            openlist.push_back(temp);
            std::push_heap(openlist.begin(), openlist.end());
        }
        valid = true;
    }
    //if(!Overlay::isObstacle(current->_x, current->_y, current->_z-STEP))
    if(!checkObstacle(Vector3f(current->_x, current->_y, current->_z-STEP)))
    {
        temp->_x = current->_x;
        temp->_z = current->_z-STEP; 
        for(unsigned int i = 0; i < closelist.size() && valid; ++i)
        {
            if(temp->isSamePosition(closelist[i]))
                valid = false;
        }
        if(valid)
        {
            temp->_g = current->_g+GSCORE;
            temp->_h = calculateHn(Vector3f(float(temp->_x), float(temp->_y), float(temp->_z)), goalvf);
            temp->calculateFn();
            temp->setParent(current);
            openlist.push_back(temp);
            std::push_heap(openlist.begin(), openlist.end());
        }
        valid = true;
    }
        if(firstGo && !openlist.empty())
        {
            std::make_heap(openlist.begin(), openlist.end());
            firstGo = false;
        }
        closelist.push_back(current);
        return current;
    }
    return NULL;

}