Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Refactoring the A* walkpath algorithm to use c++ features instead of magic words #8127

Closed
wants to merge 12 commits into from
Closed

Refactoring the A* walkpath algorithm to use c++ features instead of magic words #8127

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
9404bcd
A* macros refactor
AoShinRO Feb 9, 2024
381524c
vstumpf requested changes
AoShinRO Feb 19, 2024
b0e4057
pushing heap at start
AoShinRO Feb 20, 2024
a2b3528
open_heap as new class
AoShinRO Feb 21, 2024
1898589
indexing the heap
AoShinRO Feb 21, 2024
de4726a
cleaning a whitespace
AoShinRO Feb 21, 2024
c9f051a
cleaning some duplicated macros
AoShinRO Feb 21, 2024
15e2467
single heap comparion + heap node update
AoShinRO Feb 21, 2024
2f8b599
finished tracking by indexes
AoShinRO Feb 21, 2024
4b76799
done?
AoShinRO Feb 23, 2024
8f34d6c
merging update func
AoShinRO Feb 23, 2024
cd3346d
changing std::find to ARR_FIND
AoShinRO Feb 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
85 changes: 33 additions & 52 deletions src/map/navi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,12 +49,7 @@ struct path_node {
};

/// Binary heap of path nodes
BHEAP_STRUCT_DECL(node_heap, struct path_node*);
static BHEAP_STRUCT_VAR(node_heap, g_open_set); // use static heap for all path calculations
// it get's initialized in do_init_path, freed in do_final_path.

/// Comparator for binary heap of path nodes (minimum cost at top)
#define NODE_MINTOPCMP(i,j) ((i)->f_cost - (j)->f_cost)
static std::vector<path_node*> g_open_set; // use static heap for all path calculations

#define calc_index(x,y) (((x)+(y)*MAX_WALKPATH_NAVI) & (MAX_WALKPATH_NAVI*MAX_WALKPATH_NAVI-1))

Expand All @@ -75,39 +70,13 @@ static enum directions walk_choices [3][3] =
/// @name A* pathfinding related functions
/// @{

/// Pushes path_node to the binary node_heap.
/// Ensures there is enough space in array to store new element.

static void heap_push_node(struct node_heap *heap, struct path_node *node)
AoShinRO marked this conversation as resolved.
Show resolved Hide resolved
{
#ifndef __clang_analyzer__ // TODO: Figure out why clang's static analyzer doesn't like this
BHEAP_ENSURE2(*heap, 1, 256, struct path_node **);
BHEAP_PUSH2(*heap, node, NODE_MINTOPCMP);
#endif // __clang_analyzer__
}

/// Updates path_node in the binary node_heap.
static int heap_update_node(struct node_heap *heap, struct path_node *node)
{
int i;
ARR_FIND(0, BHEAP_LENGTH(*heap), i, BHEAP_DATA(*heap)[i] == node);
if (i == BHEAP_LENGTH(*heap)) {
ShowError("heap_update_node: node not found\n");
return 1;
}
BHEAP_UPDATE(*heap, i, NODE_MINTOPCMP);
return 0;
}
// end 1:1 copy of definitions from path.cpp


// So we don't have to allocate every time, use static structures
static struct path_node tp[MAX_WALKPATH_NAVI * MAX_WALKPATH_NAVI + 1];
static int tpused[MAX_WALKPATH_NAVI * MAX_WALKPATH_NAVI + 1];

/// Path_node processing in A* pathfinding.
/// Adds new node to heap and updates/re-adds old ones if necessary.
static int add_path(struct node_heap *heap, int16 x, int16 y, int g_cost, struct path_node *parent, int h_cost)
static int add_path(std::vector<path_node*>& heap, int16 x, int16 y, int g_cost, struct path_node *parent, int h_cost)
{
int i = calc_index(x, y);

Expand All @@ -118,9 +87,15 @@ static int add_path(struct node_heap *heap, int16 x, int16 y, int g_cost, struct
tp[i].parent = parent;
tp[i].f_cost = g_cost + h_cost;
if (tp[i].flag == SET_CLOSED) {
heap_push_node(heap, &tp[i]); // Put it in open set again
heap.push_back(&tp[i]);
std::push_heap(heap.begin(), heap.end(), [](const path_node* a, const path_node* b) {
return a->f_cost > b->f_cost;
AoShinRO marked this conversation as resolved.
Show resolved Hide resolved
});
}
else if (heap_update_node(heap, &tp[i])) {
else if (std::find(heap.begin(), heap.end(), &tp[i]) != heap.end()) {
std::make_heap(heap.begin(), heap.end(), [](const path_node* a, const path_node* b) {
return a->f_cost > b->f_cost;
});
return 1;
}
tp[i].flag = SET_OPEN;
Expand All @@ -139,7 +114,10 @@ static int add_path(struct node_heap *heap, int16 x, int16 y, int g_cost, struct
tp[i].f_cost = g_cost + h_cost;
tp[i].flag = SET_OPEN;
tpused[i] = 1 + (x << 16 | y);
heap_push_node(heap, &tp[i]);
heap.push_back(&tp[i]);
std::push_heap(heap.begin(), heap.end(), [](const path_node* a, const path_node* b) {
return a->f_cost > b->f_cost;
});
return 0;
}
///@}
Expand Down Expand Up @@ -189,7 +167,7 @@ bool navi_path_search(struct navi_walkpath_data *wpd, const struct navi_pos *fro
// A* (A-star) pathfinding
// We always use A* for finding walkpaths because it is what game client uses.
// Easy pathfinding cuts corners of non-walkable cells, but client always walks around it.
BHEAP_RESET(g_open_set);
g_open_set.clear();

memset(tpused, 0, sizeof(tpused));

Expand All @@ -203,7 +181,8 @@ bool navi_path_search(struct navi_walkpath_data *wpd, const struct navi_pos *fro
tp[i].flag = SET_OPEN;
tpused[i] = 1 + (from->x << 16 | from->y);

heap_push_node(&g_open_set, &tp[i]); // Put start node to 'open' set
g_open_set.push_back(&tp[i]);
AoShinRO marked this conversation as resolved.
Show resolved Hide resolved
current = g_open_set.front();
AoShinRO marked this conversation as resolved.
Show resolved Hide resolved

for (;;) {
int e = 0; // error flag
Expand All @@ -218,12 +197,16 @@ bool navi_path_search(struct navi_walkpath_data *wpd, const struct navi_pos *fro

int g_cost;

if (BHEAP_LENGTH(g_open_set) == 0) {
if (g_open_set.size() == 0)
return false;
}

current = BHEAP_PEEK(g_open_set); // Look for the lowest f_cost node in the 'open' set
BHEAP_POP2(g_open_set, NODE_MINTOPCMP); // Remove it from 'open' set
// Look for the lowest f_cost node in the 'open' set
auto it = std::min_element(g_open_set.begin(), g_open_set.end(), [](const path_node* a, const path_node* b) {
return a->f_cost < b->f_cost;
});
AoShinRO marked this conversation as resolved.
Show resolved Hide resolved

current = *it;
g_open_set.erase(it); // Remove it from 'open' set
AoShinRO marked this conversation as resolved.
Show resolved Hide resolved

x = current->x;
y = current->y;
Expand All @@ -243,21 +226,21 @@ bool navi_path_search(struct navi_walkpath_data *wpd, const struct navi_pos *fro
#define chk_dir(d) ((allowed_dirs & (d)) == (d))
// Process neighbors of current node
if (chk_dir(PATH_DIR_SOUTH|PATH_DIR_EAST) && !map_getcellp(mapdata, x+1, y-1, cell))
e += add_path(&g_open_set, x+1, y-1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x+1, y-1, dest->x, dest->y)); // (x+1, y-1) 5
e += add_path(g_open_set, x+1, y-1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x+1, y-1, dest->x, dest->y)); // (x+1, y-1) 5
if (chk_dir(PATH_DIR_EAST))
e += add_path(&g_open_set, x+1, y, g_cost + MOVE_COST, current, heuristic(x+1, y, dest->x, dest->y)); // (x+1, y) 6
e += add_path(g_open_set, x+1, y, g_cost + MOVE_COST, current, heuristic(x+1, y, dest->x, dest->y)); // (x+1, y) 6
if (chk_dir(PATH_DIR_NORTH|PATH_DIR_EAST) && !map_getcellp(mapdata, x+1, y+1, cell))
e += add_path(&g_open_set, x+1, y+1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x+1, y+1, dest->x, dest->y)); // (x+1, y+1) 7
e += add_path(g_open_set, x+1, y+1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x+1, y+1, dest->x, dest->y)); // (x+1, y+1) 7
if (chk_dir(PATH_DIR_NORTH))
e += add_path(&g_open_set, x, y+1, g_cost + MOVE_COST, current, heuristic(x, y+1, dest->x, dest->y)); // (x, y+1) 0
e += add_path(g_open_set, x, y+1, g_cost + MOVE_COST, current, heuristic(x, y+1, dest->x, dest->y)); // (x, y+1) 0
if (chk_dir(PATH_DIR_NORTH|PATH_DIR_WEST) && !map_getcellp(mapdata, x-1, y+1, cell))
e += add_path(&g_open_set, x-1, y+1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x-1, y+1, dest->x, dest->y)); // (x-1, y+1) 1
e += add_path(g_open_set, x-1, y+1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x-1, y+1, dest->x, dest->y)); // (x-1, y+1) 1
if (chk_dir(PATH_DIR_WEST))
e += add_path(&g_open_set, x-1, y, g_cost + MOVE_COST, current, heuristic(x-1, y, dest->x, dest->y)); // (x-1, y) 2
e += add_path(g_open_set, x-1, y, g_cost + MOVE_COST, current, heuristic(x-1, y, dest->x, dest->y)); // (x-1, y) 2
if (chk_dir(PATH_DIR_SOUTH|PATH_DIR_WEST) && !map_getcellp(mapdata, x-1, y-1, cell))
e += add_path(&g_open_set, x-1, y-1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x-1, y-1, dest->x, dest->y)); // (x-1, y-1) 3
e += add_path(g_open_set, x-1, y-1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x-1, y-1, dest->x, dest->y)); // (x-1, y-1) 3
if (chk_dir(PATH_DIR_SOUTH))
e += add_path(&g_open_set, x, y-1, g_cost + MOVE_COST, current, heuristic(x, y-1, dest->x, dest->y)); // (x, y-1) 4
e += add_path(g_open_set, x, y-1, g_cost + MOVE_COST, current, heuristic(x, y-1, dest->x, dest->y)); // (x, y-1) 4
#undef chk_dir
if (e) {
return false;
Expand Down Expand Up @@ -631,7 +614,6 @@ void write_map_distances() {


void navi_create_lists() {
BHEAP_INIT(g_open_set);

auto starttime = std::chrono::system_clock::now();

Expand All @@ -649,7 +631,6 @@ void navi_create_lists() {
currenttime = std::chrono::system_clock::now();
ShowInfo("Link Distances took %ums\n", std::chrono::duration_cast<std::chrono::milliseconds>(currenttime - starttime));

BHEAP_CLEAR(g_open_set);
}

#endif
96 changes: 38 additions & 58 deletions src/map/path.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,13 +40,7 @@ struct path_node {
};

/// Binary heap of path nodes
BHEAP_STRUCT_DECL(node_heap, struct path_node*);
static BHEAP_STRUCT_VAR(node_heap, g_open_set); // use static heap for all path calculations
// it get's initialized in do_init_path, freed in do_final_path.


/// Comparator for binary heap of path nodes (minimum cost at top)
#define NODE_MINTOPCMP(i,j) ((i)->f_cost - (j)->f_cost)
static std::vector<path_node*> g_open_set; // use static heap for all path calculations

#define calc_index(x,y) (((x)+(y)*MAX_WALKPATH) & (MAX_WALKPATH*MAX_WALKPATH-1))

Expand All @@ -65,11 +59,9 @@ static enum directions walk_choices [3][3] =


void do_init_path(){
BHEAP_INIT(g_open_set); // [fwi]: BHEAP_STRUCT_VAR already initialized the heap, this is rudendant & just for code-conformance/readability
}//

void do_final_path(){
BHEAP_CLEAR(g_open_set);
}//


Expand Down Expand Up @@ -195,33 +187,9 @@ bool path_search_long(struct shootpath_data *spd,int16 m,int16 x0,int16 y0,int16
/// @name A* pathfinding related functions
/// @{

/// Pushes path_node to the binary node_heap.
/// Ensures there is enough space in array to store new element.

static void heap_push_node(struct node_heap *heap, struct path_node *node)
{
#ifndef __clang_analyzer__ // TODO: Figure out why clang's static analyzer doesn't like this
BHEAP_ENSURE2(*heap, 1, 256, struct path_node **);
BHEAP_PUSH2(*heap, node, NODE_MINTOPCMP);
#endif // __clang_analyzer__
}

/// Updates path_node in the binary node_heap.
static int heap_update_node(struct node_heap *heap, struct path_node *node)
{
int i;
ARR_FIND(0, BHEAP_LENGTH(*heap), i, BHEAP_DATA(*heap)[i] == node);
if (i == BHEAP_LENGTH(*heap)) {
ShowError("heap_update_node: node not found\n");
return 1;
}
BHEAP_UPDATE(*heap, i, NODE_MINTOPCMP);
return 0;
}

/// Path_node processing in A* pathfinding.
/// Adds new node to heap and updates/re-adds old ones if necessary.
static int add_path(struct node_heap *heap, struct path_node *tp, int16 x, int16 y, int g_cost, struct path_node *parent, int h_cost)
static int add_path(std::vector<path_node*>& heap, std::vector<path_node>& tp, int16 x, int16 y, int g_cost, struct path_node* parent, int h_cost)
{
int i = calc_index(x, y);

Expand All @@ -232,9 +200,15 @@ static int add_path(struct node_heap *heap, struct path_node *tp, int16 x, int16
tp[i].parent = parent;
tp[i].f_cost = g_cost + h_cost;
if (tp[i].flag == SET_CLOSED) {
heap_push_node(heap, &tp[i]); // Put it in open set again
heap.push_back(&tp[i]); // Put it in open set again
std::push_heap(heap.begin(), heap.end(), [](const path_node* a, const path_node* b) {
return a->f_cost > b->f_cost;
});
}
else if (heap_update_node(heap, &tp[i])) {
else if (std::find(heap.begin(), heap.end(), &tp[i]) != heap.end()) {
std::make_heap(heap.begin(), heap.end(), [](const path_node* a, const path_node* b) {
return a->f_cost > b->f_cost;
});
return 1;
}
tp[i].flag = SET_OPEN;
Expand All @@ -252,7 +226,10 @@ static int add_path(struct node_heap *heap, struct path_node *tp, int16 x, int16
tp[i].parent = parent;
tp[i].f_cost = g_cost + h_cost;
tp[i].flag = SET_OPEN;
heap_push_node(heap, &tp[i]);
heap.push_back(&tp[i]);
std::push_heap(heap.begin(), heap.end(), [](const path_node* a, const path_node* b) {
return a->f_cost > b->f_cost;
});
return 0;
}
///@}
Expand Down Expand Up @@ -327,7 +304,7 @@ bool path_search(struct walkpath_data *wpd, int16 m, int16 x0, int16 y0, int16 x
// FIXME: This array is too small to ensure all paths shorter than MAX_WALKPATH
// can be found without node collision: calc_index(node1) = calc_index(node2).
// Figure out more proper size or another way to keep track of known nodes.
struct path_node tp[MAX_WALKPATH * MAX_WALKPATH];
std::vector<path_node> tp(MAX_WALKPATH * MAX_WALKPATH);
struct path_node *current, *it;
int xs = mapdata->xs - 1;
int ys = mapdata->ys - 1;
Expand All @@ -337,9 +314,7 @@ bool path_search(struct walkpath_data *wpd, int16 m, int16 x0, int16 y0, int16 x
// A* (A-star) pathfinding
// We always use A* for finding walkpaths because it is what game client uses.
// Easy pathfinding cuts corners of non-walkable cells, but client always walks around it.
BHEAP_RESET(g_open_set);

memset(tp, 0, sizeof(tp));
g_open_set.clear();

// Start node
i = calc_index(x0, y0);
Expand All @@ -350,7 +325,8 @@ bool path_search(struct walkpath_data *wpd, int16 m, int16 x0, int16 y0, int16 x
tp[i].f_cost = heuristic(x0, y0, x1, y1);
tp[i].flag = SET_OPEN;

heap_push_node(&g_open_set, &tp[i]); // Put start node to 'open' set
g_open_set.push_back(&tp[i]);
current = g_open_set.front();

for(;;) {
int e = 0; // error flag
Expand All @@ -365,12 +341,16 @@ bool path_search(struct walkpath_data *wpd, int16 m, int16 x0, int16 y0, int16 x

int g_cost;

if (BHEAP_LENGTH(g_open_set) == 0) {
if (g_open_set.size() == 0)
return false;
}

current = BHEAP_PEEK(g_open_set); // Look for the lowest f_cost node in the 'open' set
BHEAP_POP2(g_open_set, NODE_MINTOPCMP); // Remove it from 'open' set
// Look for the lowest f_cost node in the 'open' set
auto it = std::min_element(g_open_set.begin(), g_open_set.end(), [](const path_node* a, const path_node* b) {
return a->f_cost < b->f_cost;
});

current = *it;
g_open_set.erase(it); // Remove it from 'open' set

x = current->x;
y = current->y;
Expand All @@ -389,22 +369,22 @@ bool path_search(struct walkpath_data *wpd, int16 m, int16 x0, int16 y0, int16 x

#define chk_dir(d) ((allowed_dirs & (d)) == (d))
// Process neighbors of current node
if (chk_dir(PATH_DIR_SOUTH|PATH_DIR_EAST) && !map_getcellp(mapdata, x+1, y-1, cell))
e += add_path(&g_open_set, tp, x+1, y-1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x+1, y-1, x1, y1)); // (x+1, y-1) 5
if (chk_dir(PATH_DIR_SOUTH | PATH_DIR_EAST) && !map_getcellp(mapdata, x + 1, y - 1, cell))
e += add_path(g_open_set, tp, x + 1, y - 1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x + 1, y - 1, x1, y1)); // (x+1, y-1) 5
if (chk_dir(PATH_DIR_EAST))
e += add_path(&g_open_set, tp, x+1, y, g_cost + MOVE_COST, current, heuristic(x+1, y, x1, y1)); // (x+1, y) 6
if (chk_dir(PATH_DIR_NORTH|PATH_DIR_EAST) && !map_getcellp(mapdata, x+1, y+1, cell))
e += add_path(&g_open_set, tp, x+1, y+1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x+1, y+1, x1, y1)); // (x+1, y+1) 7
e += add_path(g_open_set, tp, x + 1, y, g_cost + MOVE_COST, current, heuristic(x + 1, y, x1, y1)); // (x+1, y) 6
if (chk_dir(PATH_DIR_NORTH | PATH_DIR_EAST) && !map_getcellp(mapdata, x + 1, y + 1, cell))
e += add_path(g_open_set, tp, x + 1, y + 1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x + 1, y + 1, x1, y1)); // (x+1, y+1) 7
if (chk_dir(PATH_DIR_NORTH))
e += add_path(&g_open_set, tp, x, y+1, g_cost + MOVE_COST, current, heuristic(x, y+1, x1, y1)); // (x, y+1) 0
if (chk_dir(PATH_DIR_NORTH|PATH_DIR_WEST) && !map_getcellp(mapdata, x-1, y+1, cell))
e += add_path(&g_open_set, tp, x-1, y+1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x-1, y+1, x1, y1)); // (x-1, y+1) 1
e += add_path(g_open_set, tp, x, y + 1, g_cost + MOVE_COST, current, heuristic(x, y + 1, x1, y1)); // (x, y+1) 0
if (chk_dir(PATH_DIR_NORTH | PATH_DIR_WEST) && !map_getcellp(mapdata, x - 1, y + 1, cell))
e += add_path(g_open_set, tp, x - 1, y + 1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x - 1, y + 1, x1, y1)); // (x-1, y+1) 1
if (chk_dir(PATH_DIR_WEST))
e += add_path(&g_open_set, tp, x-1, y, g_cost + MOVE_COST, current, heuristic(x-1, y, x1, y1)); // (x-1, y) 2
if (chk_dir(PATH_DIR_SOUTH|PATH_DIR_WEST) && !map_getcellp(mapdata, x-1, y-1, cell))
e += add_path(&g_open_set, tp, x-1, y-1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x-1, y-1, x1, y1)); // (x-1, y-1) 3
e += add_path(g_open_set, tp, x - 1, y, g_cost + MOVE_COST, current, heuristic(x - 1, y, x1, y1)); // (x-1, y) 2
if (chk_dir(PATH_DIR_SOUTH | PATH_DIR_WEST) && !map_getcellp(mapdata, x - 1, y - 1, cell))
e += add_path(g_open_set, tp, x - 1, y - 1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x - 1, y - 1, x1, y1)); // (x-1, y-1) 3
if (chk_dir(PATH_DIR_SOUTH))
e += add_path(&g_open_set, tp, x, y-1, g_cost + MOVE_COST, current, heuristic(x, y-1, x1, y1)); // (x, y-1) 4
e += add_path(g_open_set, tp, x, y - 1, g_cost + MOVE_COST, current, heuristic(x, y - 1, x1, y1)); // (x, y-1) 4
#undef chk_dir
if (e) {
return false;
Expand Down
Loading