directed_dfs.cpp

Go to the documentation of this file.

#include <bwi_mapper/directed_dfs.h>
#include <bwi_mapper/map_utils.h>

namespace bwi_mapper {

  bool DirectedDFS::searchForPath(const Point2d &start, const Point2d &goal,
      uint32_t depth, bool in_obstacle_space) {

    std::vector<bool> visited(map_.info.height * map_.info.width, false);
    return searchForPath(start, goal, depth, visited, in_obstacle_space);
  }

  bool DirectedDFS::searchForPath(const Point2d &start, const Point2d &goal, 
      uint32_t depth, std::vector<bool> &visited, bool in_obstacle_space) {

    //std::cout << start.x << " " << start.y << std::endl;

    // Termination crit
    if (start == goal) {
      return true;
    }
    if (depth == 0) {
      return false;
    }

    uint32_t start_idx = MAP_IDX(map_.info.width, start.x, start.y);
    visited[start_idx] = true;

    std::vector<Point2d> neighbours;
    getOrderedNeighbours(start, goal, visited, neighbours, in_obstacle_space);
    for (size_t i = 0; i < neighbours.size(); ++i) {
      Point2d& n = neighbours[i];
      // Check if it has been visited again - quite likely that one of the 
      // previous loop iterations have covered this already
      uint32_t n_idx = MAP_IDX(map_.info.width, n.x, n.y);
      if (visited[n_idx]) {
        continue;
      }
      bool success = searchForPath(n, goal, depth - 1, visited);
      if (success)
        return true;
    }

    return false; // disconnected components
  }


  void DirectedDFS::getOrderedNeighbours(const Point2d &from, 
      const Point2d &goal, const std::vector<bool> &visited, 
      std::vector<Point2d> &neighbours, bool in_obstacle_space) {

    size_t neighbour_count = 8;
    uint32_t x_offset[] = {-1, 0, 1, -1, 1, -1, 0, 1};
    uint32_t y_offset[] = {-1, -1, -1, 0, 0, 1, 1, 1};
    neighbours.clear();
    for (size_t i = 0; i < neighbour_count; ++i) {
      // Check if neighbours are still on map
      Point2d p = from + Point2d(x_offset[i],y_offset[i]);
      // covers negative case as well (unsigned)
      if (p.x >= (int) map_.info.width || p.y >= (int) map_.info.height ||
          p.x <= 0 || p.y <= 0) { 
        continue;
      }
      uint32_t map_idx = MAP_IDX(map_.info.width, p.x, p.y);
      if (visited[map_idx] || (in_obstacle_space && map_.data[map_idx] == 0) ||
          (!in_obstacle_space && map_.data[map_idx] != 0)) {
        continue;
      }
      p.distance_from_ref = norm(p - goal); 
      neighbours.push_back(p);
    }
    std::sort(neighbours.begin(), neighbours.end(), Point2dDistanceComp());
  }

} /* bwi_mapper */