Я пытаюсь реализовать алгоритм поиска A * в C ++.Моя проблема заключается в том, что моя реализация не выбирает один из вариантов, а исследует оба.
#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> ¤t, 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> ¤t, 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 ";
}
}
Теперь он печатает:
S 1 0 0 0 0
= 1 x x x 0
= 1 = = = F
= 1 = = x 0
= = = x 1 0
Я ожидал бы напечатать
S 1 0 0 0 0
= 1 x x x 0
= 1 = = = F
= 1 = x x 0
= = = x 1 0
Файл 1.board выглядит следующим образом:
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,