lundi 1 juillet 2019

A* search two possibilities

I'm trying to implement the A* search algo in C++. My issue is that my implementation doesn't pick one of the options but explorers both of them.

#include <iostream>
#include <vector>
#include <fstream>
#include <sstream>
#include <algorithm>

using std::cout;
using std::vector;
using std::string;
using std::ifstream;
using std::istringstream;
using std::sort;

enum class State {
    kEmpty, kObstacle, kClosed, kStart, kFinish, kPath
};

const int directionDeltas[4][2]{
        {0,  -1},
        {1,  0},
        {0,  1},
        {-1, 0}
};

vector<vector<State>> ReadBoard(const string &path);

vector<State> ParseLine(const string &line);

void PrintBoard(const vector<vector<State>> &board);

const string CellString(const State &state);

vector<vector<State>> Search(vector<vector<State>> &board, const vector<int> &init, const vector<int> &goal);

int Heuristic(const int &x1, const int &y1, const int &x2, const int &y2);

void AddToOpen(vector<int> current, vector<vector<int>> &open, vector<vector<State>> &board);

void SortOpen(vector<vector<int>> *vector);

void ExpandNeighbours(const vector<int> &current, const vector<int> &goal, vector<vector<int>> &open,
                      vector<vector<State>> &board);

bool ValidateCell(const int &x, const int &y, const vector<vector<State>> &board);

int main() {
    vector<int> init{0, 0};
    vector<int> goal{2, 5};

    vector<vector<State>> board = ReadBoard("../1.board");
    vector<vector<State>> solution = Search(board, init, goal);
    PrintBoard(solution);
    return 0;
}

vector<vector<State>> Search(vector<vector<State>> &board, const vector<int> &init, const vector<int> &goal) {
    vector<vector<int >> open{};

    int x = init[0];
    int y = init[1];
    int g = 0;
    int h = Heuristic(x, y, goal[0], goal[1]);
    AddToOpen(vector<int>{x, y, g, h}, open, board);


    while (!open.empty()) {
        SortOpen(&open);
        vector<int> current = open.back();
        open.pop_back();

        int x2 = current[0];
        int y2 = current[1];
        board[x2][y2] = State::kPath;

        if (x2 == goal[0] && y2 == goal[1]) {
            board[init[0]][init[1]] = State::kStart;
            board[goal[0]][goal[1]] = State::kFinish;

            return board;
        }

        ExpandNeighbours(current, goal, open, board);
    }

    cout << "Could not find a path." << "\n";
    return vector<vector<State>>{};
}

void ExpandNeighbours(const vector<int> &current, const vector<int> &goal, vector<vector<int>> &open,
                      vector<vector<State>> &board) {

    int x = current[0];
    int y = current[1];
    int g = current[2];

    for (auto direction: directionDeltas) {
        int x2 = x + direction[0];
        int y2 = y + direction[1];


        if (ValidateCell(x2, y2, board)) {
            int g2 = g + 1;
            int h2 = Heuristic(x2, y2, goal[0], goal[1]);

            AddToOpen(vector<int>{x2, y2, g2, h2}, open, board);
        }
    }
}

bool ValidateCell(const int &x, const int &y, const vector<vector<State>> &board) {
    bool x_valid = x >= 0 && x < board.size();
    bool y_valid = y >= 0 && y < board[0].size();

    return x_valid && y_valid && board[x][y] == State::kEmpty;
}

bool Compare(const vector<int> &a, const vector<int> &b) {
    int f1 = a[2] + a[3];
    int f2 = b[2] + b[3];

    return f1 > f2;
}

void SortOpen(vector<vector<int>> *vector) {
    sort(vector->begin(), vector->end(), Compare);
}


void AddToOpen(vector<int> current, vector<vector<int>> &open, vector<vector<State>> &board) {
    board[current[0]][current[1]] = State::kClosed;
    open.push_back(current);
}

int Heuristic(const int &x1, const int &y1, const int &x2, const int &y2) {
    return abs(x2 - x1) + abs(y2 - y1);
}

vector<vector<State>> ReadBoard(const string &path) {
    vector<vector<State>> board{};

    ifstream file(path);
    if (file) {
        string line;
        while (getline(file, line)) {
            vector<State> row = ParseLine(line);
            board.push_back(row);
        }
    }

    return board;
}

vector<State> ParseLine(const string &line) {
    vector<State> row{};
    istringstream s_line(line);

    int n;
    char c;
    if (s_line) {
        while (s_line >> n >> c && c == ',') {
            if (n == 0) {
                row.push_back(State::kEmpty);
            } else {
                row.push_back(State::kObstacle);
            }
        }
    }

    return row;
}

void PrintBoard(const vector<vector<State>> &board) {
    for (const vector<State> &row : board) {
        for (const State &state: row) {
            cout << CellString(state);
        }
        cout << "\n";
    }
}

const string CellString(const State &state) {
    switch (state) {
        case State::kEmpty:
            return "0   ";
        case State::kObstacle:
            return "1   ";
        case State::kClosed:
            return "x   ";
        case State::kPath:
            return "=   ";
        case State::kStart:
            return "S   ";
        case State::kFinish:
            return "F   ";
    }
}


Now it prints:

S   1   0   0   0   0   
=   1   x   x   x   0   
=   1   =   =   =   F   
=   1   =   =   x   0   
=   =   =   x   1   0   

I would expect to print

S   1   0   0   0   0   
=   1   x   x   x   0   
=   1   =   =   =   F   
=   1   =   x   x   0   
=   =   =   x   1   0   

The 1.board file looks like this:

0,1,0,0,0,0,
0,1,0,0,0,0,
0,1,0,0,0,0,
0,1,0,0,0,0,
0,0,0,0,1,0,

Aucun commentaire:

Enregistrer un commentaire