tron@2186: /* $Id$ */
tron@2186: 
truelight@0: #include "stdafx.h"
Darkvater@1891: #include "openttd.h"
tron@2163: #include "functions.h"
tron@679: #include "map.h"
tron@1209: #include "tile.h"
truelight@0: #include "pathfind.h"
ludde@2044: #include "rail.h"
ludde@2044: #include "debug.h"
tron@2153: #include "variables.h"
truelight@0: 
truelight@0: // remember which tiles we have already visited so we don't visit them again.
tron@1977: static bool TPFSetTileBit(TrackPathFinder *tpf, TileIndex tile, int dir)
truelight@0: {
truelight@0: 	uint hash, val, offs;
truelight@0: 	TrackPathFinderLink *link, *new_link;
truelight@0: 	uint bits = 1 << dir;
truelight@0: 
truelight@0: 	if (tpf->disable_tile_hash)
truelight@0: 		return true;
truelight@0: 
truelight@0: 	hash = PATHFIND_HASH_TILE(tile);
truelight@0: 
truelight@0: 	val = tpf->hash_head[hash];
truelight@0: 
truelight@0: 	if (val == 0) {
truelight@0: 		/* unused hash entry, set the appropriate bit in it and return true
truelight@0: 		 * to indicate that a bit was set. */
truelight@0: 		tpf->hash_head[hash] = bits;
tron@1986: 		tpf->hash_tile[hash] = tile;
truelight@0: 		return true;
truelight@0: 	} else if (!(val & 0x8000)) {
truelight@0: 		/* single tile */
truelight@0: 
tron@1986: 		if (tile == tpf->hash_tile[hash]) {
truelight@0: 			/* found another bit for the same tile,
truelight@0: 			 * check if this bit is already set, if so, return false */
truelight@0: 			if (val & bits)
truelight@0: 				return false;
truelight@0: 
truelight@0: 			/* otherwise set the bit and return true to indicate that the bit
truelight@0: 			 * was set */
truelight@0: 			tpf->hash_head[hash] = val | bits;
truelight@0: 			return true;
truelight@0: 		} else {
truelight@0: 			/* two tiles with the same hash, need to make a link */
truelight@193: 
truelight@0: 			/* allocate a link. if out of links, handle this by returning
truelight@0: 			 * that a tile was already visisted. */
ludde@2044: 			if (tpf->num_links_left == 0) {
truelight@0: 				return false;
ludde@2044: 			}
truelight@0: 			tpf->num_links_left--;
truelight@0: 			link = tpf->new_link++;
truelight@0: 
truelight@0: 			/* move the data that was previously in the hash_??? variables
truelight@0: 			 * to the link struct, and let the hash variables point to the link */
truelight@0: 			link->tile = tpf->hash_tile[hash];
truelight@0: 			tpf->hash_tile[hash] = PATHFIND_GET_LINK_OFFS(tpf, link);
truelight@0: 
truelight@0: 			link->flags = tpf->hash_head[hash];
truelight@193: 			tpf->hash_head[hash] = 0xFFFF; /* multi link */
truelight@0: 
truelight@0: 			link->next = 0xFFFF;
truelight@0: 		}
truelight@0: 	} else {
truelight@0: 		/* a linked list of many tiles,
truelight@0: 		 * find the one corresponding to the tile, if it exists.
truelight@0: 		 * otherwise make a new link */
truelight@193: 
truelight@0: 		offs = tpf->hash_tile[hash];
truelight@0: 		do {
truelight@0: 			link = PATHFIND_GET_LINK_PTR(tpf, offs);
tron@1986: 			if (tile == link->tile) {
truelight@0: 				/* found the tile in the link list,
truelight@0: 				 * check if the bit was alrady set, if so return false to indicate that the
truelight@0: 				 * bit was already set */
truelight@0: 				if (link->flags & bits)
truelight@0: 					return false;
truelight@0: 				link->flags |= bits;
truelight@0: 				return true;
truelight@0: 			}
truelight@0: 		} while ((offs=link->next) != 0xFFFF);
truelight@0: 	}
truelight@193: 
truelight@0: 	/* get here if we need to add a new link to link,
truelight@0: 	 * first, allocate a new link, in the same way as before */
ludde@2044: 	if (tpf->num_links_left == 0) {
truelight@0: 			return false;
ludde@2044: 	}
truelight@0: 	tpf->num_links_left--;
truelight@0: 	new_link = tpf->new_link++;
truelight@0: 
truelight@0: 	/* then fill the link with the new info, and establish a ptr from the old
truelight@0: 	 * link to the new one */
tron@1986: 	new_link->tile = tile;
truelight@0: 	new_link->flags = bits;
truelight@0: 	new_link->next = 0xFFFF;
truelight@0: 
truelight@0: 	link->next = PATHFIND_GET_LINK_OFFS(tpf, new_link);
truelight@0: 	return true;
truelight@0: }
truelight@0: 
truelight@0: static const byte _bits_mask[4] = {
truelight@0: 	0x19,
truelight@0: 	0x16,
truelight@0: 	0x25,
truelight@0: 	0x2A,
truelight@0: };
truelight@0: 
truelight@0: static const byte _tpf_new_direction[14] = {
truelight@0: 	0,1,0,1,2,1, 0,0,
truelight@0: 	2,3,3,2,3,0,
truelight@0: };
truelight@0: 
truelight@0: static const byte _tpf_prev_direction[14] = {
truelight@0: 	0,1,1,0,1,2, 0,0,
truelight@0: 	2,3,2,3,0,3,
truelight@0: };
truelight@0: 
truelight@0: 
truelight@0: static const byte _otherdir_mask[4] = {
truelight@0: 	0x10,
truelight@0: 	0,
truelight@0: 	0x5,
truelight@0: 	0x2A,
truelight@0: };
truelight@0: 
tron@1977: static void TPFMode2(TrackPathFinder *tpf, TileIndex tile, int direction)
truelight@0: {
truelight@0: 	uint bits;
truelight@0: 	int i;
truelight@0: 	RememberData rd;
truelight@753: 	int owner = -1;
truelight@747: 
matthijs@2006: 	/* XXX: Mode 2 is currently only used for ships, why is this code here? */
truelight@752: 	if (tpf->tracktype == TRANSPORT_RAIL) {
tron@1035: 		if (IsTileType(tile, MP_RAILWAY) || IsTileType(tile, MP_STATION) || IsTileType(tile, MP_TUNNELBRIDGE)) {
tron@1901: 			owner = GetTileOwner(tile);
truelight@913: 			/* Check if we are on the middle of a bridge (has no owner) */
tron@2049: 			if (IsTileType(tile, MP_TUNNELBRIDGE) && (_m[tile].m5 & 0xC0) == 0xC0)
truelight@913: 				owner = -1;
truelight@913: 		}
truelight@752: 	}
truelight@0: 
truelight@0: 	// This addition will sometimes overflow by a single tile.
truelight@0: 	// The use of TILE_MASK here makes sure that we still point at a valid
truelight@193: 	// tile, and then this tile will be in the sentinel row/col, so GetTileTrackStatus will fail.
tron@900: 	tile = TILE_MASK(tile + TileOffsByDir(direction));
truelight@0: 
truelight@747: 	/* Check in case of rail if the owner is the same */
truelight@1034: 	if (tpf->tracktype == TRANSPORT_RAIL) {
tron@1035: 		if (IsTileType(tile, MP_RAILWAY) || IsTileType(tile, MP_STATION) || IsTileType(tile, MP_TUNNELBRIDGE))
truelight@913: 			/* Check if we are on the middle of a bridge (has no owner) */
tron@2049: 			if (!IsTileType(tile, MP_TUNNELBRIDGE) || (_m[tile].m5 & 0xC0) != 0xC0)
tron@1901: 				if (owner != -1 && !IsTileOwner(tile, owner))
truelight@913: 					return;
truelight@752: 	}
truelight@747: 
truelight@0: 	if (++tpf->rd.cur_length > 50)
truelight@0: 		return;
truelight@193: 
truelight@0: 	bits = GetTileTrackStatus(tile, tpf->tracktype);
truelight@0: 	bits = (byte)((bits | (bits >> 8)) & _bits_mask[direction]);
truelight@0: 	if (bits == 0)
truelight@0: 		return;
truelight@0: 
tron@926: 	assert(TileX(tile) != MapMaxX() && TileY(tile) != MapMaxY());
truelight@0: 
truelight@0: 	if ( (bits & (bits - 1)) == 0 ) {
truelight@0: 		/* only one direction */
truelight@0: 		i = 0;
truelight@0: 		while (!(bits&1))
truelight@0: 			i++, bits>>=1;
truelight@0: 
truelight@0: 		rd = tpf->rd;
truelight@0: 		goto continue_here;
truelight@0: 	}
truelight@0: 	/* several directions */
truelight@0: 	i=0;
truelight@0: 	do {
truelight@0: 		if (!(bits & 1)) continue;
truelight@0: 		rd = tpf->rd;
truelight@0: 
truelight@0: 		// Change direction 4 times only
truelight@0: 		if ((byte)i != tpf->rd.pft_var6) {
truelight@0: 			if(++tpf->rd.depth > 4) {
truelight@193: 				tpf->rd = rd;
truelight@0: 				return;
truelight@0: 			}
truelight@0: 			tpf->rd.pft_var6 = (byte)i;
truelight@0: 		}
truelight@0: 
truelight@0: continue_here:;
truelight@0: 		tpf->the_dir = HASBIT(_otherdir_mask[direction],i) ? (i+8) : i;
truelight@193: 
truelight@0: 		if (!tpf->enum_proc(tile, tpf->userdata, tpf->the_dir, tpf->rd.cur_length, NULL)) {
truelight@0: 			TPFMode2(tpf, tile, _tpf_new_direction[tpf->the_dir]);
truelight@0: 		}
truelight@0: 
truelight@0: 		tpf->rd = rd;
truelight@0: 	} while (++i, bits>>=1);
truelight@0: 
truelight@0: }
truelight@0: 
truelight@0: static const int8 _get_tunlen_inc[5] = { -16, 0, 16, 0, -16 };
truelight@0: 
darkvater@22: /* Returns the end tile and the length of a tunnel. The length does not
darkvater@22:  * include the starting tile (entry), it does include the end tile (exit).
darkvater@22:  */
tron@2493: FindLengthOfTunnelResult FindLengthOfTunnel(TileIndex tile, uint direction)
truelight@0: {
truelight@0: 	FindLengthOfTunnelResult flotr;
truelight@0: 	int x,y;
truelight@0: 	byte z;
truelight@0: 
truelight@0: 	flotr.length = 0;
truelight@0: 
tron@926: 	x = TileX(tile) * 16;
tron@926: 	y = TileY(tile) * 16;
truelight@0: 
truelight@0: 	z = GetSlopeZ(x+8, y+8);
truelight@0: 
truelight@0: 	for(;;) {
truelight@0: 		flotr.length++;
truelight@0: 
truelight@0: 		x += _get_tunlen_inc[direction];
truelight@0: 		y += _get_tunlen_inc[direction+1];
truelight@0: 
tron@1980: 		tile = TileVirtXY(x, y);
truelight@0: 
tron@1035: 		if (IsTileType(tile, MP_TUNNELBRIDGE) &&
tron@2493: 				GB(_m[tile].m5, 4, 4) == 0 &&               // tunnel entrance/exit
tron@2493: 				// GB(_m[tile].m5, 2, 2) == type &&            // rail/road-tunnel <-- This is not necesary to check, right?
tron@2493: 				(GB(_m[tile].m5, 0, 2) ^ 2) == direction && // entrance towards: 0 = NE, 1 = SE, 2 = SW, 3 = NW
tron@2493: 				GetSlopeZ(x + 8, y + 8) == z) {
tron@2493: 			break;
tron@2493: 		}
truelight@0: 	}
truelight@0: 
truelight@0: 	flotr.tile = tile;
truelight@0: 	return flotr;
truelight@0: }
truelight@0: 
truelight@0: static const uint16 _tpfmode1_and[4] = { 0x1009, 0x16, 0x520, 0x2A00 };
truelight@0: 
tron@1977: static uint SkipToEndOfTunnel(TrackPathFinder *tpf, TileIndex tile, int direction)
tron@1977: {
truelight@0: 	FindLengthOfTunnelResult flotr;
truelight@0: 	TPFSetTileBit(tpf, tile, 14);
truelight@159: 	flotr = FindLengthOfTunnel(tile, direction);
truelight@0: 	tpf->rd.cur_length += flotr.length;
truelight@0: 	TPFSetTileBit(tpf, flotr.tile, 14);
truelight@0: 	return flotr.tile;
truelight@0: }
truelight@0: 
truelight@0: const byte _ffb_64[128] = {
truelight@0: 0,0,1,0,2,0,1,0,
truelight@0: 3,0,1,0,2,0,1,0,
truelight@0: 4,0,1,0,2,0,1,0,
truelight@0: 3,0,1,0,2,0,1,0,
truelight@0: 5,0,1,0,2,0,1,0,
truelight@0: 3,0,1,0,2,0,1,0,
truelight@0: 4,0,1,0,2,0,1,0,
truelight@0: 3,0,1,0,2,0,1,0,
truelight@0: 
truelight@0: 0,0,0,2,0,4,4,6,
truelight@0: 0,8,8,10,8,12,12,14,
truelight@0: 0,16,16,18,16,20,20,22,
truelight@0: 16,24,24,26,24,28,28,30,
truelight@0: 0,32,32,34,32,36,36,38,
truelight@0: 32,40,40,42,40,44,44,46,
truelight@0: 32,48,48,50,48,52,52,54,
truelight@0: 48,56,56,58,56,60,60,62,
truelight@0: };
truelight@0: 
tron@2493: static void TPFMode1(TrackPathFinder *tpf, TileIndex tile, uint direction)
truelight@0: {
truelight@0: 	uint bits;
truelight@0: 	int i;
truelight@0: 	RememberData rd;
tron@1977: 	TileIndex tile_org = tile;
truelight@0: 
tron@2493: 	if (IsTileType(tile, MP_TUNNELBRIDGE) && GB(_m[tile].m5, 4, 4) == 0) {
tron@2493: 		if (GB(_m[tile].m5, 0, 2) != direction ||
tron@2493: 				GB(_m[tile].m5, 2, 2) != tpf->tracktype) {
truelight@0: 			return;
tron@2493: 		}
truelight@193: 		tile = SkipToEndOfTunnel(tpf, tile, direction);
truelight@0: 	}
tron@900: 	tile += TileOffsByDir(direction);
truelight@747: 
truelight@747: 	/* Check in case of rail if the owner is the same */
truelight@752: 	if (tpf->tracktype == TRANSPORT_RAIL) {
tron@1035: 		if (IsTileType(tile_org, MP_RAILWAY) || IsTileType(tile_org, MP_STATION) || IsTileType(tile_org, MP_TUNNELBRIDGE))
tron@1035: 			if (IsTileType(tile, MP_RAILWAY) || IsTileType(tile, MP_STATION) || IsTileType(tile, MP_TUNNELBRIDGE))
truelight@913: 				/* Check if we are on a bridge (middle parts don't have an owner */
tron@2049: 				if (!IsTileType(tile, MP_TUNNELBRIDGE) || (_m[tile].m5 & 0xC0) != 0xC0)
tron@2049: 					if (!IsTileType(tile_org, MP_TUNNELBRIDGE) || (_m[tile_org].m5 & 0xC0) != 0xC0)
tron@1901: 						if (GetTileOwner(tile_org) != GetTileOwner(tile))
truelight@913: 							return;
truelight@752: 	}
truelight@747: 
truelight@0: 	tpf->rd.cur_length++;
truelight@0: 
truelight@0: 	bits = GetTileTrackStatus(tile, tpf->tracktype);
truelight@0: 
truelight@0: 	if ((byte)bits != tpf->var2) {
truelight@0: 		bits &= _tpfmode1_and[direction];
truelight@0: 		bits = bits | (bits>>8);
truelight@0: 	}
truelight@0: 	bits &= 0xBF;
truelight@0: 
truelight@0: 	if (bits != 0) {
truelight@0: 		if (!tpf->disable_tile_hash || (tpf->rd.cur_length <= 64 && (KILL_FIRST_BIT(bits) == 0 || ++tpf->rd.depth <= 7))) {
truelight@0: 			do {
truelight@0: 				i = FIND_FIRST_BIT(bits);
truelight@0: 				bits = KILL_FIRST_BIT(bits);
truelight@0: 
truelight@0: 				tpf->the_dir = (_otherdir_mask[direction] & (byte)(1 << i)) ? (i+8) : i;
truelight@0: 				rd = tpf->rd;
truelight@193: 
truelight@0: 				if (TPFSetTileBit(tpf, tile, tpf->the_dir) &&
truelight@0: 						!tpf->enum_proc(tile, tpf->userdata, tpf->the_dir, tpf->rd.cur_length, &tpf->rd.pft_var6) ) {
truelight@0: 					TPFMode1(tpf, tile, _tpf_new_direction[tpf->the_dir]);
truelight@0: 				}
truelight@0: 				tpf->rd = rd;
truelight@0: 			} while (bits != 0);
truelight@0: 		}
truelight@0: 	}
truelight@0: 
truelight@0: 	/* the next is only used when signals are checked.
truelight@0: 	 * seems to go in 2 directions simultaneously */
truelight@193: 
truelight@0: 	/* if i can get rid of this, tail end recursion can be used to minimize
truelight@193: 	 * stack space dramatically. */
matthijs@2006: 
matthijs@2006: 	/* If we are doing signal setting, we must reverse at evere tile, so we
matthijs@2006: 	 * iterate all the tracks in a signal block, even when a normal train would
matthijs@2006: 	 * not reach it (for example, when two lines merge */
truelight@0: 	if (tpf->hasbit_13)
truelight@0: 		return;
truelight@193: 
truelight@0: 	tile = tile_org;
truelight@0: 	direction ^= 2;
truelight@0: 
truelight@0: 	bits = GetTileTrackStatus(tile, tpf->tracktype);
truelight@0: 	bits |= (bits >> 8);
truelight@0: 
truelight@0: 	if ( (byte)bits != tpf->var2) {
truelight@0: 		bits &= _bits_mask[direction];
truelight@0: 	}
truelight@0: 
truelight@0: 	bits &= 0xBF;
truelight@0: 	if (bits == 0)
truelight@0: 		return;
truelight@0: 
truelight@0: 	do {
truelight@0: 		i = FIND_FIRST_BIT(bits);
truelight@0: 		bits = KILL_FIRST_BIT(bits);
truelight@193: 
truelight@0: 		tpf->the_dir = (_otherdir_mask[direction] & (byte)(1 << i)) ? (i+8) : i;
truelight@0: 		rd = tpf->rd;
truelight@0: 		if (TPFSetTileBit(tpf, tile, tpf->the_dir) &&
truelight@0: 				!tpf->enum_proc(tile, tpf->userdata, tpf->the_dir, tpf->rd.cur_length, &tpf->rd.pft_var6) ) {
truelight@0: 			TPFMode1(tpf, tile, _tpf_new_direction[tpf->the_dir]);
truelight@0: 		}
truelight@0: 		tpf->rd = rd;
truelight@0: 	} while (bits != 0);
truelight@0: }
truelight@0: 
tron@1977: void FollowTrack(TileIndex tile, uint16 flags, byte direction, TPFEnumProc *enum_proc, TPFAfterProc *after_proc, void *data)
truelight@0: {
darkvater@318: 	TrackPathFinder tpf;
truelight@0: 
truelight@0: 	assert(direction < 4);
truelight@0: 
truelight@193: 	/* initialize path finder variables */
darkvater@318: 	tpf.userdata = data;
darkvater@318: 	tpf.enum_proc = enum_proc;
darkvater@318: 	tpf.new_link = tpf.links;
darkvater@318: 	tpf.num_links_left = lengthof(tpf.links);
truelight@0: 
darkvater@318: 	tpf.rd.cur_length = 0;
darkvater@318: 	tpf.rd.depth = 0;
darkvater@318: 	tpf.rd.pft_var6 = 0;
truelight@193: 
darkvater@318: 	tpf.var2 = HASBIT(flags, 15) ? 0x43 : 0xFF; /* 0x8000 */
truelight@0: 
Darkvater@9928: 	tpf.disable_tile_hash = HASBIT(flags, 12);  /* 0x1000 */
Darkvater@9928: 	tpf.hasbit_13         = HASBIT(flags, 13);  /* 0x2000 */
truelight@0: 
truelight@0: 
darkvater@318: 	tpf.tracktype = (byte)flags;
truelight@0: 
truelight@193: 	if (HASBIT(flags, 11)) {
darkvater@318: 		tpf.rd.pft_var6 = 0xFF;
darkvater@318: 		tpf.enum_proc(tile, data, 0, 0, 0);
darkvater@318: 		TPFMode2(&tpf, tile, direction);
truelight@0: 	} else {
truelight@0: 		/* clear the hash_heads */
darkvater@318: 		memset(tpf.hash_head, 0, sizeof(tpf.hash_head));
darkvater@318: 		TPFMode1(&tpf, tile, direction);
truelight@0: 	}
truelight@0: 
truelight@0: 	if (after_proc != NULL)
darkvater@318: 		after_proc(&tpf);
truelight@0: }
truelight@0: 
truelight@0: typedef struct {
truelight@0: 	TileIndex tile;
ludde@2125: 	uint16 cur_length; // This is the current length to this tile.
ludde@2125: 	uint16 priority; // This is the current length + estimated length to the goal.
truelight@0: 	byte track;
truelight@0: 	byte depth;
truelight@0: 	byte state;
truelight@0: 	byte first_track;
truelight@0: } StackedItem;
truelight@0: 
truelight@0: static const byte _new_track[6][4] = {
truelight@0: {0,0xff,8,0xff,},
truelight@0: {0xff,1,0xff,9,},
truelight@0: {0xff,2,10,0xff,},
truelight@0: {3,0xff,0xff,11,},
truelight@0: {12,4,0xff,0xff,},
truelight@0: {0xff,0xff,5,13,},
truelight@0: };
truelight@0: 
truelight@0: typedef struct HashLink {
truelight@0: 	TileIndex tile;
truelight@0: 	uint16 typelength;
truelight@0: 	uint16 next;
truelight@0: } HashLink;
truelight@0: 
truelight@0: typedef struct {
ludde@2125: 	NTPEnumProc *enum_proc;
truelight@0: 	void *userdata;
ludde@2125: 	TileIndex dest;
truelight@0: 
truelight@0: 	byte tracktype;
truelight@0: 	uint maxlength;
truelight@0: 
truelight@0: 	HashLink *new_link;
truelight@0: 	uint num_links_left;
truelight@0: 
tron@979: 	uint nstack;
truelight@0: 	StackedItem stack[256]; // priority queue of stacked items
truelight@0: 
ludde@2125: 	uint16 hash_head[0x400]; // hash heads. 0 means unused. 0xFFFC = length, 0x3 = dir
truelight@0: 	TileIndex hash_tile[0x400]; // tiles. or links.
truelight@0: 
truelight@0: 	HashLink links[0x400]; // hash links
truelight@0: 
truelight@0: } NewTrackPathFinder;
truelight@0: #define NTP_GET_LINK_OFFS(tpf, link) ((byte*)(link) - (byte*)tpf->links)
truelight@0: #define NTP_GET_LINK_PTR(tpf, link_offs) (HashLink*)((byte*)tpf->links + (link_offs))
truelight@0: 
truelight@0: #define ARR(i) tpf->stack[(i)-1]
truelight@0: 
truelight@0: // called after a new element was added in the queue at the last index.
truelight@0: // move it down to the proper position
tron@536: static inline void HeapifyUp(NewTrackPathFinder *tpf)
truelight@0: {
truelight@0: 	StackedItem si;
truelight@0: 	int i = ++tpf->nstack;
truelight@193: 
ludde@2125: 	while (i != 1 && ARR(i).priority < ARR(i>>1).priority) {
truelight@0: 		// the child element is larger than the parent item.
truelight@193: 		// swap the child item and the parent item.
truelight@0: 		si = ARR(i); ARR(i) = ARR(i>>1); ARR(i>>1) = si;
truelight@0: 		i>>=1;
truelight@0: 	}
truelight@0: }
truelight@0: 
truelight@0: // called after the element 0 was eaten. fill it with a new element
tron@536: static inline void HeapifyDown(NewTrackPathFinder *tpf)
truelight@0: {
truelight@0: 	StackedItem si;
truelight@0: 	int i = 1, j;
tron@979: 	int n;
tron@979: 
tron@979: 	assert(tpf->nstack > 0);
tron@979: 	n = --tpf->nstack;
truelight@0: 
truelight@0: 	if (n == 0) return; // heap is empty so nothing to do?
truelight@0: 
truelight@0: 	// copy the last item to index 0. we use it as base for heapify.
truelight@0: 	ARR(1) = ARR(n+1);
truelight@0: 
truelight@0: 	while ((j=i*2) <= n) {
truelight@0: 		// figure out which is smaller of the children.
ludde@2125: 		if (j != n && ARR(j).priority > ARR(j+1).priority)
truelight@0: 			j++; // right item is smaller
truelight@0: 
truelight@0: 		assert(i <= n && j <= n);
ludde@2125: 		if (ARR(i).priority <= ARR(j).priority)
truelight@0: 			break; // base elem smaller than smallest, done!
truelight@193: 
truelight@0: 		// swap parent with the child
truelight@0: 		si = ARR(i); ARR(i) = ARR(j); ARR(j) = si;
truelight@0: 		i = j;
truelight@0: 	}
truelight@0: }
truelight@0: 
truelight@0: // mark a tile as visited and store the length of the path.
truelight@0: // if we already had a better path to this tile, return false.
truelight@0: // otherwise return true.
tron@1977: static bool NtpVisit(NewTrackPathFinder *tpf, TileIndex tile, uint dir, uint length)
truelight@0: {
truelight@0: 	uint hash,head;
truelight@0: 	HashLink *link, *new_link;
truelight@0: 
ludde@2044: 	assert(length < 16384-1);
truelight@193: 
truelight@0: 	hash = PATHFIND_HASH_TILE(tile);
truelight@0: 
truelight@0: 	// never visited before?
truelight@0: 	if ((head=tpf->hash_head[hash]) == 0) {
truelight@0: 		tpf->hash_tile[hash] = tile;
truelight@0: 		tpf->hash_head[hash] = dir | (length << 2);
truelight@0: 		return true;
truelight@0: 	}
truelight@0: 
truelight@0: 	if (head != 0xffff) {
tron@1986: 		if (tile == tpf->hash_tile[hash] && (head & 0x3) == dir) {
truelight@193: 
truelight@0: 			// longer length
truelight@0: 			if (length >= (head >> 2)) return false;
truelight@193: 
truelight@0: 			tpf->hash_head[hash] = dir | (length << 2);
truelight@0: 			return true;
truelight@0: 		}
truelight@0: 		// two tiles with the same hash, need to make a link
truelight@0: 		// allocate a link. if out of links, handle this by returning
truelight@0: 		// that a tile was already visisted.
ludde@2125: 		if (tpf->num_links_left == 0) {
ludde@2125: 			DEBUG(ntp, 1) ("[NTP] no links left");
truelight@0: 			return false;
ludde@2125: 		}
ludde@2125: 
truelight@0: 		tpf->num_links_left--;
truelight@0: 		link = tpf->new_link++;
truelight@0: 
truelight@0: 		/* move the data that was previously in the hash_??? variables
truelight@0: 		 * to the link struct, and let the hash variables point to the link */
truelight@0: 		link->tile = tpf->hash_tile[hash];
truelight@0: 		tpf->hash_tile[hash] = NTP_GET_LINK_OFFS(tpf, link);
truelight@0: 
truelight@0: 		link->typelength = tpf->hash_head[hash];
truelight@193: 		tpf->hash_head[hash] = 0xFFFF; /* multi link */
truelight@0: 		link->next = 0xFFFF;
truelight@0: 	} else {
truelight@0: 		// a linked list of many tiles,
truelight@0: 		// find the one corresponding to the tile, if it exists.
truelight@0: 		// otherwise make a new link
truelight@193: 
truelight@0: 		uint offs = tpf->hash_tile[hash];
truelight@0: 		do {
truelight@0: 			link = NTP_GET_LINK_PTR(tpf, offs);
tron@1986: 			if (tile == link->tile && (uint)(link->typelength & 0x3) == dir) {
truelight@0: 				if (length >= (uint)(link->typelength >> 2)) return false;
truelight@0: 				link->typelength = dir | (length << 2);
truelight@0: 				return true;
truelight@0: 			}
truelight@0: 		} while ((offs=link->next) != 0xFFFF);
truelight@0: 	}
truelight@193: 
truelight@0: 	/* get here if we need to add a new link to link,
truelight@0: 	 * first, allocate a new link, in the same way as before */
ludde@2125: 	if (tpf->num_links_left == 0) {
ludde@2125: 		DEBUG(ntp, 1) ("[NTP] no links left");
ludde@2125: 		return false;
ludde@2125: 	}
truelight@0: 	tpf->num_links_left--;
truelight@0: 	new_link = tpf->new_link++;
truelight@0: 
truelight@0: 	/* then fill the link with the new info, and establish a ptr from the old
truelight@0: 	 * link to the new one */
tron@1986: 	new_link->tile = tile;
truelight@0: 	new_link->typelength = dir | (length << 2);
truelight@0: 	new_link->next = 0xFFFF;
truelight@0: 
truelight@0: 	link->next = NTP_GET_LINK_OFFS(tpf, new_link);
truelight@0: 	return true;
truelight@0: }
truelight@0: 
matthijs@2782: /**
matthijs@2782:  * Checks if the shortest path to the given tile/dir so far is still the given
matthijs@2782:  * length.
matthijs@2782:  * @return true if the length is still the same
matthijs@2782:  * @pre    The given tile/dir combination should be present in the hash, by a
matthijs@2782:  *         previous call to NtpVisit().
matthijs@2782:  */
tron@1977: static bool NtpCheck(NewTrackPathFinder *tpf, TileIndex tile, uint dir, uint length)
truelight@0: {
truelight@0: 	uint hash,head,offs;
truelight@0: 	HashLink *link;
truelight@0: 
truelight@0: 	hash = PATHFIND_HASH_TILE(tile);
truelight@0: 	head=tpf->hash_head[hash];
truelight@0: 	assert(head);
truelight@0: 
truelight@0: 	if (head != 0xffff) {
truelight@0: 		assert( tpf->hash_tile[hash] == tile && (head & 3) == dir);
truelight@0: 		assert( (head >> 2) <= length);
truelight@0: 		return length == (head >> 2);
truelight@0: 	}
truelight@0: 
truelight@0: 	// else it's a linked list of many tiles
truelight@0: 	offs = tpf->hash_tile[hash];
truelight@0: 	for(;;) {
truelight@0: 		link = NTP_GET_LINK_PTR(tpf, offs);
tron@1986: 		if (tile == link->tile && (uint)(link->typelength & 0x3) == dir) {
truelight@0: 			assert( (uint)(link->typelength >> 2) <= length);
truelight@0: 			return length == (uint)(link->typelength >> 2);
truelight@0: 		}
truelight@0: 		offs = link->next;
truelight@0: 		assert(offs != 0xffff);
truelight@0: 	}
truelight@0: }
truelight@0: 
truelight@0: 
ludde@2044: static const uint16 _is_upwards_slope[15] = {
ludde@2044: 	0, // no tileh
ludde@2044: 	(1 << TRACKDIR_DIAG1_SW) | (1 << TRACKDIR_DIAG2_NW), // 1
ludde@2044: 	(1 << TRACKDIR_DIAG1_SW) | (1 << TRACKDIR_DIAG2_SE), // 2
ludde@2044: 	(1 << TRACKDIR_DIAG1_SW), // 3
ludde@2044: 	(1 << TRACKDIR_DIAG1_NE) | (1 << TRACKDIR_DIAG2_SE), // 4
ludde@2044: 	0, // 5
ludde@2044: 	(1 << TRACKDIR_DIAG2_SE), // 6
ludde@2044: 	0, // 7
ludde@2044: 	(1 << TRACKDIR_DIAG1_NE) | (1 << TRACKDIR_DIAG2_NW), // 8,
ludde@2044: 	(1 << TRACKDIR_DIAG2_NW), // 9
ludde@2044: 	0, //10
ludde@2044: 	0, //11,
ludde@2044: 	(1 << TRACKDIR_DIAG1_NE), //12
ludde@2044: 	0, //13
ludde@2044: 	0, //14
ludde@2044: };
ludde@2044: 
ludde@2044: 
ludde@2125: #define DIAG_FACTOR 3
ludde@2125: #define STR_FACTOR 2
ludde@2125: 
ludde@2125: 
ludde@2125: static uint DistanceMoo(TileIndex t0, TileIndex t1)
ludde@2125: {
ludde@2125: 	const uint dx = abs(TileX(t0) - TileX(t1));
ludde@2125: 	const uint dy = abs(TileY(t0) - TileY(t1));
ludde@2125: 
ludde@2125: 	const uint straightTracks = 2 * min(dx, dy); /* The number of straight (not full length) tracks */
ludde@2125: 	/* OPTIMISATION:
ludde@2125: 	 * Original: diagTracks = max(dx, dy) - min(dx,dy);
ludde@2125: 	 * Proof:
ludde@2125: 	 * (dx-dy) - straightTracks  == (min + max) - straightTracks = min + // max - 2 * min = max - min */
ludde@2125: 	const uint diagTracks = dx + dy - straightTracks; /* The number of diagonal (full tile length) tracks. */
ludde@2125: 
ludde@2125: 	return diagTracks*DIAG_FACTOR + straightTracks*STR_FACTOR;
ludde@2125: }
ludde@2125: 
ludde@2125: // These has to be small cause the max length of a track
ludde@2125: // is currently limited to 16384
ludde@2125: 
ludde@2125: static const byte _length_of_track[16] = {
ludde@2125: 	DIAG_FACTOR,DIAG_FACTOR,STR_FACTOR,STR_FACTOR,STR_FACTOR,STR_FACTOR,0,0,
ludde@2125: 	DIAG_FACTOR,DIAG_FACTOR,STR_FACTOR,STR_FACTOR,STR_FACTOR,STR_FACTOR,0,0
ludde@2125: };
ludde@2125: 
truelight@0: // new more optimized pathfinder for trains...
ludde@2125: // Tile is the tile the train is at.
ludde@2125: // direction is the tile the train is moving towards.
ludde@2125: 
tron@1977: static void NTPEnum(NewTrackPathFinder *tpf, TileIndex tile, uint direction)
truelight@0: {
matthijs@2782: 	TrackBits bits, allbits;
matthijs@2782: 	uint track;
matthijs@2782: 	TileIndex tile_org;
truelight@0: 	StackedItem si;
truelight@0: 	FindLengthOfTunnelResult flotr;
ludde@2125: 	int estimation;
truelight@0: 
ludde@2261: 
ludde@2136: 
ludde@2125: 	// Need to have a special case for the start.
ludde@2125: 	// We shouldn't call the callback for the current tile.
ludde@2125: 	si.cur_length = 1; // Need to start at 1 cause 0 is a reserved value.
truelight@0: 	si.depth = 0;
truelight@0: 	si.state = 0;
ludde@2136: 	si.first_track = 0xFF;
ludde@2125: 	goto start_at;
truelight@0: 
truelight@0: 	for(;;) {
ludde@2125: 		// Get the next item to search from from the priority queue
ludde@2125: 		do {
ludde@2125: 			if (tpf->nstack == 0)
ludde@2125: 				return; // nothing left? then we're done!
ludde@2125: 			si = tpf->stack[0];
ludde@2125: 			tile = si.tile;
truelight@0: 
ludde@2125: 			HeapifyDown(tpf);
ludde@2125: 			// Make sure we havn't already visited this tile.
ludde@2125: 		} while (!NtpCheck(tpf, tile, _tpf_prev_direction[si.track], si.cur_length));
truelight@0: 
ludde@2125: 		// Add the length of this track.
ludde@2125: 		si.cur_length += _length_of_track[si.track];
truelight@193: 
ludde@2125: callback_and_continue:
ludde@2125: 		if (tpf->enum_proc(tile, tpf->userdata, si.first_track, si.cur_length))
ludde@2125: 			return;
truelight@0: 
ludde@2136: 		assert(si.track <= 13);
ludde@2125: 		direction = _tpf_new_direction[si.track];
ludde@2044: 
ludde@2125: start_at:
ludde@2125: 		// If the tile is the entry tile of a tunnel, and we're not going out of the tunnel,
ludde@2125: 		//   need to find the exit of the tunnel.
ludde@2125: 		if (IsTileType(tile, MP_TUNNELBRIDGE)) {
tron@2493: 			if (GB(_m[tile].m5, 4, 4) == 0 &&
tron@2493: 					GB(_m[tile].m5, 0, 2) != (direction ^ 2)) {
ludde@2125: 				/* This is a tunnel tile */
ludde@2125: 				/* We are not just driving out of the tunnel */
tron@2493: 				if (GB(_m[tile].m5, 0, 2) != direction || GB(_m[tile].m5, 2, 2) != tpf->tracktype)
ludde@2125: 					/* We are not driving into the tunnel, or it
ludde@2125: 					 * is an invalid tunnel */
ludde@2125: 					continue;
ludde@2125: 				flotr = FindLengthOfTunnel(tile, direction);
ludde@2125: 				si.cur_length += flotr.length * DIAG_FACTOR;
ludde@2125: 				tile = flotr.tile;
ludde@2125: 				// tile now points to the exit tile of the tunnel
ludde@2125: 			}
ludde@2044: 		}
ludde@2044: 
ludde@2125: 		// This is a special loop used to go through
ludde@2125: 		// a rail net and find the first intersection
ludde@2125: 		tile_org = tile;
ludde@2125: 		for(;;) {
ludde@2136: 			assert(direction <= 3);
ludde@2125: 			tile += TileOffsByDir(direction);
ludde@2044: 
ludde@2125: 			// too long search length? bail out.
ludde@2125: 			if (si.cur_length >= tpf->maxlength) {
ludde@2125: 				DEBUG(ntp,1) ("[NTP] cur_length too big");
ludde@2125: 				bits = 0;
ludde@2125: 				break;
ludde@2044: 			}
ludde@2044: 
ludde@2125: 			// Not a regular rail tile?
ludde@2125: 			// Then we can't use the code below, but revert to more general code.
ludde@2125: 			if (!IsTileType(tile, MP_RAILWAY) || !IsPlainRailTile(tile)) {
ludde@2125: 				// We found a tile which is not a normal railway tile.
ludde@2125: 				// Determine which tracks that exist on this tile.
ludde@2125: 				bits = GetTileTrackStatus(tile, TRANSPORT_RAIL) & _tpfmode1_and[direction];
ludde@2125: 				bits = (bits | (bits >> 8)) & 0x3F;
ludde@2125: 
ludde@2125: 				// Check that the tile contains exactly one track
matthijs@2782: 				if (bits == 0 || KILL_FIRST_BIT(bits) != 0) break;
ludde@2125: 
ludde@2125: 				///////////////////
ludde@2125: 				// If we reach here, the tile has exactly one track.
ludde@2125: 				//   tile - index to a tile that is not rail tile, but still straight (with optional signals)
ludde@2125: 				//   bits - bitmask of which track that exist on the tile (exactly one bit is set)
ludde@2125: 				//   direction - which direction are we moving in?
ludde@2125: 				///////////////////
ludde@2125: 				si.track = _new_track[FIND_FIRST_BIT(bits)][direction];
ludde@2125: 				si.cur_length += _length_of_track[si.track];
ludde@2125: 				goto callback_and_continue;
ludde@2125: 			}
ludde@2125: 
matthijs@2782: 			/* Regular rail tile, determine which tracks exist. */
matthijs@2782: 			allbits = _m[tile].m5 & TRACK_BIT_MASK;
matthijs@2782: 			/* Which tracks are reachable? */
matthijs@2782: 			bits = allbits & DiagdirReachesTracks(direction);
ludde@2125: 
matthijs@2782: 			/* The tile has no reachable tracks => End of rail segment
matthijs@2782: 			 * or Intersection => End of rail segment. We check this agains all the
matthijs@2782: 			 * bits, not just reachable ones, to prevent infinite loops. */
matthijs@2782: 			if (bits == 0 || TracksOverlap(allbits)) break;
ludde@2137: 
matthijs@2782: 			/* If we reach here, the tile has exactly one track, and this
matthijs@2782: 			 track is reachable => Rail segment continues */
ludde@2125: 
ludde@2125: 			track = _new_track[FIND_FIRST_BIT(bits)][direction];
ludde@2137: 			assert(track != 0xff);
ludde@2137: 
ludde@2125: 			si.cur_length += _length_of_track[track];
ludde@2125: 
ludde@2125: 			// Check if this rail is an upwards slope. If it is, then add a penalty.
ludde@2125: 			// Small optimization here.. if (track&7)>1 then it can't be a slope so we avoid calling GetTileSlope
ludde@2125: 			if ((track & 7) <= 1 && (_is_upwards_slope[GetTileSlope(tile, NULL)] & (1 << track)) ) {
ludde@2125: 				// upwards slope. add some penalty.
ludde@2125: 				si.cur_length += 4*DIAG_FACTOR;
ludde@2125: 			}
ludde@2125: 
ludde@2125: 			// railway tile with signals..?
ludde@2125: 			if (HasSignals(tile)) {
ludde@2125: 				byte m3;
ludde@2125: 
ludde@2125: 				m3 = _m[tile].m3;
ludde@2125: 				if (!(m3 & SignalAlongTrackdir(track))) {
matthijs@2782: 					// if one way signal not pointing towards us, stop going in this direction => End of rail segment.
ludde@2125: 					if (m3 & SignalAgainstTrackdir(track)) {
ludde@2125: 						bits = 0;
ludde@2125: 						break;
ludde@2125: 					}
ludde@2125: 				} else if (_m[tile].m2 & SignalAlongTrackdir(track)) {
ludde@2125: 					// green signal in our direction. either one way or two way.
ludde@2125: 					si.state |= 3;
ludde@2125: 				} else {
ludde@2125: 					// reached a red signal.
ludde@2125: 					if (m3 & SignalAgainstTrackdir(track)) {
ludde@2125: 						// two way red signal. unless we passed another green signal on the way,
matthijs@2782: 						// stop going in this direction => End of rail segment.
ludde@2125: 						// this is to prevent us from going into a full platform.
ludde@2125: 						if (!(si.state&1)) {
ludde@2125: 							bits = 0;
ludde@2125: 							break;
ludde@2125: 						}
ludde@2125: 					}
ludde@2125: 					if (!(si.state & 2)) {
ludde@2125: 						// Is this the first signal we see? And it's red... add penalty
ludde@2125: 						si.cur_length += 10*DIAG_FACTOR;
ludde@2125: 						si.state += 2; // remember that we added penalty.
ludde@2125: 						// Because we added a penalty, we can't just continue as usual.
ludde@2125: 						// Need to get out and let A* do it's job with
ludde@2125: 						// possibly finding an even shorter path.
ludde@2125: 						break;
ludde@2125: 					}
ludde@2125: 				}
ludde@2125: 
ludde@2125: 				if (tpf->enum_proc(tile, tpf->userdata, si.first_track, si.cur_length))
matthijs@2782: 					return; /* Don't process this tile any further */
ludde@2125: 			}
ludde@2125: 
ludde@2125: 			// continue with the next track
ludde@2125: 			direction = _tpf_new_direction[track];
ludde@2125: 
ludde@2125: 			// safety check if we're running around chasing our tail... (infinite loop)
ludde@2125: 			if (tile == tile_org) {
ludde@2125: 				bits = 0;
ludde@2125: 				break;
ludde@2125: 			}
ludde@2044: 		}
ludde@2044: 
ludde@2125: 		// There are no tracks to choose between.
ludde@2125: 		// Stop searching in this direction
ludde@2125: 		if (bits == 0)
ludde@2125: 			continue;
truelight@0: 
ludde@2125: 		////////////////
ludde@2125: 		// We got multiple tracks to choose between (intersection).
ludde@2125: 		// Branch the search space into several branches.
ludde@2125: 		////////////////
truelight@193: 
ludde@2125: 		// Check if we've already visited this intersection.
ludde@2125: 		// If we've already visited it with a better length, then
ludde@2125: 		// there's no point in visiting it again.
ludde@2125: 		if (!NtpVisit(tpf, tile, direction, si.cur_length))
ludde@2125: 			continue;
truelight@0: 
ludde@2044: 		// Push all possible alternatives that we can reach from here
ludde@2044: 		// onto the priority heap.
ludde@2125: 		// 'bits' contains the tracks that we can choose between.
ludde@2125: 
ludde@2125: 		// First compute the estimated distance to the target.
ludde@2125: 		// This is used to implement A*
ludde@2125: 		estimation = 0;
ludde@2125: 		if (tpf->dest != 0)
ludde@2125: 			estimation = DistanceMoo(tile, tpf->dest);
ludde@2125: 
ludde@2125: 		si.depth++;
matthijs@2774: 		if (si.depth == 0)
matthijs@2774: 			continue; /* We overflowed our depth. No more searching in this direction. */
truelight@0: 		si.tile = tile;
truelight@0: 		do {
truelight@0: 			si.track = _new_track[FIND_FIRST_BIT(bits)][direction];
matthijs@2782: 			assert(si.track != 0xFF);
ludde@2125: 			si.priority = si.cur_length + estimation;
ludde@2125: 
truelight@0: 			// out of stack items, bail out?
ludde@2044: 			if (tpf->nstack >= lengthof(tpf->stack)) {
ludde@2125: 				DEBUG(ntp, 1) ("[NTP] out of stack");
truelight@0: 				break;
ludde@2044: 			}
ludde@2125: 
truelight@0: 			tpf->stack[tpf->nstack] = si;
truelight@0: 			HeapifyUp(tpf);
truelight@0: 		} while ((bits = KILL_FIRST_BIT(bits)) != 0);
truelight@0: 
ludde@2044: 		// If this is the first intersection, we need to fill the first_track member.
truelight@0: 		// so the code outside knows which path is better.
truelight@0: 		// also randomize the order in which we search through them.
truelight@0: 		if (si.depth == 1) {
ludde@2125: 			assert(tpf->nstack == 1 || tpf->nstack == 2 || tpf->nstack == 3);
ludde@2125: 			if (tpf->nstack != 1) {
ludde@2125: 				uint32 r = Random();
ludde@2125: 				if (r&1) swap_byte(&tpf->stack[0].track, &tpf->stack[1].track);
ludde@2125: 				if (tpf->nstack != 2) {
ludde@2125: 					byte t = tpf->stack[2].track;
ludde@2125: 					if (r&2) swap_byte(&tpf->stack[0].track, &t);
ludde@2125: 					if (r&4) swap_byte(&tpf->stack[1].track, &t);
ludde@2125: 					tpf->stack[2].first_track = tpf->stack[2].track = t;
ludde@2125: 				}
ludde@2125: 				tpf->stack[0].first_track = tpf->stack[0].track;
ludde@2125: 				tpf->stack[1].first_track = tpf->stack[1].track;
truelight@0: 			}
truelight@0: 		}
ludde@2044: 
ludde@2125: 		// Continue with the next from the queue...
ludde@2125: 	}
truelight@0: }
truelight@0: 
truelight@0: 
truelight@0: // new pathfinder for trains. better and faster.
ludde@2125: void NewTrainPathfind(TileIndex tile, TileIndex dest, byte direction, NTPEnumProc *enum_proc, void *data)
truelight@0: {
ludde@2044: 	NewTrackPathFinder tpf;
truelight@0: 
ludde@2125: 	tpf.dest = dest;
ludde@2044: 	tpf.userdata = data;
ludde@2044: 	tpf.enum_proc = enum_proc;
ludde@2044: 	tpf.tracktype = 0;
ludde@2125: 	tpf.maxlength = min(_patches.pf_maxlength * 3, 10000);
ludde@2044: 	tpf.nstack = 0;
ludde@2044: 	tpf.new_link = tpf.links;
ludde@2044: 	tpf.num_links_left = lengthof(tpf.links);
ludde@2044: 	memset(tpf.hash_head, 0, sizeof(tpf.hash_head));
ludde@2044: 
ludde@2044: 	NTPEnum(&tpf, tile, direction);
truelight@0: }