Pathfinding now works exactly as on the client

- This should solve most of the position lag problems, especially ones that caused monsters to suddenly appear next to you (happened especially often when using icewall)
- Added a new heap implementation to db.h (POP2, PUSH2, SIFTUP, SIFTDOWN, UPDATE) that simulates the heap that the client uses, there was no other way to reproduce 100% exact behavior
- I recommend using the old heap implementation for everything else
- Updated path.c to use the new heap macros and also fixed the order in which the different possible directions are pushed into heap
- Special thanks to rversteegen for helping me with various tests and the heap implementation
- Special thanks to ultramage for providing info that helped us narrowing down the possible variables

Mega thanks to Playtester (rathena:c009b3f4a)

2 files changed, 119 insertions, 23 deletions

diff --git a/src/common/db.h b/src/common/db.h
index bf59e37d6..f807188eb 100644
--- a/src/common/db.h
+++ b/src/common/db.h
@@ -1347,6 +1347,7 @@ void  linkdb_foreach (struct linkdb_node** head, LinkDBFunc func, ...);
 /////////////////////////////////////////////////////////////////////
 // Binary heap library based on defines. (uses the vector defines above)
 // uses aMalloc, aRealloc, aFree
+// WARNING: BHEAP implementation details affect behaviour of A* pathfinding
 
 
 
@@ -1459,6 +1460,21 @@ void  linkdb_foreach (struct linkdb_node** head, LinkDBFunc func, ...);
 
 
 
+/// See BHEAP_PUSH. Version used by A* implementation, matching client bheap.
+///
+/// @param __heap Binary heap
+/// @param __val Value
+/// @param __topcmp Comparator
+/// @param __swp Swapper
+#define BHEAP_PUSH2(__heap,__val,__topcmp,__swp) \
+	do{ \
+		size_t _i_ = VECTOR_LENGTH(__heap); \
+		VECTOR_PUSH(__heap,__val); /* insert at end */ \
+		BHEAP_SIFTDOWN(__heap,0,_i_,__topcmp,__swp); \
+	}while(0)
+
+
+
 /// Removes the top value of the heap. (using the '=' operator)
 /// Assumes the heap is not empty.
 ///
@@ -1474,6 +1490,21 @@ void  linkdb_foreach (struct linkdb_node** head, LinkDBFunc func, ...);
 
 
 
+/// See BHEAP_POP. Version used by A* implementation, matching client bheap.
+///
+/// @param __heap Binary heap
+/// @param __topcmp Comparator
+/// @param __swp Swapper
+#define BHEAP_POP2(__heap,__topcmp,__swp) \
+	do{ \
+		VECTOR_INDEX(__heap,0) = VECTOR_POP(__heap); /* put last at index */ \
+		if( !VECTOR_LENGTH(__heap) ) /* removed last, nothing to do */ \
+			break; \
+		BHEAP_SIFTUP(__heap,0,__topcmp,__swp);	\
+	}while(0)
+
+
+
 /// Removes the target value of the heap. (using the '=' operator)
 /// Assumes the index exists.
 ///
@@ -1522,6 +1553,74 @@ void  linkdb_foreach (struct linkdb_node** head, LinkDBFunc func, ...);
 
 
 
+/// Follow path up towards (but not all the way to) the root, swapping nodes until finding
+/// a place where the new item that was placed at __idx fits.
+/// Only goes as high as __startidx (usually 0).
+///
+/// @param __heap Binary heap
+/// @param __startidx Index of an ancestor of __idx
+/// @param __idx Index of an inserted element
+/// @param __topcmp Comparator
+/// @param __swp Swapper
+#define BHEAP_SIFTDOWN(__heap,__startidx,__idx,__topcmp,__swp)	\
+	do{ \
+		size_t _i2_ = __idx; \
+		while( _i2_ > __startidx ) \
+		{ /* restore heap property in parents */ \
+			size_t _parent_ = (_i2_-1)/2; \
+			if( __topcmp(VECTOR_INDEX(__heap,_parent_),VECTOR_INDEX(__heap,_i2_)) <= 0 ) \
+				break; /* done */ \
+			__swp(VECTOR_INDEX(__heap,_parent_),VECTOR_INDEX(__heap,_i2_)); \
+			_i2_ = _parent_; \
+		} \
+	}while(0)
+
+
+
+/// Repeatedly swap the smaller child with parent, after placing a new item at __idx.
+///
+/// @param __heap Binary heap
+/// @param __idx Index of an inserted element
+/// @param __topcmp Comparator
+/// @param __swp Swapper
+#define BHEAP_SIFTUP(__heap,__idx,__topcmp,__swp) \
+	do{ \
+		size_t _i_ = __idx; \
+		size_t _lchild_ = _i_*2 + 1; \
+		while( _lchild_ < VECTOR_LENGTH(__heap) ) \
+		{ /* restore heap property in childs */ \
+			size_t _rchild_ = _i_*2 + 2; \
+			if( _rchild_ >= VECTOR_LENGTH(__heap) || __topcmp(VECTOR_INDEX(__heap,_lchild_),VECTOR_INDEX(__heap,_rchild_)) < 0 ) \
+			{ /* left child */ \
+				__swp(VECTOR_INDEX(__heap,_i_),VECTOR_INDEX(__heap,_lchild_)); \
+				_i_ = _lchild_; \
+			} \
+			else \
+			{ /* right child */ \
+				__swp(VECTOR_INDEX(__heap,_i_),VECTOR_INDEX(__heap,_rchild_)); \
+				_i_ = _rchild_; \
+			} \
+			_lchild_ = _i_*2 + 1; \
+		} \
+		BHEAP_SIFTDOWN(__heap,__idx,_i_,__topcmp,__swp); \
+	}while(0)
+
+
+
+/// Call this after modifying the item at __idx__ to restore the heap
+///
+/// @param __heap Binary heap
+/// @param __idx Index
+/// @param __topcmp Comparator
+/// @param __swp Swapper
+#define BHEAP_UPDATE(__heap,__idx,__topcmp,__swp) \
+	do{ \
+		BHEAP_SIFTDOWN(__heap,0,__idx,__topcmp,__swp); \
+		BHEAP_SIFTUP(__heap,__idx,__topcmp,__swp); \
+	}while(0)
+
+
+
 /// Clears the binary heap, freeing allocated data.
 ///
 /// @param __heap Binary heap
diff --git a/src/map/path.c b/src/map/path.c
index 5a9ddf9c7..086b0af9a 100644
--- a/src/map/path.c
+++ b/src/map/path.c
@@ -176,7 +176,7 @@ 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_ENSURE(*heap, 1, 256);
-	BHEAP_PUSH(*heap, node, NODE_MINTOPCMP, swap_ptr);
+	BHEAP_PUSH2(*heap, node, NODE_MINTOPCMP, swap_ptr);
 #endif // __clang_analyzer__
 }
 
@@ -189,8 +189,7 @@ static int heap_update_node(struct node_heap *heap, struct path_node *node)
 		ShowError("heap_update_node: node not found\n");
 		return 1;
 	}
-	BHEAP_POPINDEX(*heap, i, NODE_MINTOPCMP, swap_ptr);
-	BHEAP_PUSH(*heap, node, NODE_MINTOPCMP, swap_ptr);
+	BHEAP_UPDATE(*heap, i, NODE_MINTOPCMP, swap_ptr);
 	return 0;
 }
 
@@ -304,7 +303,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_STRUCT_VAR(node_heap, open_set); // 'Open' set
 
 		// FIXME: This array is too small to ensure all paths shorter than MAX_WALKPATH
@@ -327,8 +326,8 @@ bool path_search(struct walkpath_data *wpd, int16 m, int16 x0, int16 y0, int16 x
 		tp[i].flag   = SET_OPEN;
 
 		heap_push_node(&open_set, &tp[i]); // Put start node to 'open' set
-		for(;;)
-		{
+
+		for(;;) {
 			int e = 0; // error flag
 
 			// Saves allowed directions for the current cell. Diagonal directions
@@ -347,7 +346,7 @@ bool path_search(struct walkpath_data *wpd, int16 m, int16 x0, int16 y0, int16 x
 			}
 
 			current = BHEAP_PEEK(open_set); // Look for the lowest f_cost node in the 'open' set
-			BHEAP_POP(open_set, NODE_MINTOPCMP, swap_ptr); // Remove it from 'open' set
+			BHEAP_POP2(open_set, NODE_MINTOPCMP, swap_ptr); // Remove it from 'open' set
 
 			x      = current->x;
 			y      = current->y;
@@ -367,24 +366,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
-			// TODO: Processing order affects chosen path if there is more than one path with same cost.
-			// In few cases path found by server will be different than path found by game client.
-			if (chk_dir(DIR_SOUTH))
-				e += add_path(&open_set, tp, x, y-1, g_cost + MOVE_COST, current, heuristic(x, y-1, x1, y1)); // (x, y-1) 4
-			if (chk_dir(DIR_SOUTH|DIR_WEST) && !md->getcellp(md, x-1, y-1, cell))
-				e += add_path(&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(DIR_WEST))
-				e += add_path(&open_set, tp, x-1, y, g_cost + MOVE_COST, current, heuristic(x-1, y, x1, y1)); // (x-1, y) 2
-			if (chk_dir(DIR_NORTH|DIR_WEST) && !md->getcellp(md, x-1, y+1, cell))
-				e += add_path(&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(DIR_NORTH))
-				e += add_path(&open_set, tp, x, y+1, g_cost + MOVE_COST, current, heuristic(x, y+1, x1, y1)); // (x, y+1) 0
-			if (chk_dir(DIR_NORTH|DIR_EAST) && !md->getcellp(md, x+1, y+1, cell))
-				e += add_path(&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(DIR_EAST))
-				e += add_path(&open_set, tp, x+1, y, g_cost + MOVE_COST, current, heuristic(x+1, y, x1, y1)); // (x+1, y) 6
 			if (chk_dir(DIR_SOUTH|DIR_EAST) && !md->getcellp(md, x+1, y-1, cell))
 				e += add_path(&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(DIR_EAST))
+				e += add_path(&open_set, tp, x+1, y, g_cost + MOVE_COST, current, heuristic(x+1, y, x1, y1)); // (x+1, y) 6
+			if (chk_dir(DIR_NORTH|DIR_EAST) && !md->getcellp(md, x+1, y+1, cell))
+				e += add_path(&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(DIR_NORTH))
+				e += add_path(&open_set, tp, x, y+1, g_cost + MOVE_COST, current, heuristic(x, y+1, x1, y1)); // (x, y+1) 0
+			if (chk_dir(DIR_NORTH|DIR_WEST) && !md->getcellp(md, x-1, y+1, cell))
+				e += add_path(&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(DIR_WEST))
+				e += add_path(&open_set, tp, x-1, y, g_cost + MOVE_COST, current, heuristic(x-1, y, x1, y1)); // (x-1, y) 2
+			if (chk_dir(DIR_SOUTH|DIR_WEST) && !md->getcellp(md, x-1, y-1, cell))
+				e += add_path(&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(DIR_SOUTH))
+				e += add_path(&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) {
 				BHEAP_CLEAR(open_set);