pos_t gps_liblocation_state_t::get_pos(float* alt)
{
if(enabled && fix) {
if(alt)
*alt = altitude;
return pos;
} else {
return pos_t(NAN, NAN);
}
}
void image_base::resize(image_ptr target) {
if (target->get_width() > get_width()) {
// scale up
unsigned int factor = target->get_width() / get_width();
boost::shared_array<color> line(new color[get_width()]);
unsigned int scanline = 0;
bool first = false;
for (pos_t y = 0; y < target->get_height(); y++) {
pos_t y_p = (y / factor);
if (y_p != scanline || !first) {
get_line(y_p, pos_t(0), get_width(), line.get());
first = true;
scanline = y_p;
}
for (pos_t x = 0; x < target->get_width(); x++) {
pos_t x_p = (x / factor);
target->set_pixel(x, y, line[x_p]);
}
}
}
else {
// scale down
unsigned int factor = get_width() / target->get_width();
boost::shared_array<color> line(new color[get_width()]);
for (pos_t y = 0; y < target->get_height(); y++) {
pos_t y_p = ((y * factor) + factor / 2);
get_line(y_p, pos_t(0), get_width(), line.get());
for (pos_t x = 0; x < target->get_width(); x++) {
pos_t x_p = ((x * factor) + factor / 2);
target->set_pixel(x, y, line[x_p]);
}
}
}
}
bool Sokoban::loadMap(std::string filename){
int D = 0;
// make pointer to file
std::cout << "Loading file... ";
std::ifstream file(filename);
if(!file.is_open()){
std::cout << "Couldn't open the file" << std::endl;
return false;
}
std::cout << "Done." << std::endl;
// read file letter by letter
char c;
// load file dimensions and diamonds
char entry = 0;
int value = 0;
std::cout << "Loading map settings... ";
do{
file.get(c);
if(c >= '0' && c <= '9'){
value = value * 10 + (c - '0');
}
else{
switch (entry) {
case 0:
map_size_.x_ = value;
break;
case 1:
map_size_.y_ = value;
break;
case 2:
D = value;
break;
default:
break;
}
entry++;
value = 0;
}
}while (!(c == '\0' || c == '\n'));
std::cout << "Done." << std::endl;
std::cout << "w: " << (int)map_size_.x_ << ", h: " << (int)map_size_.y_ << ", D: " << D << std::endl;
std::cout << "Loading Map...";
// init vectors
diamonds_.clear();
goals_.clear();
map_.resize(map_size_.y_);
for(int i = 0; i < map_size_.y_; i++){
(map_[i]).resize(map_size_.x_);
}
// load map
// X || ' ' = wall, J = diamond, G = goal, . = free space, M = robot, * = J + G
int x = 0;
int y = 0;
do{
file.get(c);
if(c == 'X' || c == ' '){
map_[y][x] = MAP_WALL;
}
else if(c == 'J'){
map_[y][x] = MAP_FREE;
diamonds_.emplace_back(pos_t(x,y));
}
else if(c == 'G'){
map_[y][x] = MAP_GOAL;
goals_.emplace_back(pos_t(x,y));
}
else if(c == '*'){
map_[y][x] = MAP_GOAL;
goals_.emplace_back(pos_t(x,y));
diamonds_.emplace_back(pos_t(x,y));
}
else if(c == 'M'){
map_[y][x] = MAP_FREE;
robot_.x_ = x;
robot_.y_ = y;
}
else if(c == '.'){
map_[y][x] = MAP_FREE;
}
else {
y++;
x = -1;
}
x++;
}while (!file.eof());
std::cout << "Done." << std::endl;
// close stream
file.close();
// return true
return true;
}
std::vector<std::vector<float>> Map::find_robot_moves(Map ©_map, node* N){
std::vector< std::vector<float> > cost_map(width, std::vector<float>(height) );
copy_map = *this;
for(auto n : N->diamonds){
copy_map.set(n, DIAMOND);
}
for(int x = 0; x < width; ++x){
for(int y = 0; y < height; ++y){
cost_map[x][y] = data[x][y];
}
}
for(auto n : N->diamonds){
cost_map[n.x][n.y] = DIAMOND;
}
float distance = 3; // this means distance is (color - 3)
std::priority_queue<robot_move,std::vector<robot_move>,robot_cost_comparator_functor> open_list;
std::queue<robot_move> surrounding_points;
cost_map[N->man.x][N->man.y] = distance;
if( N->parent == nullptr){
open_list.push(robot_move(distance, N->man, 'N'));
open_list.push(robot_move(distance, N->man, 'S'));
open_list.push(robot_move(distance, N->man, 'E'));
open_list.push(robot_move(distance, N->man, 'W'));
} else {
for(size_t i = 0; i < N->diamonds.size(); ++i){
if(N->man == N->parent->diamonds.at(i) ){
pos_t displacement = N->diamonds.at(i) - N->parent->diamonds.at(i);
if( displacement == above){ open_list.push(robot_move(distance, N->man, 'N'));
} else if(displacement == below){ open_list.push(robot_move(distance, N->man, 'S'));
} else if(displacement == right){ open_list.push(robot_move(distance, N->man, 'E'));
} else if(displacement == left){ open_list.push(robot_move(distance, N->man, 'W'));
}
}
}
}
robot_move current_move;
while( !open_list.empty() ) {
current_move = open_list.top();
open_list.pop();
if( cost_map[current_move.pos.x][current_move.pos.y] == FREE ){
cost_map[current_move.pos.x][current_move.pos.y] = current_move.cost;
copy_map.set(current_move.pos, current_move.direction);
}
surrounding_points = add_connected_moves( current_move, cost_map);
while( !surrounding_points.empty() ) {
open_list.push( surrounding_points.front() );
surrounding_points.pop();
}
}
char entry = 'q';
//remove all unreachables
for(auto i : N->diamonds){
float min = 1e6;
for(int x = -1; x <= 1; ++x){
for(int y = -1; y <=1; ++y){
if( (x == 0) ^ (y == 0) ){
if( (cost_map[i.x+x][i.y+y] >= 3) && (cost_map[i.x+x][i.y+y] < 1e6) ){
min = cost_map[i.x+x][i.y+y];
entry = copy_map.get(pos_t(i.x+x,i.y+y));
}
}
}
}
if(min == 1e6){
cost_map[i.x][i.y] = 1e6;
}
}
//add cost to all reachable diamonds
for(auto i : N->diamonds){
float min = 1e6;
float turn_penalty = 0;
if( cost_map[i.x][i.y] == DIAMOND){
for(int x = -1; x <= 1; ++x){
for(int y = -1; y <=1; ++y){
if( (x==0) ^ (y==0) ){
if( (cost_map[i.x+x][i.y+y] >= 3) && (cost_map[i.x+x][i.y+y] < min) && (copy_map.get(pos_t(i.x+x,i.y+y)) != DIAMOND) && ( (copy_map.get(pos_t(i.x-x,i.y-y)) > 3) || (copy_map.get(pos_t(i.x-x,i.y-y)) == FREE) ) ){
min = cost_map[i.x+x][i.y+y];
entry = copy_map.get(pos_t(i.x+x,i.y+y));
pos_t direction = pos_t(x,y); //eg. if there is a free spot on the right and the entry is facing north, the robot have to turn left
switch(entry){
case 'N':
if( direction == above){ turn_penalty = 0; }
else if(direction == right){ turn_penalty = LEFT_COST; }
else if(direction == left ){ turn_penalty = RIGHT_COST; }
break;
case 'S':
if( direction == below){ turn_penalty = 0; }
else if(direction == right){ turn_penalty = RIGHT_COST; }
else if(direction == left ){ turn_penalty = LEFT_COST; }
break;
case 'E':
if( direction == above){ turn_penalty = LEFT_COST; }
else if(direction == below){ turn_penalty = RIGHT_COST; }
else if(direction == right){ turn_penalty = 0; }
break;
case 'W':
if( direction == above){ turn_penalty = RIGHT_COST; }
else if(direction == below){ turn_penalty = LEFT_COST; }
else if(direction == left ){ turn_penalty = 0; }
default:
break;
}
if(N->man != i+direction && turn_penalty > 0){
turn_penalty += BACK_COST;
}
}
}
}
}
cost_map[i.x][i.y] = min + FORWARD_COST + turn_penalty;
}
}
for(int x = 1; x < width-1; ++x){
for(int y = 1; y < height-1; ++y){
if(cost_map[x][y] != 1e6)
copy_map.set(pos_t(x,y),cost_map[x][y]);
}
}
return move(cost_map);
}
// graph where s_i = {pos_D, pos_R, cost}, pointer to parents.
// possible entries (diamond pushes) are added with wavefront
// the shortest path till now is explored further (heap)
// hash table to check if node exists
bool Sokoban::findPath(){
printMap(map_);
std::cout << "Init. finding paths..." << std::endl;
// graph to store the tree of possibilities
graph paths;
// has a solution been found
bool solution_found = false;
// - make needed maps
// heuristic map
std::vector< std::vector<char> > heuristic_map = map_;
wavefront(heuristic_map, goals_);
// printMap(heuristic_map);
// std::cout << std::endl;
// deadlock map
std::vector< std::vector<char> > deadlock_map = map_;
deadlocks(deadlock_map);
// printMap(deadlock_map);
// the next child to consider (lowest cost)
node * cheapest_leaf = nullptr;
// node to add
node nextnode(0, cheapest_leaf, heuristic_map);
nextnode.setRobot(pos_t(robot_.x_, robot_.y_));
nextnode.setDiamonds(diamonds_);
std::cout << "# of Diamonds: " << diamonds_.size() << std::endl;
// add the initial position
paths.createChild(nextnode);
int lastCost = 0;
std::cout << "Exploring paths..." << std::endl;
// run this until the path is found or no paths can be taken
int iterations = 0; // debug thing
while(!solution_found && paths.getNextChild(cheapest_leaf)){
iterations++;
// if(lastCost < cheapest_leaf->getCost() + cheapest_leaf->getHeuristic()){
// lastCost = cheapest_leaf->getCost() + cheapest_leaf->getHeuristic();
// std::cout << "current cost: " << lastCost << ", open-/closed-list size: " << paths.getOpenListSize() << " / " << paths.getClosedListSize() << std::endl;
// }
// check cheapest leaf if it is the solution
solution_found = cheapest_leaf->compSolution(map_);
if(!solution_found){
// generate the key for the element
std::string key = cheapest_leaf->getKey(map_size_.x_);
// check that this node has not already been found, if so, remove it
bool unique_node = paths.nodeUnique(key);
bool valid_node = false;
std::vector< pos_t > * diamonds;
if(unique_node){
paths.addChild(cheapest_leaf, key);
// get diamonds pointer
diamonds = cheapest_leaf->getDiamonds();
// check that the node is valid (all diamonds are in valid positions)
valid_node = validNode(diamonds, deadlock_map);
}
else{
paths.deleteChild(cheapest_leaf);
}
std::vector< std::vector<char> > wf_map;
if(unique_node && valid_node){
wf_map = map_;
// make wf map
for(int d = 0; d < diamonds->size(); d++){
int x = (*diamonds)[d].x_;
int y = (*diamonds)[d].y_;
wf_map[y][x] = MAP_DIAMOND;
}
// make wavefront from pos to see the distance to the diamonds
std::vector< pos_t > robot_position = {*(cheapest_leaf->getRobotPos())};
wavefront(wf_map, robot_position);
// find the diamonds that can be moved
std::vector< node > moves;
possibleMoves(wf_map, diamonds, moves, cheapest_leaf);
// add the moves to the heap (does not take care of states being the same)
for(int newChild = 0; newChild < moves.size(); newChild++){
// update heuristics first
moves[newChild].updateHeuristic(heuristic_map);
// add the child
// paths.createChild(moves[newChild]);
}
paths.createChild(moves);
}
}
else{
// return shortest path
node * parent = cheapest_leaf->getParent();
node * child = cheapest_leaf;
path_ = "";
while(parent != nullptr){
std::vector< pos_t > p_diamonds;
std::vector< pos_t > c_diamonds;
std::vector< std::vector<char> > wf_map;
p_diamonds = *(parent->getDiamonds());
c_diamonds = *(child->getDiamonds());
// set diamonds to walls to generate wfamp to move with
wf_map = map_;
for(int d = 0; d < p_diamonds.size(); d++){
int x = (p_diamonds)[d].x_;
int y = (p_diamonds)[d].y_;
wf_map[y][x] = MAP_DIAMOND;
}
// robot pos
pos_t p_position = *(parent->getRobotPos());
pos_t c_position = *(child->getRobotPos());
std::vector< pos_t > wf_start = {p_position};
wavefront(wf_map, wf_start);
// find the diamond that was moved
int p_d = 0;
int c_d = 0;
while(p_diamonds.size() != 1){
bool somethingremoved = false;
c_d = 0;
while(c_d < c_diamonds.size() && !somethingremoved){
if((p_diamonds)[p_d].y_ == (c_diamonds)[c_d].y_ &&(p_diamonds)[p_d].x_ == (c_diamonds)[c_d].x_){
// remove the diamond
p_diamonds.erase(p_diamonds.begin() + p_d);
c_diamonds.erase(c_diamonds.begin() + c_d);
somethingremoved = true;
}
c_d++;
}
p_d++;
if(p_d >= p_diamonds.size()){
p_d = 0;
}
}
// adjust the start pos to be t push spot
int x = (p_diamonds)[0].x_ - (c_diamonds)[0].x_;
int y = (p_diamonds)[0].y_ - (c_diamonds)[0].y_;
c_position.x_ += x;
c_position.y_ += y;
// find sub path
std::string subpath = "";
findSubPath(subpath,wf_map, p_position, c_position);
// add the final push
if(x == 1 && y == 0){
subpath += "W";
} else if(x == -1 && y == 0){
subpath += "E";
} else if(x == 0 && y == 1){
subpath += "N";
} else if(x == 0 && y == -1){
subpath += "S";
}
// add the path
path_ = subpath + path_;
// find new nodes
child = parent;
parent = child->getParent();
}
std::cout << "Cheapest path length: " << cheapest_leaf->getCost() << std::endl;
}
}
if(!paths.getNextChild(cheapest_leaf)){
std::cout << "No more possible paths left to explore.\n";
}
std::cout << "# of iterations gone through: " << iterations << "\n";
std::cout << "# of elements in closed list: " << paths.getClosedListSize() << "\n";
std::cout << "# of elements in open list: " << paths.getOpenListSize() << "\n";
return solution_found;
}
