Merge pull request #65 from piotrhalaczkiewicz/pathfinding

Pathfinding code cleanup.

1 files changed, 242 insertions, 244 deletions

diff --git a/src/map/path.c b/src/map/path.c
index 95895cb2a..32a4189bb 100644
--- a/src/map/path.c
+++ b/src/map/path.c
@@ -2,25 +2,55 @@
 // For more information, see LICENCE in the main folder
 
 #include "../common/cbasetypes.h"
+#include "../common/db.h"
+#include "../common/malloc.h"
 #include "../common/nullpo.h"
 #include "../common/random.h"
 #include "../common/showmsg.h"
-#include "../common/malloc.h"
-#include "map.h"
-#include "battle.h"
+
 #include "path.h"
+#include "map.h"
 
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 
+#define SET_OPEN 0
+#define SET_CLOSED 1
+
+#define DIR_NORTH 1
+#define DIR_WEST 2
+#define DIR_SOUTH 4
+#define DIR_EAST 8
+
+/// @name Structures and defines for A* pathfinding
+/// @{
+
+/// Path node
+struct path_node {
+	struct path_node *parent; ///< pointer to parent (for path reconstruction)
+	short x; ///< X-coordinate
+	short y; ///< Y-coordinate
+	short g_cost; ///< Actual cost from start to this node
+	short f_cost; ///< g_cost + heuristic(this, goal)
+	short flag; ///< SET_OPEN / SET_CLOSED
+};
+
+/// Binary heap of path nodes
+BHEAP_STRUCT_DECL(node_heap, struct path_node*);
 
-#define MAX_HEAP 150
+/// Comparator for binary heap of path nodes (minimum cost at top)
+#define NODE_MINTOPCMP(i,j) ((i)->f_cost - (j)->f_cost)
 
-struct tmp_path { short x,y,dist,before,cost,flag;};
 #define calc_index(x,y) (((x)+(y)*MAX_WALKPATH) & (MAX_WALKPATH*MAX_WALKPATH-1))
 
-const char walk_choices [3][3] =
+/// Estimates the cost from (x0,y0) to (x1,y1).
+/// This is inadmissible (overestimating) heuristic used by game client.
+#define heuristic(x0, y0, x1, y1)	(MOVE_COST * (abs((x1) - (x0)) + abs((y1) - (y0)))) // Manhattan distance
+/// @}
+
+// Translates dx,dy into walking direction
+static const unsigned char walk_choices [3][3] =
 {
 	{1,0,7},
 	{2,-1,6},
@@ -28,124 +58,6 @@ const char walk_choices [3][3] =
 };
 
 /*==========================================
- * heap push (helper function)
- *------------------------------------------*/
-static void push_heap_path(int *heap,struct tmp_path *tp,int index)
-{
-	int i,h;
-
-	h = heap[0];
-	heap[0]++;
-
-	for( i = (h-1)/2; h > 0 && tp[index].cost < tp[heap[i+1]].cost; i = (h-1)/2 )
-		heap[h+1] = heap[i+1], h = i;
-
-	heap[h+1] = index;
-}
-
-/*==========================================
- * heap update (helper function)
- * Move toward the root because cost has decreased.
- *------------------------------------------*/
-static void update_heap_path(int *heap,struct tmp_path *tp,int index)
-{
-	int i,h;
-
-	ARR_FIND( 0, heap[0], h, heap[h+1] == index );
-	if( h == heap[0] )
-	{
-		ShowError("update_heap_path bug\n");
-		exit(EXIT_FAILURE);
-	}
-
-	for( i = (h-1)/2; h > 0 && tp[index].cost < tp[heap[i+1]].cost; i = (h-1)/2 )
-		heap[h+1] = heap[i+1], h = i;
-
-	heap[h+1] = index;
-}
-
-/*==========================================
- * heap pop (helper function)
- *------------------------------------------*/
-static int pop_heap_path(int *heap,struct tmp_path *tp)
-{
-	int i,h,k;
-	int ret,last;
-
-	if( heap[0] <= 0 )
-		return -1;
-	ret = heap[1];
-	last = heap[heap[0]];
-	heap[0]--;
-
-	for( h = 0, k = 2; k < heap[0]; k = k*2+2 )
-	{
-		if( tp[heap[k+1]].cost > tp[heap[k]].cost )
-			k--;
-		heap[h+1] = heap[k+1], h = k;
-	}
-
-	if( k == heap[0] )
-		heap[h+1] = heap[k], h = k-1;
-
-	for( i = (h-1)/2; h > 0 && tp[heap[i+1]].cost > tp[last].cost; i = (h-1)/2 )
-		heap[h+1] = heap[i+1], h = i;
-
-	heap[h+1]=last;
-
-	return ret;
-}
-
-/*==========================================
- * calculate cost for the specified position
- *------------------------------------------*/
-static int calc_cost(struct tmp_path *p,int16 x1,int16 y1)
-{
-	int xd = abs(x1 - p->x);
-	int yd = abs(y1 - p->y);
-	return (xd + yd)*10 + p->dist;
-}
-
-/*==========================================
- * attach/adjust path if neccessary
- *------------------------------------------*/
-static int add_path(int *heap,struct tmp_path *tp,int16 x,int16 y,int dist,int before,int cost)
-{
-	int i;
-
-	i = calc_index(x,y);
-
-	if( tp[i].x == x && tp[i].y == y )
-	{
-		if( tp[i].dist > dist )
-		{
-			tp[i].dist = dist;
-			tp[i].before = before;
-			tp[i].cost = cost;
-			if( tp[i].flag )
-				push_heap_path(heap,tp,i);
-			else
-				update_heap_path(heap,tp,i);
-			tp[i].flag = 0;
-		}
-		return 0;
-	}
-
-	if( tp[i].x || tp[i].y )
-		return 1;
-
-	tp[i].x = x;
-	tp[i].y = y;
-	tp[i].dist = dist;
-	tp[i].before = before;
-	tp[i].cost = cost;
-	tp[i].flag = 0;
-	push_heap_path(heap,tp,i);
-
-	return 0;
-}
-
-/*==========================================
  * Find the closest reachable cell, 'count' cells away from (x0,y0) in direction (dx,dy).
  * Income after the coordinates of the blow
  *------------------------------------------*/
@@ -262,164 +174,250 @@ bool path_search_long(struct shootpath_data *spd,int16 m,int16 x0,int16 y0,int16
 	return true;
 }
 
+/// @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)
+{
+	BHEAP_ENSURE(*heap, 1, 256);
+	BHEAP_PUSH(*heap, node, NODE_MINTOPCMP, swap_ptr);
+}
+
+/// 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_POPINDEX(*heap, i, NODE_MINTOPCMP, swap_ptr);
+	BHEAP_PUSH(*heap, node, NODE_MINTOPCMP, swap_ptr);
+	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)
+{
+	int i = calc_index(x, y);
+
+	if (tp[i].x == x && tp[i].y == y) { // We processed this node before
+		if (g_cost < tp[i].g_cost) { // New path to this node is better than old one
+			// Update costs and parent
+			tp[i].g_cost = g_cost;
+			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
+			}
+			else if (heap_update_node(heap, &tp[i])) {
+				return 1;
+			}
+			tp[i].flag = SET_OPEN;
+		}
+		return 0;
+	}
+
+	if (tp[i].x || tp[i].y) // Index is already taken; see `tp` array FIXME for details
+		return 1;
+
+	// New node
+	tp[i].x = x;
+	tp[i].y = y;
+	tp[i].g_cost = g_cost;
+	tp[i].parent = parent;
+	tp[i].f_cost = g_cost + h_cost;
+	tp[i].flag = SET_OPEN;
+	heap_push_node(heap, &tp[i]);
+	return 0;
+}
+///@}
+
 /*==========================================
  * path search (x0,y0)->(x1,y1)
  * wpd: path info will be written here
  * flag: &1 = easy path search only
  * cell: type of obstruction to check for
  *------------------------------------------*/
-bool path_search(struct walkpath_data *wpd,int16 m,int16 x0,int16 y0,int16 x1,int16 y1,int flag,cell_chk cell)
+bool path_search(struct walkpath_data *wpd, int16 m, int16 x0, int16 y0, int16 x1, int16 y1, int flag, cell_chk cell)
 {
-	int heap[MAX_HEAP+1];
-	struct tmp_path tp[MAX_WALKPATH*MAX_WALKPATH];
-	register int i,j,len,x,y,dx,dy;
-	int rp,xs,ys;
+	register int i, j, x, y, dx, dy;
 	struct map_data *md;
 	struct walkpath_data s_wpd;
 
-	if( wpd == NULL )
+	if (wpd == NULL)
 		wpd = &s_wpd; // use dummy output variable
 
-	if( !map[m].cell )
+	if (!map[m].cell)
 		return false;
 	md = &map[m];
 
 #ifdef CELL_NOSTACK
 	//Do not check starting cell as that would get you stuck.
-	if( x0 < 0 || x0 >= md->xs || y0 < 0 || y0 >= md->ys )
+	if (x0 < 0 || x0 >= md->xs || y0 < 0 || y0 >= md->ys)
 #else
-	if( x0 < 0 || x0 >= md->xs || y0 < 0 || y0 >= md->ys /*|| md->getcellp(md,x0,y0,cell)*/ )
+	if (x0 < 0 || x0 >= md->xs || y0 < 0 || y0 >= md->ys /*|| md->getcellp(md,x0,y0,cell)*/)
 #endif
 		return false;
-	if( x1 < 0 || x1 >= md->xs || y1 < 0 || y1 >= md->ys || md->getcellp(md,x1,y1,cell) )
+
+	// Check destination cell
+	if (x1 < 0 || x1 >= md->xs || y1 < 0 || y1 >= md->ys || md->getcellp(md,x1,y1,cell))
 		return false;
 
-	// calculate (sgn(x1-x0), sgn(y1-y0))
-	dx = ((dx = x1-x0)) ? ((dx<0) ? -1 : 1) : 0;
-	dy = ((dy = y1-y0)) ? ((dy<0) ? -1 : 1) : 0;
+	if (flag&1) {
+		// Try finding direct path to target
+		// Direct path goes diagonally first, then in straight line.
 
-	// try finding direct path to target
-	x = x0;
-	y = y0;
-	i = 0;
-	while( i < ARRAYLENGTH(wpd->path) )
-	{
-		wpd->path[i] = walk_choices[-dy + 1][dx + 1];
-		i++;
+		// calculate (sgn(x1-x0), sgn(y1-y0))
+		dx = ((dx = x1-x0)) ? ((dx<0) ? -1 : 1) : 0;
+		dy = ((dy = y1-y0)) ? ((dy<0) ? -1 : 1) : 0;
 
-		x += dx;
-		y += dy;
+		x = x0; // Current position = starting cell
+		y = y0;
+		i = 0;
+		while( i < ARRAYLENGTH(wpd->path) )
+		{
+			wpd->path[i] = walk_choices[-dy + 1][dx + 1];
+			i++;
 
-		if( x == x1 ) dx = 0;
-		if( y == y1 ) dy = 0;
+			x += dx; // Advance current position
+			y += dy;
 
-		if( dx == 0 && dy == 0 )
-			break; // success
-		if( md->getcellp(md,x,y,cell) )
-			break; // obstacle = failure
-	}
+			if( x == x1 ) dx = 0; // destination x reached, no longer move along x-axis
+			if( y == y1 ) dy = 0; // destination y reached, no longer move along y-axis
 
-	if( x == x1 && y == y1 )
-	{ //easy path successful.
-		wpd->path_len = i;
-		wpd->path_pos = 0;
-		return true;
+			if( dx == 0 && dy == 0 )
+				break; // success
+			if( md->getcellp(md,x,y,cell) )
+				break; // obstacle = failure
+		}
+
+		if( x == x1 && y == y1 )
+		{ // easy path successful.
+			wpd->path_len = i;
+			wpd->path_pos = 0;
+			return true;
+		}
+
+		return false; // easy path unsuccessful 
 	}
+	else { // !(flag&1)
+		// 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
+		// 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];
+		struct path_node *current, *it;
+		int xs = md->xs - 1;
+		int ys = md->ys - 1;
+		int len = 0;
+		memset(tp, 0, sizeof(tp));
+
+		// Start node
+		i = calc_index(x0, y0);
+		tp[i].parent = NULL;
+		tp[i].x      = x0;
+		tp[i].y      = y0;
+		tp[i].g_cost = 0;
+		tp[i].f_cost = heuristic(x0, y0, x1, y1);
+		tp[i].flag   = SET_OPEN;
+
+		heap_push_node(&open_set, &tp[i]); // Put start node to 'open' set
+		for(;;)
+		{
+			int e = 0; // error flag
 
-	if( flag&1 )
-		return false;
+			// Saves allowed directions for the current cell. Diagonal directions
+			// are only allowed if both directions around it are allowed. This is
+			// to prevent cutting corner of nearby wall.
+			// For example, you can only go NW from the current cell, if you can
+			// go N *and* you can go W. Otherwise you need to walk around the
+			// (corner of the) non-walkable cell.
+			int allowed_dirs = 0;
 
-	memset(tp,0,sizeof(tp));
-
-	i=calc_index(x0,y0);
-	tp[i].x=x0;
-	tp[i].y=y0;
-	tp[i].dist=0;
-	tp[i].before=0;
-	tp[i].cost=calc_cost(&tp[i],x1,y1);
-	tp[i].flag=0;
-	heap[0]=0;
-	push_heap_path(heap,tp,calc_index(x0,y0));
-    xs = md->xs - 1; // Place by subtracting a pre-
-	ys = md->ys-1;
-
-	for(;;)
-	{
-		int e=0,f=0,dist,cost,dc[4]={0,0,0,0};
+			int g_cost;
 
-		if(heap[0]==0)
-			return false;
-		rp   = pop_heap_path(heap,tp);
-		x    = tp[rp].x;
-		y    = tp[rp].y;
-		dist = tp[rp].dist + 10;
-		cost = tp[rp].cost;
-
-		if(x==x1 && y==y1)
-			break;
-
-		// dc[0] : y++ Incremental cost at the time
-		// dc[1] : x--
-		// dc[2] : y--
-		// dc[3] : x++
-
-		if(y < ys && !md->getcellp(md,x  ,y+1,cell)) {
-			f |= 1; dc[0] = (y >= y1 ? 20 : 0);
-			e+=add_path(heap,tp,x  ,y+1,dist,rp,cost+dc[0]); // (x,   y+1)
-		}
-		if(x > 0  && !md->getcellp(md,x-1,y  ,cell)) {
-			f |= 2; dc[1] = (x <= x1 ? 20 : 0);
-			e+=add_path(heap,tp,x-1,y  ,dist,rp,cost+dc[1]); // (x-1, y  )
-		}
-		if(y > 0  && !md->getcellp(md,x  ,y-1,cell)) {
-			f |= 4; dc[2] = (y <= y1 ? 20 : 0);
-			e+=add_path(heap,tp,x  ,y-1,dist,rp,cost+dc[2]); // (x  , y-1)
-		}
-		if(x < xs && !md->getcellp(md,x+1,y  ,cell)) {
-			f |= 8; dc[3] = (x >= x1 ? 20 : 0);
-			e+=add_path(heap,tp,x+1,y  ,dist,rp,cost+dc[3]); // (x+1, y  )
-		}
-		if( (f & (2+1)) == (2+1) && !md->getcellp(md,x-1,y+1,cell))
-			e+=add_path(heap,tp,x-1,y+1,dist+4,rp,cost+dc[1]+dc[0]-6);		// (x-1, y+1)
-		if( (f & (2+4)) == (2+4) && !md->getcellp(md,x-1,y-1,cell))
-			e+=add_path(heap,tp,x-1,y-1,dist+4,rp,cost+dc[1]+dc[2]-6);		// (x-1, y-1)
-		if( (f & (8+4)) == (8+4) && !md->getcellp(md,x+1,y-1,cell))
-			e+=add_path(heap,tp,x+1,y-1,dist+4,rp,cost+dc[3]+dc[2]-6);		// (x+1, y-1)
-		if( (f & (8+1)) == (8+1) && !md->getcellp(md,x+1,y+1,cell))
-			e+=add_path(heap,tp,x+1,y+1,dist+4,rp,cost+dc[3]+dc[0]-6);		// (x+1, y+1)
-		tp[rp].flag=1;
-		if(e || heap[0]>=MAX_HEAP-5)
-			return false;
-	}
+			if (BHEAP_LENGTH(open_set) == 0) {
+				BHEAP_CLEAR(open_set);
+				return false;
+			}
 
-	if( !(x==x1 && y==y1) ) // will never happen...
-		return false;
+			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
 
-	for(len=0,i=rp;len<100 && i!=calc_index(x0,y0);i=tp[i].before,len++);
-	if(len==100 || len>=sizeof(wpd->path))
-		return false;
+			x      = current->x;
+			y      = current->y;
+			g_cost = current->g_cost;
+
+			current->flag = SET_CLOSED; // Add current node to 'closed' set
 
-	wpd->path_len = len;
-	wpd->path_pos = 0;
-	for(i=rp,j=len-1;j>=0;i=tp[i].before,j--) {
-		int dx  = tp[i].x - tp[tp[i].before].x;
-		int dy  = tp[i].y - tp[tp[i].before].y;
-		uint8 dir;
-		if( dx == 0 ) {
-			dir = (dy > 0 ? 0 : 4);
-		} else if( dx > 0 ) {
-			dir = (dy == 0 ? 6 : (dy < 0 ? 5 : 7) );
-		} else {
-			dir = (dy == 0 ? 2 : (dy > 0 ? 1 : 3) );
+			if (x == x1 && y == y1) {
+				BHEAP_CLEAR(open_set);
+				break;
+			}
+
+			if (y < ys && !md->getcellp(md, x, y+1, cell)) allowed_dirs |= DIR_NORTH;
+			if (y >  0 && !md->getcellp(md, x, y-1, cell)) allowed_dirs |= DIR_SOUTH;
+			if (x < xs && !md->getcellp(md, x+1, y, cell)) allowed_dirs |= DIR_EAST;
+			if (x >  0 && !md->getcellp(md, x-1, y, cell)) allowed_dirs |= DIR_WEST;
+
+#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
+#undef chk_dir
+			if (e) {
+				BHEAP_CLEAR(open_set);
+				return false;
+			}
 		}
-		wpd->path[j] = dir;
-	}
 
-	return true;
+		for (it = current; it->parent != NULL; it = it->parent, len++);
+		if (len > sizeof(wpd->path)) {
+			return false;
+		}
+
+		// Recreate path
+		wpd->path_len = len;
+		wpd->path_pos = 0;
+		for (it = current, j = len-1; j >= 0; it = it->parent, j--) {
+			dx = it->x - it->parent->x;
+			dy = it->y - it->parent->y;
+			wpd->path[j] = walk_choices[-dy + 1][dx + 1];
+		}
+		return true;
+	} // A* end
+
+	return false;
 }
 
 
-//Distance functions, taken from http://www.flpcode.com/articles/article_fastdistance.shtml
+//Distance functions, taken from http://www.flipcode.com/articles/article_fastdistance.shtml
 int check_distance(int dx, int dy, int distance)
 {
 #ifdef CIRCULAR_AREA
@@ -439,7 +437,7 @@ unsigned int distance(int dx, int dy)
 
 	if ( dx < 0 ) dx = -dx;
 	if ( dy < 0 ) dy = -dy;
-	//There appears to be something wrong with the aproximation below when either dx/dy is 0! [Skotlex]
+	//There appears to be something wrong with the approximation below when either dx/dy is 0! [Skotlex]
 	if ( dx == 0 ) return dy;
 	if ( dy == 0 ) return dx;