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

src/common/db.h

@@ -1292,6 +1292,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
 
 
 
@@ -1404,6 +1405,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.
 ///
@@ -1418,6 +1434,22 @@ void  linkdb_foreach( struct linkdb_node** head, LinkDBFunc func, ...  );
 #define BHEAP_POP(__heap,__topcmp,__swp) BHEAP_POPINDEX(__heap,0,__topcmp,__swp)
 
 
+
+/// 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.
 ///
@@ -1466,6 +1498,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

src/map/path.c

@@ -190,7 +190,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__
 }
 
@@ -203,8 +203,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_UPDATE(*heap, i, NODE_MINTOPCMP, swap_ptr);
-	BHEAP_PUSH(*heap, node, NODE_MINTOPCMP, swap_ptr);
 	return 0;
 }
 
@@ -361,7 +360,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;
@@ -381,24 +380,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) && !map_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) && !map_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) && !map_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) && !map_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) && !map_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) && !map_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) && !map_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);