hybrid_breadth_first.cpp

#include <math.h>
#include <algorithm>
#include <iostream>
#include <vector>
#include <tuple>
#include "hybrid_breadth_first.h"

using namespace std;

// Initializes HBF
HBF::HBF() {}

HBF::~HBF() {}

bool HBF::compare_maze_s(const HBF::maze_s &lhs, const HBF::maze_s &rhs) {
  return lhs.f < rhs.f;
}

double HBF::heuristic(double x, double y, vector<int> &goal){
  return fabs(y - goal[0]) + fabs(x - goal[1]); //return grid distance to goal
}

int HBF::theta_to_stack_number(double theta){
  // Takes an angle (in radians) and returns which "stack" in the 3D 
  //   configuration space this angle corresponds to. Angles near 0 go in the 
  //   lower stacks while angles near 2 * pi go in the higher stacks.
  double new_theta = fmod((theta + 2 * M_PI),(2 * M_PI));
  int stack_number = (int)(round(new_theta * NUM_THETA_CELLS / (2*M_PI))) 
                   % NUM_THETA_CELLS;

  return stack_number;
}

int HBF::idx(double float_num) {
  // Returns the index into the grid for continuous position. So if x is 3.621, 
  //   then this would return 3 to indicate that 3.621 corresponds to array 
  //   index 3.
  return int(floor(float_num));
}

vector<HBF::maze_s> HBF::expand(HBF::maze_s &state, vector<int> &goal) {
  int g = state.g;
  double x = state.x;
  double y = state.y;
  double theta = state.theta;
    
  int g2 = g+1;
  vector<HBF::maze_s> next_states;

  for(double delta_i = -35; delta_i < 40; delta_i+=35) { //+= 5
    double delta = M_PI / 180.0 * delta_i;
    double omega = SPEED / LENGTH * tan(delta);
    double theta2 = theta + omega;
    if(theta2 < 0) {
      theta2 += 2*M_PI;
    }
    double x2 = x + SPEED * cos(theta);
    double y2 = y + SPEED * sin(theta);
    HBF::maze_s state2;
    state2.f = g2 + heuristic(x2, y2, goal);
    
    state2.g = g2;
    state2.x = x2;
    state2.y = y2;
    // state2.f = g2 + H_grid[x2][y2];
    state2.theta = theta2;
    next_states.push_back(state2);
  }

  return next_states;
}

vector< HBF::maze_s> HBF::reconstruct_path(
  vector<vector<vector<HBF::maze_s>>> &came_from, vector<double> &start, 
  HBF::maze_s &final) {

  vector<maze_s> path = {final};
  
  int stack = theta_to_stack_number(final.theta);

  maze_s current = came_from[stack][idx(final.x)][idx(final.y)];
  
  stack = theta_to_stack_number(current.theta);
  
  double x = current.x;
  double y = current.y;

  while(x != start[0] || y != start[1]) {
    path.push_back(current);
    current = came_from[stack][idx(x)][idx(y)];
    x = current.x;
    y = current.y;
    stack = theta_to_stack_number(current.theta);
  }
  
  return path;
}

HBF::maze_path HBF::search(vector< vector<int> > &grid, vector<double> &start, 
                           vector<int> &goal) {
  // Working Implementation of breadth first search. Does NOT use a heuristic
  //   and as a result this is pretty inefficient. Try modifying this algorithm 
  //   into hybrid A* by adding heuristics appropriately.

  /**
   * TODO: Add heuristics and convert this function into hybrid A*
   */
  vector<vector<vector<int>>> closed( NUM_THETA_CELLS, vector<vector<int>>(grid[0].size(), vector<int>(grid.size())));
  vector<vector<vector<maze_s>>> came_from(NUM_THETA_CELLS, vector<vector<maze_s>>(grid[0].size(), vector<maze_s>(grid.size())));
  double theta = start[2];
  int stack = theta_to_stack_number(theta);
  int g = 0;

  maze_s state;
  state.g = g;
  state.x = start[0];
  state.y = start[1];
  state.f = g + heuristic(state.x, state.y, goal);
  state.theta = theta;
  
//   A star start;
    cout<<"***************"<<endl;
    cout<<"Start Astar search"<<endl;
    cout<<"***************"<<endl;
    vector<vector<int>> H_grid = grid, explored = grid;
    vector<tuple<int,int,int,int>> open; //<include<tuple>; //x, y, cost_from_start, f.
    open.push_back(make_tuple(0,state.x,state.y, 0));
    explored[state.x][state.y] = 2;
    vector<vector<int>> delta = { {1,0}, {0,-1}, {-1,0}, {0,1} };

    while(open.size()!=0){
        tuple<int,int,int,int> current = open[0];
        //check if current i s goal.
        open.erase(open.begin());
        int x = get<1>(current);
        int y = get<2>(current);
        if(x == goal[0] && y == goal[1]){
            cout<<"A star search complete"<<endl;
            break;
        }
        for(int i = 0; i<delta.size(); i++){
            int x_neighbor = x + delta[i][0];
            int y_neighbor = y + delta[i][1];
            if((x_neighbor < 0 || x_neighbor >= grid.size()) || (y_neighbor < 0 || y_neighbor >= grid[0].size()))
                continue;
            if(explored[x_neighbor][y_neighbor]==0){
                int cost = get<3>(current) + 1;
                int h = abs(x_neighbor-x) + abs(y_neighbor-y);
                open.push_back( make_tuple(cost+h, x_neighbor, y_neighbor, cost ));
                H_grid[x_neighbor][y_neighbor] = cost + h;
                explored[x_neighbor][y_neighbor] = 2;
            }
        sort(open.begin(), open.end());
        }
    }
    for(int i = 0; i < H_grid.size(); ++i) {
    cout << H_grid[i][0];
    for(int j = 1; j < H_grid[0].size(); ++j) {
      cout << "," << H_grid[i][j];
    }
    cout << endl;
  }
//  A star complete;

  closed[stack][idx(state.x)][idx(state.y)] = 1;
  came_from[stack][idx(state.x)][idx(state.y)] = state;
  int total_closed = 1;
  vector<maze_s> opened = {state};
  bool finished = false;
  int flag = 0;
  int counter = 0;
  while(!opened.empty()) {
    sort(opened.begin(), opened.end(), compare_maze_s);
    maze_s current = opened[0]; //grab first elment
    opened.erase(opened.begin()); //pop first element

    int x = current.x;
    int y = current.y;

    if(idx(x) == goal[0] && idx(y) == goal[1]) {
      std::cout << "found path to goal in " << total_closed << " expansions" 
                << std::endl;
      maze_path path;
      path.came_from = came_from;
      path.closed = closed;
      path.final = current;

      return path;
    }

    vector<maze_s> next_state = expand(current, goal);
    

    for(int i = 0; i < next_state.size(); ++i) {
      int g2 = next_state[i].g;
      double x2 = next_state[i].x;
      double y2 = next_state[i].y;
      double theta2 = next_state[i].theta;
      if(flag==0){
          cout<<"Next x is : "<<x2<<endl;
            cout<<"Next y is : "<<y2<<endl;
            cout<<"***************"<<endl;
      }
      

    //   next_state[i].f = -H_grid[x2][y2];

      
      if((x2 < 0 || x2 >= grid.size()) || (y2 < 0 || y2 >= grid[0].size())) {
        // invalid cell
        continue;
      }

      int stack2 = theta_to_stack_number(theta2);

      if(closed[stack2][idx(x2)][idx(y2)] == 0 && grid[idx(x2)][idx(y2)] == 0) {
        opened.push_back(next_state[i]);
        closed[stack2][idx(x2)][idx(y2)] = 1;
        came_from[stack2][idx(x2)][idx(y2)] = current;
        ++total_closed;
      }
    }
    counter++;
    // cout<<"Single iteration Done"<<endl;
    if(counter>1)
        flag = 1;
  }

  std::cout << "no valid path." << std::endl;
  HBF::maze_path path;
  path.came_from = came_from;
  path.closed = closed;
  path.final = state;
  return path;
}