diff options
Diffstat (limited to 'src/pathfinder')
25 files changed, 7054 insertions, 0 deletions
diff --git a/src/pathfinder/npf/aystar.cpp b/src/pathfinder/npf/aystar.cpp new file mode 100644 index 000000000..8c0a65706 --- /dev/null +++ b/src/pathfinder/npf/aystar.cpp @@ -0,0 +1,306 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file aystar.cpp Implementation of A*. */ + +/* + * This file has the core function for AyStar + * AyStar is a fast pathfinding routine and is used for things like + * AI_pathfinding and Train_pathfinding. + * For more information about AyStar (A* Algorithm), you can look at + * http://en.wikipedia.org/wiki/A-star_search_algorithm + */ + +/* + * Friendly reminder: + * Call (AyStar).free() when you are done with Aystar. It reserves a lot of memory + * And when not free'd, it can cause system-crashes. + * Also remember that when you stop an algorithm before it is finished, your + * should call clear() yourself! + */ + +#include "../../stdafx.h" +#include "../../core/alloc_func.hpp" +#include "aystar.h" + +int _aystar_stats_open_size; +int _aystar_stats_closed_size; + +/* This looks in the Hash if a node exists in ClosedList + * If so, it returns the PathNode, else NULL */ +static PathNode *AyStarMain_ClosedList_IsInList(AyStar *aystar, const AyStarNode *node) +{ + return (PathNode*)Hash_Get(&aystar->ClosedListHash, node->tile, node->direction); +} + +/* This adds a node to the ClosedList + * It makes a copy of the data */ +static void AyStarMain_ClosedList_Add(AyStar *aystar, const PathNode *node) +{ + /* Add a node to the ClosedList */ + PathNode *new_node = MallocT<PathNode>(1); + *new_node = *node; + Hash_Set(&aystar->ClosedListHash, node->node.tile, node->node.direction, new_node); +} + +/* Checks if a node is in the OpenList + * If so, it returns the OpenListNode, else NULL */ +static OpenListNode *AyStarMain_OpenList_IsInList(AyStar *aystar, const AyStarNode *node) +{ + return (OpenListNode*)Hash_Get(&aystar->OpenListHash, node->tile, node->direction); +} + +/* Gets the best node from OpenList + * returns the best node, or NULL of none is found + * Also it deletes the node from the OpenList */ +static OpenListNode *AyStarMain_OpenList_Pop(AyStar *aystar) +{ + /* Return the item the Queue returns.. the best next OpenList item. */ + OpenListNode *res = (OpenListNode*)aystar->OpenListQueue.pop(&aystar->OpenListQueue); + if (res != NULL) { + Hash_Delete(&aystar->OpenListHash, res->path.node.tile, res->path.node.direction); + } + + return res; +} + +/* Adds a node to the OpenList + * It makes a copy of node, and puts the pointer of parent in the struct */ +static void AyStarMain_OpenList_Add(AyStar *aystar, PathNode *parent, const AyStarNode *node, int f, int g) +{ + /* Add a new Node to the OpenList */ + OpenListNode *new_node = MallocT<OpenListNode>(1); + new_node->g = g; + new_node->path.parent = parent; + new_node->path.node = *node; + Hash_Set(&aystar->OpenListHash, node->tile, node->direction, new_node); + + /* Add it to the queue */ + aystar->OpenListQueue.push(&aystar->OpenListQueue, new_node, f); +} + +/* + * Checks one tile and calculate his f-value + * return values: + * AYSTAR_DONE : indicates we are done + */ +static int AyStarMain_CheckTile(AyStar *aystar, AyStarNode *current, OpenListNode *parent) +{ + int new_f, new_g, new_h; + PathNode *closedlist_parent; + OpenListNode *check; + + /* Check the new node against the ClosedList */ + if (AyStarMain_ClosedList_IsInList(aystar, current) != NULL) return AYSTAR_DONE; + + /* Calculate the G-value for this node */ + new_g = aystar->CalculateG(aystar, current, parent); + /* If the value was INVALID_NODE, we don't do anything with this node */ + if (new_g == AYSTAR_INVALID_NODE) return AYSTAR_DONE; + + /* There should not be given any other error-code.. */ + assert(new_g >= 0); + /* Add the parent g-value to the new g-value */ + new_g += parent->g; + if (aystar->max_path_cost != 0 && (uint)new_g > aystar->max_path_cost) return AYSTAR_DONE; + + /* Calculate the h-value */ + new_h = aystar->CalculateH(aystar, current, parent); + /* There should not be given any error-code.. */ + assert(new_h >= 0); + + /* The f-value if g + h */ + new_f = new_g + new_h; + + /* Get the pointer to the parent in the ClosedList (the currentone is to a copy of the one in the OpenList) */ + closedlist_parent = AyStarMain_ClosedList_IsInList(aystar, &parent->path.node); + + /* Check if this item is already in the OpenList */ + check = AyStarMain_OpenList_IsInList(aystar, current); + if (check != NULL) { + uint i; + /* Yes, check if this g value is lower.. */ + if (new_g > check->g) return AYSTAR_DONE; + aystar->OpenListQueue.del(&aystar->OpenListQueue, check, 0); + /* It is lower, so change it to this item */ + check->g = new_g; + check->path.parent = closedlist_parent; + /* Copy user data, will probably have changed */ + for (i = 0; i < lengthof(current->user_data); i++) { + check->path.node.user_data[i] = current->user_data[i]; + } + /* Readd him in the OpenListQueue */ + aystar->OpenListQueue.push(&aystar->OpenListQueue, check, new_f); + } else { + /* A new node, add him to the OpenList */ + AyStarMain_OpenList_Add(aystar, closedlist_parent, current, new_f, new_g); + } + + return AYSTAR_DONE; +} + +/* + * This function is the core of AyStar. It handles one item and checks + * his neighbour items. If they are valid, they are added to be checked too. + * return values: + * AYSTAR_EMPTY_OPENLIST : indicates all items are tested, and no path + * has been found. + * AYSTAR_LIMIT_REACHED : Indicates that the max_nodes limit has been + * reached. + * AYSTAR_FOUND_END_NODE : indicates we found the end. Path_found now is true, and in path is the path found. + * AYSTAR_STILL_BUSY : indicates we have done this tile, did not found the path yet, and have items left to try. + */ +static int AyStarMain_Loop(AyStar *aystar) +{ + int i, r; + + /* Get the best node from OpenList */ + OpenListNode *current = AyStarMain_OpenList_Pop(aystar); + /* If empty, drop an error */ + if (current == NULL) return AYSTAR_EMPTY_OPENLIST; + + /* Check for end node and if found, return that code */ + if (aystar->EndNodeCheck(aystar, current) == AYSTAR_FOUND_END_NODE) { + if (aystar->FoundEndNode != NULL) + aystar->FoundEndNode(aystar, current); + free(current); + return AYSTAR_FOUND_END_NODE; + } + + /* Add the node to the ClosedList */ + AyStarMain_ClosedList_Add(aystar, ¤t->path); + + /* Load the neighbours */ + aystar->GetNeighbours(aystar, current); + + /* Go through all neighbours */ + for (i = 0; i < aystar->num_neighbours; i++) { + /* Check and add them to the OpenList if needed */ + r = aystar->checktile(aystar, &aystar->neighbours[i], current); + } + + /* Free the node */ + free(current); + + if (aystar->max_search_nodes != 0 && Hash_Size(&aystar->ClosedListHash) >= aystar->max_search_nodes) { + /* We've expanded enough nodes */ + return AYSTAR_LIMIT_REACHED; + } else { + /* Return that we are still busy */ + return AYSTAR_STILL_BUSY; + } +} + +/* + * This function frees the memory it allocated + */ +static void AyStarMain_Free(AyStar *aystar) +{ + aystar->OpenListQueue.free(&aystar->OpenListQueue, false); + /* 2nd argument above is false, below is true, to free the values only + * once */ + delete_Hash(&aystar->OpenListHash, true); + delete_Hash(&aystar->ClosedListHash, true); +#ifdef AYSTAR_DEBUG + printf("[AyStar] Memory free'd\n"); +#endif +} + +/* + * This function make the memory go back to zero + * This function should be called when you are using the same instance again. + */ +void AyStarMain_Clear(AyStar *aystar) +{ + /* Clean the Queue, but not the elements within. That will be done by + * the hash. */ + aystar->OpenListQueue.clear(&aystar->OpenListQueue, false); + /* Clean the hashes */ + clear_Hash(&aystar->OpenListHash, true); + clear_Hash(&aystar->ClosedListHash, true); + +#ifdef AYSTAR_DEBUG + printf("[AyStar] Cleared AyStar\n"); +#endif +} + +/* + * This is the function you call to run AyStar. + * return values: + * AYSTAR_FOUND_END_NODE : indicates we found an end node. + * AYSTAR_NO_PATH : indicates that there was no path found. + * AYSTAR_STILL_BUSY : indicates we have done some checked, that we did not found the path yet, and that we still have items left to try. + * When the algorithm is done (when the return value is not AYSTAR_STILL_BUSY) + * aystar->clear() is called. Note that when you stop the algorithm halfway, + * you should still call clear() yourself! + */ +int AyStarMain_Main(AyStar *aystar) +{ + int r, i = 0; + /* Loop through the OpenList + * Quit if result is no AYSTAR_STILL_BUSY or is more than loops_per_tick */ + while ((r = aystar->loop(aystar)) == AYSTAR_STILL_BUSY && (aystar->loops_per_tick == 0 || ++i < aystar->loops_per_tick)) { } +#ifdef AYSTAR_DEBUG + switch (r) { + case AYSTAR_FOUND_END_NODE: printf("[AyStar] Found path!\n"); break; + case AYSTAR_EMPTY_OPENLIST: printf("[AyStar] OpenList run dry, no path found\n"); break; + case AYSTAR_LIMIT_REACHED: printf("[AyStar] Exceeded search_nodes, no path found\n"); break; + default: break; + } +#endif + if (r != AYSTAR_STILL_BUSY) { + /* We're done, clean up */ + _aystar_stats_open_size = aystar->OpenListHash.size; + _aystar_stats_closed_size = aystar->ClosedListHash.size; + aystar->clear(aystar); + } + + switch (r) { + case AYSTAR_FOUND_END_NODE: return AYSTAR_FOUND_END_NODE; + case AYSTAR_EMPTY_OPENLIST: + case AYSTAR_LIMIT_REACHED: return AYSTAR_NO_PATH; + default: return AYSTAR_STILL_BUSY; + } +} + +/* + * Adds a node from where to start an algorithm. Multiple nodes can be added + * if wanted. You should make sure that clear() is called before adding nodes + * if the AyStar has been used before (though the normal main loop calls + * clear() automatically when the algorithm finishes + * g is the cost for starting with this node. + */ +static void AyStarMain_AddStartNode(AyStar *aystar, AyStarNode *start_node, uint g) +{ +#ifdef AYSTAR_DEBUG + printf("[AyStar] Starting A* Algorithm from node (%d, %d, %d)\n", + TileX(start_node->tile), TileY(start_node->tile), start_node->direction); +#endif + AyStarMain_OpenList_Add(aystar, NULL, start_node, 0, g); +} + +void init_AyStar(AyStar *aystar, Hash_HashProc hash, uint num_buckets) +{ + /* Allocated the Hash for the OpenList and ClosedList */ + init_Hash(&aystar->OpenListHash, hash, num_buckets); + init_Hash(&aystar->ClosedListHash, hash, num_buckets); + + /* Set up our sorting queue + * BinaryHeap allocates a block of 1024 nodes + * When thatone gets full it reserves an otherone, till this number + * That is why it can stay this high */ + init_BinaryHeap(&aystar->OpenListQueue, 102400); + + aystar->addstart = AyStarMain_AddStartNode; + aystar->main = AyStarMain_Main; + aystar->loop = AyStarMain_Loop; + aystar->free = AyStarMain_Free; + aystar->clear = AyStarMain_Clear; + aystar->checktile = AyStarMain_CheckTile; +} diff --git a/src/pathfinder/npf/aystar.h b/src/pathfinder/npf/aystar.h new file mode 100644 index 000000000..56c1804e0 --- /dev/null +++ b/src/pathfinder/npf/aystar.h @@ -0,0 +1,181 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file aystar.h + * This file has the header for AyStar + * AyStar is a fast pathfinding routine and is used for things like + * AI_pathfinding and Train_pathfinding. + * For more information about AyStar (A* Algorithm), you can look at + * http://en.wikipedia.org/wiki/A-star_search_algorithm + */ + +#ifndef AYSTAR_H +#define AYSTAR_H + +#include "queue.h" +#include "../../tile_type.h" +#include "../../track_type.h" + +//#define AYSTAR_DEBUG +enum { + AYSTAR_FOUND_END_NODE, + AYSTAR_EMPTY_OPENLIST, + AYSTAR_STILL_BUSY, + AYSTAR_NO_PATH, + AYSTAR_LIMIT_REACHED, + AYSTAR_DONE +}; + +enum{ + AYSTAR_INVALID_NODE = -1, +}; + +struct AyStarNode { + TileIndex tile; + Trackdir direction; + uint user_data[2]; +}; + +/* The resulting path has nodes looking like this. */ +struct PathNode { + AyStarNode node; + /* The parent of this item */ + PathNode *parent; +}; + +/* For internal use only + * We do not save the h-value, because it is only needed to calculate the f-value. + * h-value should _always_ be the distance left to the end-tile. */ +struct OpenListNode { + int g; + PathNode path; +}; + +struct AyStar; +/* + * This function is called to check if the end-tile is found + * return values can be: + * AYSTAR_FOUND_END_NODE : indicates this is the end tile + * AYSTAR_DONE : indicates this is not the end tile (or direction was wrong) + */ +/* + * The 2nd parameter should be OpenListNode, and NOT AyStarNode. AyStarNode is + * part of OpenListNode and so it could be accessed without any problems. + * The good part about OpenListNode is, and how AIs use it, that you can + * access the parent of the current node, and so check if you, for example + * don't try to enter the file tile with a 90-degree curve. So please, leave + * this an OpenListNode, it works just fine -- TrueLight + */ +typedef int32 AyStar_EndNodeCheck(AyStar *aystar, OpenListNode *current); + +/* + * This function is called to calculate the G-value for AyStar Algorithm. + * return values can be: + * AYSTAR_INVALID_NODE : indicates an item is not valid (e.g.: unwalkable) + * Any value >= 0 : the g-value for this tile + */ +typedef int32 AyStar_CalculateG(AyStar *aystar, AyStarNode *current, OpenListNode *parent); + +/* + * This function is called to calculate the H-value for AyStar Algorithm. + * Mostly, this must result the distance (Manhattan way) between the + * current point and the end point + * return values can be: + * Any value >= 0 : the h-value for this tile + */ +typedef int32 AyStar_CalculateH(AyStar *aystar, AyStarNode *current, OpenListNode *parent); + +/* + * This function request the tiles around the current tile and put them in tiles_around + * tiles_around is never resetted, so if you are not using directions, just leave it alone. + * Warning: never add more tiles_around than memory allocated for it. + */ +typedef void AyStar_GetNeighbours(AyStar *aystar, OpenListNode *current); + +/* + * If the End Node is found, this function is called. + * It can do, for example, calculate the route and put that in an array + */ +typedef void AyStar_FoundEndNode(AyStar *aystar, OpenListNode *current); + +/* For internal use, see aystar.cpp */ +typedef void AyStar_AddStartNode(AyStar *aystar, AyStarNode *start_node, uint g); +typedef int AyStar_Main(AyStar *aystar); +typedef int AyStar_Loop(AyStar *aystar); +typedef int AyStar_CheckTile(AyStar *aystar, AyStarNode *current, OpenListNode *parent); +typedef void AyStar_Free(AyStar *aystar); +typedef void AyStar_Clear(AyStar *aystar); + +struct AyStar { +/* These fields should be filled before initting the AyStar, but not changed + * afterwards (except for user_data and user_path)! (free and init again to change them) */ + + /* These should point to the application specific routines that do the + * actual work */ + AyStar_CalculateG *CalculateG; + AyStar_CalculateH *CalculateH; + AyStar_GetNeighbours *GetNeighbours; + AyStar_EndNodeCheck *EndNodeCheck; + AyStar_FoundEndNode *FoundEndNode; + + /* These are completely untouched by AyStar, they can be accesed by + * the application specific routines to input and output data. + * user_path should typically contain data about the resulting path + * afterwards, user_target should typically contain information about + * what where looking for, and user_data can contain just about + * everything */ + void *user_path; + void *user_target; + uint user_data[10]; + + /* How many loops are there called before AyStarMain_Main gives + * control back to the caller. 0 = until done */ + byte loops_per_tick; + /* If the g-value goes over this number, it stops searching + * 0 = infinite */ + uint max_path_cost; + /* The maximum amount of nodes that will be expanded, 0 = infinite */ + uint max_search_nodes; + + /* These should be filled with the neighbours of a tile by + * GetNeighbours */ + AyStarNode neighbours[12]; + byte num_neighbours; + + /* These will contain the methods for manipulating the AyStar. Only + * main() should be called externally */ + AyStar_AddStartNode *addstart; + AyStar_Main *main; + AyStar_Loop *loop; + AyStar_Free *free; + AyStar_Clear *clear; + AyStar_CheckTile *checktile; + + /* These will contain the open and closed lists */ + + /* The actual closed list */ + Hash ClosedListHash; + /* The open queue */ + Queue OpenListQueue; + /* An extra hash to speed up the process of looking up an element in + * the open list */ + Hash OpenListHash; +}; + + +int AyStarMain_Main(AyStar *aystar); +void AyStarMain_Clear(AyStar *aystar); + +/* Initialize an AyStar. You should fill all appropriate fields before + * callling init_AyStar (see the declaration of AyStar for which fields are + * internal */ +void init_AyStar(AyStar *aystar, Hash_HashProc hash, uint num_buckets); + + +#endif /* AYSTAR_H */ diff --git a/src/pathfinder/npf/npf.cpp b/src/pathfinder/npf/npf.cpp new file mode 100644 index 000000000..f4211c298 --- /dev/null +++ b/src/pathfinder/npf/npf.cpp @@ -0,0 +1,1120 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file npf.cpp Implementation of the NPF pathfinder. */ + +#include "../../stdafx.h" +#include "../../debug.h" +#include "../../landscape.h" +#include "../../depot_base.h" +#include "../../network/network.h" +#include "../../tunnelbridge_map.h" +#include "../../functions.h" +#include "../../tunnelbridge.h" +#include "../../pbs.h" +#include "../../train.h" +#include "../pathfinder_func.h" +#include "npf.h" + +static AyStar _npf_aystar; + +/* The cost of each trackdir. A diagonal piece is the full NPF_TILE_LENGTH, + * the shorter piece is sqrt(2)/2*NPF_TILE_LENGTH =~ 0.7071 + */ +#define NPF_STRAIGHT_LENGTH (uint)(NPF_TILE_LENGTH * STRAIGHT_TRACK_LENGTH) +static const uint _trackdir_length[TRACKDIR_END] = { + NPF_TILE_LENGTH, NPF_TILE_LENGTH, NPF_STRAIGHT_LENGTH, NPF_STRAIGHT_LENGTH, NPF_STRAIGHT_LENGTH, NPF_STRAIGHT_LENGTH, + 0, 0, + NPF_TILE_LENGTH, NPF_TILE_LENGTH, NPF_STRAIGHT_LENGTH, NPF_STRAIGHT_LENGTH, NPF_STRAIGHT_LENGTH, NPF_STRAIGHT_LENGTH +}; + +/** + * Calculates the minimum distance traveled to get from t0 to t1 when only + * using tracks (ie, only making 45 degree turns). Returns the distance in the + * NPF scale, ie the number of full tiles multiplied by NPF_TILE_LENGTH to + * prevent rounding. + */ +static uint NPFDistanceTrack(TileIndex t0, TileIndex t1) +{ + const uint dx = Delta(TileX(t0), TileX(t1)); + const uint dy = Delta(TileY(t0), TileY(t1)); + + const uint straightTracks = 2 * min(dx, dy); // The number of straight (not full length) tracks + /* OPTIMISATION: + * Original: diagTracks = max(dx, dy) - min(dx,dy); + * Proof: + * (dx+dy) - straightTracks == (min + max) - straightTracks = min + max - 2 * min = max - min */ + const uint diagTracks = dx + dy - straightTracks; // The number of diagonal (full tile length) tracks. + + /* Don't factor out NPF_TILE_LENGTH below, this will round values and lose + * precision */ + return diagTracks * NPF_TILE_LENGTH + straightTracks * NPF_TILE_LENGTH * STRAIGHT_TRACK_LENGTH; +} + + +#if 0 +static uint NTPHash(uint key1, uint key2) +{ + /* This function uses the old hash, which is fixed on 10 bits (1024 buckets) */ + return PATHFIND_HASH_TILE(key1); +} +#endif + +/** + * Calculates a hash value for use in the NPF. + * @param key1 The TileIndex of the tile to hash + * @param key2 The Trackdir of the track on the tile. + * + * @todo Think of a better hash. + */ +static uint NPFHash(uint key1, uint key2) +{ + /* TODO: think of a better hash? */ + uint part1 = TileX(key1) & NPF_HASH_HALFMASK; + uint part2 = TileY(key1) & NPF_HASH_HALFMASK; + + assert(IsValidTrackdir((Trackdir)key2)); + assert(IsValidTile(key1)); + return ((part1 << NPF_HASH_HALFBITS | part2) + (NPF_HASH_SIZE * key2 / TRACKDIR_END)) % NPF_HASH_SIZE; +} + +static int32 NPFCalcZero(AyStar *as, AyStarNode *current, OpenListNode *parent) +{ + return 0; +} + +/* Calcs the heuristic to the target station or tile. For train stations, it + * takes into account the direction of approach. + */ +static int32 NPFCalcStationOrTileHeuristic(AyStar *as, AyStarNode *current, OpenListNode *parent) +{ + NPFFindStationOrTileData *fstd = (NPFFindStationOrTileData*)as->user_target; + NPFFoundTargetData *ftd = (NPFFoundTargetData*)as->user_path; + TileIndex from = current->tile; + TileIndex to = fstd->dest_coords; + uint dist; + + /* for train-stations, we are going to aim for the closest station tile */ + if (as->user_data[NPF_TYPE] == TRANSPORT_RAIL && fstd->station_index != INVALID_STATION) + to = CalcClosestStationTile(fstd->station_index, from); + + if (as->user_data[NPF_TYPE] == TRANSPORT_ROAD) { + /* Since roads only have diagonal pieces, we use manhattan distance here */ + dist = DistanceManhattan(from, to) * NPF_TILE_LENGTH; + } else { + /* Ships and trains can also go diagonal, so the minimum distance is shorter */ + dist = NPFDistanceTrack(from, to); + } + + DEBUG(npf, 4, "Calculating H for: (%d, %d). Result: %d", TileX(current->tile), TileY(current->tile), dist); + + if (dist < ftd->best_bird_dist) { + ftd->best_bird_dist = dist; + ftd->best_trackdir = (Trackdir)current->user_data[NPF_TRACKDIR_CHOICE]; + } + return dist; +} + + +/* Fills AyStarNode.user_data[NPF_TRACKDIRCHOICE] with the chosen direction to + * get here, either getting it from the current choice or from the parent's + * choice */ +static void NPFFillTrackdirChoice(AyStarNode *current, OpenListNode *parent) +{ + if (parent->path.parent == NULL) { + Trackdir trackdir = current->direction; + /* This is a first order decision, so we'd better save the + * direction we chose */ + current->user_data[NPF_TRACKDIR_CHOICE] = trackdir; + DEBUG(npf, 6, "Saving trackdir: 0x%X", trackdir); + } else { + /* We've already made the decision, so just save our parent's decision */ + current->user_data[NPF_TRACKDIR_CHOICE] = parent->path.node.user_data[NPF_TRACKDIR_CHOICE]; + } +} + +/* Will return the cost of the tunnel. If it is an entry, it will return the + * cost of that tile. If the tile is an exit, it will return the tunnel length + * including the exit tile. Requires that this is a Tunnel tile */ +static uint NPFTunnelCost(AyStarNode *current) +{ + DiagDirection exitdir = TrackdirToExitdir(current->direction); + TileIndex tile = current->tile; + if (GetTunnelBridgeDirection(tile) == ReverseDiagDir(exitdir)) { + /* We just popped out if this tunnel, since were + * facing the tunnel exit */ + return NPF_TILE_LENGTH * (GetTunnelBridgeLength(current->tile, GetOtherTunnelEnd(current->tile)) + 1); + /* @todo: Penalty for tunnels? */ + } else { + /* We are entering the tunnel, the enter tile is just a + * straight track */ + return NPF_TILE_LENGTH; + } +} + +static inline uint NPFBridgeCost(AyStarNode *current) +{ + return NPF_TILE_LENGTH * GetTunnelBridgeLength(current->tile, GetOtherBridgeEnd(current->tile)); +} + +static uint NPFSlopeCost(AyStarNode *current) +{ + TileIndex next = current->tile + TileOffsByDiagDir(TrackdirToExitdir(current->direction)); + + /* Get center of tiles */ + int x1 = TileX(current->tile) * TILE_SIZE + TILE_SIZE / 2; + int y1 = TileY(current->tile) * TILE_SIZE + TILE_SIZE / 2; + int x2 = TileX(next) * TILE_SIZE + TILE_SIZE / 2; + int y2 = TileY(next) * TILE_SIZE + TILE_SIZE / 2; + + int dx4 = (x2 - x1) / 4; + int dy4 = (y2 - y1) / 4; + + /* Get the height on both sides of the tile edge. + * Avoid testing the height on the tile-center. This will fail for halftile-foundations. + */ + int z1 = GetSlopeZ(x1 + dx4, y1 + dy4); + int z2 = GetSlopeZ(x2 - dx4, y2 - dy4); + + if (z2 - z1 > 1) { + /* Slope up */ + return _settings_game.pf.npf.npf_rail_slope_penalty; + } + return 0; + /* Should we give a bonus for slope down? Probably not, we + * could just substract that bonus from the penalty, because + * there is only one level of steepness... */ +} + +static uint NPFReservedTrackCost(AyStarNode *current) +{ + TileIndex tile = current->tile; + TrackBits track = TrackToTrackBits(TrackdirToTrack(current->direction)); + TrackBits res = GetReservedTrackbits(tile); + + if (NPFGetFlag(current, NPF_FLAG_3RD_SIGNAL) || ((res & track) == TRACK_BIT_NONE && !TracksOverlap(res | track))) return 0; + + if (IsTileType(tile, MP_TUNNELBRIDGE)) { + DiagDirection exitdir = TrackdirToExitdir(current->direction); + if (GetTunnelBridgeDirection(tile) == ReverseDiagDir(exitdir)) { + return _settings_game.pf.npf.npf_rail_pbs_cross_penalty * (GetTunnelBridgeLength(tile, GetOtherTunnelBridgeEnd(tile)) + 1); + } + } + return _settings_game.pf.npf.npf_rail_pbs_cross_penalty; +} + +/** + * Mark tiles by mowing the grass when npf debug level >= 1. + * Will not work for multiplayer games, since it can (will) cause desyncs. + */ +static void NPFMarkTile(TileIndex tile) +{ +#ifndef NO_DEBUG_MESSAGES + if (_debug_npf_level < 1 || _networking) return; + switch (GetTileType(tile)) { + case MP_RAILWAY: + /* DEBUG: mark visited tiles by mowing the grass under them ;-) */ + if (!IsRailDepot(tile)) { + SetRailGroundType(tile, RAIL_GROUND_BARREN); + MarkTileDirtyByTile(tile); + } + break; + + case MP_ROAD: + if (!IsRoadDepot(tile)) { + SetRoadside(tile, ROADSIDE_BARREN); + MarkTileDirtyByTile(tile); + } + break; + + default: + break; + } +#endif +} + +static int32 NPFWaterPathCost(AyStar *as, AyStarNode *current, OpenListNode *parent) +{ + /* TileIndex tile = current->tile; */ + int32 cost = 0; + Trackdir trackdir = current->direction; + + cost = _trackdir_length[trackdir]; // Should be different for diagonal tracks + + if (IsBuoyTile(current->tile) && IsDiagonalTrackdir(trackdir)) + cost += _settings_game.pf.npf.npf_buoy_penalty; // A small penalty for going over buoys + + if (current->direction != NextTrackdir((Trackdir)parent->path.node.direction)) + cost += _settings_game.pf.npf.npf_water_curve_penalty; + + /* @todo More penalties? */ + + return cost; +} + +/* Determine the cost of this node, for road tracks */ +static int32 NPFRoadPathCost(AyStar *as, AyStarNode *current, OpenListNode *parent) +{ + TileIndex tile = current->tile; + int32 cost = 0; + + /* Determine base length */ + switch (GetTileType(tile)) { + case MP_TUNNELBRIDGE: + cost = IsTunnel(tile) ? NPFTunnelCost(current) : NPFBridgeCost(current); + break; + + case MP_ROAD: + cost = NPF_TILE_LENGTH; + /* Increase the cost for level crossings */ + if (IsLevelCrossing(tile)) cost += _settings_game.pf.npf.npf_crossing_penalty; + break; + + case MP_STATION: + cost = NPF_TILE_LENGTH; + /* Increase the cost for drive-through road stops */ + if (IsDriveThroughStopTile(tile)) cost += _settings_game.pf.npf.npf_road_drive_through_penalty; + break; + + default: + break; + } + + /* Determine extra costs */ + + /* Check for slope */ + cost += NPFSlopeCost(current); + + /* Check for turns. Road vehicles only really drive diagonal, turns are + * represented by non-diagonal tracks */ + if (!IsDiagonalTrackdir(current->direction)) + cost += _settings_game.pf.npf.npf_road_curve_penalty; + + NPFMarkTile(tile); + DEBUG(npf, 4, "Calculating G for: (%d, %d). Result: %d", TileX(current->tile), TileY(current->tile), cost); + return cost; +} + + +/* Determine the cost of this node, for railway tracks */ +static int32 NPFRailPathCost(AyStar *as, AyStarNode *current, OpenListNode *parent) +{ + TileIndex tile = current->tile; + Trackdir trackdir = current->direction; + int32 cost = 0; + /* HACK: We create a OpenListNode manually, so we can call EndNodeCheck */ + OpenListNode new_node; + + /* Determine base length */ + switch (GetTileType(tile)) { + case MP_TUNNELBRIDGE: + cost = IsTunnel(tile) ? NPFTunnelCost(current) : NPFBridgeCost(current); + break; + + case MP_RAILWAY: + cost = _trackdir_length[trackdir]; // Should be different for diagonal tracks + break; + + case MP_ROAD: // Railway crossing + cost = NPF_TILE_LENGTH; + break; + + case MP_STATION: + /* We give a station tile a penalty. Logically we would only want to give + * station tiles that are not our destination this penalty. This would + * discourage trains to drive through busy stations. But, we can just + * give any station tile a penalty, because every possible route will get + * this penalty exactly once, on its end tile (if it's a station) and it + * will therefore not make a difference. */ + cost = NPF_TILE_LENGTH + _settings_game.pf.npf.npf_rail_station_penalty; + break; + + default: + break; + } + + /* Determine extra costs */ + + /* Check for signals */ + if (IsTileType(tile, MP_RAILWAY)) { + if (HasSignalOnTrackdir(tile, trackdir)) { + /* Ordinary track with signals */ + if (GetSignalStateByTrackdir(tile, trackdir) == SIGNAL_STATE_RED) { + /* Signal facing us is red */ + if (!NPFGetFlag(current, NPF_FLAG_SEEN_SIGNAL)) { + /* Penalize the first signal we + * encounter, if it is red */ + + /* Is this a presignal exit or combo? */ + SignalType sigtype = GetSignalType(tile, TrackdirToTrack(trackdir)); + if (!IsPbsSignal(sigtype)) { + if (sigtype == SIGTYPE_EXIT || sigtype == SIGTYPE_COMBO) { + /* Penalise exit and combo signals differently (heavier) */ + cost += _settings_game.pf.npf.npf_rail_firstred_exit_penalty; + } else { + cost += _settings_game.pf.npf.npf_rail_firstred_penalty; + } + } + } + /* Record the state of this signal */ + NPFSetFlag(current, NPF_FLAG_LAST_SIGNAL_RED, true); + } else { + /* Record the state of this signal */ + NPFSetFlag(current, NPF_FLAG_LAST_SIGNAL_RED, false); + } + if (NPFGetFlag(current, NPF_FLAG_SEEN_SIGNAL)) { + if (NPFGetFlag(current, NPF_FLAG_2ND_SIGNAL)) { + NPFSetFlag(current, NPF_FLAG_3RD_SIGNAL, true); + } else { + NPFSetFlag(current, NPF_FLAG_2ND_SIGNAL, true); + } + } else { + NPFSetFlag(current, NPF_FLAG_SEEN_SIGNAL, true); + } + } + + if (HasPbsSignalOnTrackdir(tile, ReverseTrackdir(trackdir)) && !NPFGetFlag(current, NPF_FLAG_3RD_SIGNAL)) { + cost += _settings_game.pf.npf.npf_rail_pbs_signal_back_penalty; + } + } + + /* Penalise the tile if it is a target tile and the last signal was + * red */ + /* HACK: We create a new_node here so we can call EndNodeCheck. Ugly as hell + * of course... */ + new_node.path.node = *current; + if (as->EndNodeCheck(as, &new_node) == AYSTAR_FOUND_END_NODE && NPFGetFlag(current, NPF_FLAG_LAST_SIGNAL_RED)) + cost += _settings_game.pf.npf.npf_rail_lastred_penalty; + + /* Check for slope */ + cost += NPFSlopeCost(current); + + /* Check for turns */ + if (current->direction != NextTrackdir((Trackdir)parent->path.node.direction)) + cost += _settings_game.pf.npf.npf_rail_curve_penalty; + /* TODO, with realistic acceleration, also the amount of straight track between + * curves should be taken into account, as this affects the speed limit. */ + + /* Check for reverse in depot */ + if (IsRailDepotTile(tile) && as->EndNodeCheck(as, &new_node) != AYSTAR_FOUND_END_NODE) { + /* Penalise any depot tile that is not the last tile in the path. This + * _should_ penalise every occurence of reversing in a depot (and only + * that) */ + cost += _settings_game.pf.npf.npf_rail_depot_reverse_penalty; + } + + /* Check for occupied track */ + cost += NPFReservedTrackCost(current); + + NPFMarkTile(tile); + DEBUG(npf, 4, "Calculating G for: (%d, %d). Result: %d", TileX(current->tile), TileY(current->tile), cost); + return cost; +} + +/* Will find any depot */ +static int32 NPFFindDepot(AyStar *as, OpenListNode *current) +{ + /* It's not worth caching the result with NPF_FLAG_IS_TARGET here as below, + * since checking the cache not that much faster than the actual check */ + return IsDepotTypeTile(current->path.node.tile, (TransportType)as->user_data[NPF_TYPE]) ? + AYSTAR_FOUND_END_NODE : AYSTAR_DONE; +} + +/** Find any safe and free tile. */ +static int32 NPFFindSafeTile(AyStar *as, OpenListNode *current) +{ + const Train *v = Train::From(((NPFFindStationOrTileData *)as->user_target)->v); + + return + IsSafeWaitingPosition(v, current->path.node.tile, current->path.node.direction, true, _settings_game.pf.forbid_90_deg) && + IsWaitingPositionFree(v, current->path.node.tile, current->path.node.direction, _settings_game.pf.forbid_90_deg) ? + AYSTAR_FOUND_END_NODE : AYSTAR_DONE; +} + +/* Will find a station identified using the NPFFindStationOrTileData */ +static int32 NPFFindStationOrTile(AyStar *as, OpenListNode *current) +{ + NPFFindStationOrTileData *fstd = (NPFFindStationOrTileData*)as->user_target; + AyStarNode *node = ¤t->path.node; + TileIndex tile = node->tile; + + /* If GetNeighbours said we could get here, we assume the station type + * is correct */ + if ( + (fstd->station_index == INVALID_STATION && tile == fstd->dest_coords) || // We've found the tile, or + (IsTileType(tile, MP_STATION) && GetStationIndex(tile) == fstd->station_index) // the station + ) { + return AYSTAR_FOUND_END_NODE; + } else { + return AYSTAR_DONE; + } +} + +/** + * Find the node containing the first signal on the path. + * + * If the first signal is on the very first two tiles of the path, + * the second signal is returnd. If no suitable signal is present, the + * last node of the path is returned. + */ +static const PathNode *FindSafePosition(PathNode *path, const Train *v) +{ + /* If there is no signal, reserve the whole path. */ + PathNode *sig = path; + + for (; path->parent != NULL; path = path->parent) { + if (IsSafeWaitingPosition(v, path->node.tile, path->node.direction, true, _settings_game.pf.forbid_90_deg)) { + sig = path; + } + } + + return sig; +} + +/** + * Lift the reservation of the tiles from @p start till @p end, excluding @p end itself. + */ +static void ClearPathReservation(const PathNode *start, const PathNode *end) +{ + bool first_run = true; + for (; start != end; start = start->parent) { + if (IsRailStationTile(start->node.tile) && first_run) { + SetRailStationPlatformReservation(start->node.tile, TrackdirToExitdir(start->node.direction), false); + } else { + UnreserveRailTrack(start->node.tile, TrackdirToTrack(start->node.direction)); + } + first_run = false; + } +} + +/** + * To be called when @p current contains the (shortest route to) the target node. + * Will fill the contents of the NPFFoundTargetData using + * AyStarNode[NPF_TRACKDIR_CHOICE]. If requested, path reservation + * is done here. + */ +static void NPFSaveTargetData(AyStar *as, OpenListNode *current) +{ + NPFFoundTargetData *ftd = (NPFFoundTargetData*)as->user_path; + ftd->best_trackdir = (Trackdir)current->path.node.user_data[NPF_TRACKDIR_CHOICE]; + ftd->best_path_dist = current->g; + ftd->best_bird_dist = 0; + ftd->node = current->path.node; + ftd->res_okay = false; + + if (as->user_target != NULL && ((NPFFindStationOrTileData*)as->user_target)->reserve_path && as->user_data[NPF_TYPE] == TRANSPORT_RAIL) { + /* Path reservation is requested. */ + const Train *v = Train::From(((NPFFindStationOrTileData *)as->user_target)->v); + + const PathNode *target = FindSafePosition(¤t->path, v); + ftd->node = target->node; + + /* If the target is a station skip to platform end. */ + if (IsRailStationTile(target->node.tile)) { + DiagDirection dir = TrackdirToExitdir(target->node.direction); + uint len = Station::GetByTile(target->node.tile)->GetPlatformLength(target->node.tile, dir); + TileIndex end_tile = TILE_ADD(target->node.tile, (len - 1) * TileOffsByDiagDir(dir)); + + /* Update only end tile, trackdir of a station stays the same. */ + ftd->node.tile = end_tile; + if (!IsWaitingPositionFree(v, end_tile, target->node.direction, _settings_game.pf.forbid_90_deg)) return; + SetRailStationPlatformReservation(target->node.tile, dir, true); + SetRailStationReservation(target->node.tile, false); + } else { + if (!IsWaitingPositionFree(v, target->node.tile, target->node.direction, _settings_game.pf.forbid_90_deg)) return; + } + + for (const PathNode *cur = target; cur->parent != NULL; cur = cur->parent) { + if (!TryReserveRailTrack(cur->node.tile, TrackdirToTrack(cur->node.direction))) { + /* Reservation failed, undo. */ + ClearPathReservation(target, cur); + return; + } + } + + ftd->res_okay = true; + } +} + +/** + * Finds out if a given company's vehicles are allowed to enter a given tile. + * @param owner The owner of the vehicle. + * @param tile The tile that is about to be entered. + * @param enterdir The direction in which the vehicle wants to enter the tile. + * @return true if the vehicle can enter the tile. + * @todo This function should be used in other places than just NPF, + * maybe moved to another file too. + */ +static bool CanEnterTileOwnerCheck(Owner owner, TileIndex tile, DiagDirection enterdir) +{ + if (IsTileType(tile, MP_RAILWAY) || // Rail tile (also rail depot) + HasStationTileRail(tile) || // Rail station tile/waypoint + IsRoadDepotTile(tile) || // Road depot tile + IsStandardRoadStopTile(tile)) { // Road station tile (but not drive-through stops) + return IsTileOwner(tile, owner); // You need to own these tiles entirely to use them + } + + switch (GetTileType(tile)) { + case MP_ROAD: + /* rail-road crossing : are we looking at the railway part? */ + if (IsLevelCrossing(tile) && + DiagDirToAxis(enterdir) != GetCrossingRoadAxis(tile)) { + return IsTileOwner(tile, owner); // Railway needs owner check, while the street is public + } + break; + + case MP_TUNNELBRIDGE: + if (GetTunnelBridgeTransportType(tile) == TRANSPORT_RAIL) { + return IsTileOwner(tile, owner); + } + break; + + default: + break; + } + + return true; // no need to check +} + + +/** + * Returns the direction the exit of the depot on the given tile is facing. + */ +static DiagDirection GetDepotDirection(TileIndex tile, TransportType type) +{ + assert(IsDepotTypeTile(tile, type)); + + switch (type) { + case TRANSPORT_RAIL: return GetRailDepotDirection(tile); + case TRANSPORT_ROAD: return GetRoadDepotDirection(tile); + case TRANSPORT_WATER: return GetShipDepotDirection(tile); + default: return INVALID_DIAGDIR; // Not reached + } +} + +/** Tests if a tile is a road tile with a single tramtrack (tram can reverse) */ +static DiagDirection GetSingleTramBit(TileIndex tile) +{ + if (IsNormalRoadTile(tile)) { + RoadBits rb = GetRoadBits(tile, ROADTYPE_TRAM); + switch (rb) { + case ROAD_NW: return DIAGDIR_NW; + case ROAD_SW: return DIAGDIR_SW; + case ROAD_SE: return DIAGDIR_SE; + case ROAD_NE: return DIAGDIR_NE; + default: break; + } + } + return INVALID_DIAGDIR; +} + +/** + * Tests if a tile can be entered or left only from one side. + * + * Depots, non-drive-through roadstops, and tiles with single trambits are tested. + * + * @param tile The tile of interest. + * @param type The transporttype of the vehicle. + * @param subtype For TRANSPORT_ROAD the compatible RoadTypes of the vehicle. + * @return The single entry/exit-direction of the tile, or INVALID_DIAGDIR if there are more or less directions + */ +static DiagDirection GetTileSingleEntry(TileIndex tile, TransportType type, uint subtype) +{ + if (type != TRANSPORT_WATER && IsDepotTypeTile(tile, type)) return GetDepotDirection(tile, type); + + if (type == TRANSPORT_ROAD) { + if (IsStandardRoadStopTile(tile)) return GetRoadStopDir(tile); + if (HasBit(subtype, ROADTYPE_TRAM)) return GetSingleTramBit(tile); + } + + return INVALID_DIAGDIR; +} + +/** + * Tests if a vehicle must reverse on a tile. + * + * @param tile The tile of interest. + * @param dir The direction in which the vehicle drives on a tile. + * @param type The transporttype of the vehicle. + * @param subtype For TRANSPORT_ROAD the compatible RoadTypes of the vehicle. + * @return true iff the vehicle must reverse on the tile. + */ +static inline bool ForceReverse(TileIndex tile, DiagDirection dir, TransportType type, uint subtype) +{ + DiagDirection single_entry = GetTileSingleEntry(tile, type, subtype); + return single_entry != INVALID_DIAGDIR && single_entry != dir; +} + +/** + * Tests if a vehicle can enter a tile. + * + * @param tile The tile of interest. + * @param dir The direction in which the vehicle drives onto a tile. + * @param type The transporttype of the vehicle. + * @param subtype For TRANSPORT_ROAD the compatible RoadTypes of the vehicle. + * @param railtypes For TRANSPORT_RAIL the compatible RailTypes of the vehicle. + * @param owner The owner of the vehicle. + * @return true iff the vehicle can enter the tile. + */ +static bool CanEnterTile(TileIndex tile, DiagDirection dir, TransportType type, uint subtype, RailTypes railtypes, Owner owner) +{ + /* Check tunnel entries and bridge ramps */ + if (IsTileType(tile, MP_TUNNELBRIDGE) && GetTunnelBridgeDirection(tile) != dir) return false; + + /* Test ownership */ + if (!CanEnterTileOwnerCheck(owner, tile, dir)) return false; + + /* check correct rail type (mono, maglev, etc) */ + if (type == TRANSPORT_RAIL) { + RailType rail_type = GetTileRailType(tile); + if (!HasBit(railtypes, rail_type)) return false; + } + + /* Depots, standard roadstops and single tram bits can only be entered from one direction */ + DiagDirection single_entry = GetTileSingleEntry(tile, type, subtype); + if (single_entry != INVALID_DIAGDIR && single_entry != ReverseDiagDir(dir)) return false; + + return true; +} + +/** + * Returns the driveable Trackdirs on a tile. + * + * One-way-roads are taken into account. Signals are not tested. + * + * @param dst_tile The tile of interest. + * @param src_trackdir The direction the vehicle is currently moving. + * @param type The transporttype of the vehicle. + * @param subtype For TRANSPORT_ROAD the compatible RoadTypes of the vehicle. + * @return The Trackdirs the vehicle can continue moving on. + */ +static TrackdirBits GetDriveableTrackdirBits(TileIndex dst_tile, Trackdir src_trackdir, TransportType type, uint subtype) +{ + TrackdirBits trackdirbits = TrackStatusToTrackdirBits(GetTileTrackStatus(dst_tile, type, subtype)); + + if (trackdirbits == 0 && type == TRANSPORT_ROAD && HasBit(subtype, ROADTYPE_TRAM)) { + /* GetTileTrackStatus() returns 0 for single tram bits. + * As we cannot change it there (easily) without breaking something, change it here */ + switch (GetSingleTramBit(dst_tile)) { + case DIAGDIR_NE: + case DIAGDIR_SW: + trackdirbits = TRACKDIR_BIT_X_NE | TRACKDIR_BIT_X_SW; + break; + + case DIAGDIR_NW: + case DIAGDIR_SE: + trackdirbits = TRACKDIR_BIT_Y_NW | TRACKDIR_BIT_Y_SE; + break; + + default: break; + } + } + + DEBUG(npf, 4, "Next node: (%d, %d) [%d], possible trackdirs: 0x%X", TileX(dst_tile), TileY(dst_tile), dst_tile, trackdirbits); + + /* Select only trackdirs we can reach from our current trackdir */ + trackdirbits &= TrackdirReachesTrackdirs(src_trackdir); + + /* Filter out trackdirs that would make 90 deg turns for trains */ + if (_settings_game.pf.forbid_90_deg && (type == TRANSPORT_RAIL || type == TRANSPORT_WATER)) trackdirbits &= ~TrackdirCrossesTrackdirs(src_trackdir); + + DEBUG(npf, 6, "After filtering: (%d, %d), possible trackdirs: 0x%X", TileX(dst_tile), TileY(dst_tile), trackdirbits); + + return trackdirbits; +} + + +/* Will just follow the results of GetTileTrackStatus concerning where we can + * go and where not. Uses AyStar.user_data[NPF_TYPE] as the transport type and + * an argument to GetTileTrackStatus. Will skip tunnels, meaning that the + * entry and exit are neighbours. Will fill + * AyStarNode.user_data[NPF_TRACKDIR_CHOICE] with an appropriate value, and + * copy AyStarNode.user_data[NPF_NODE_FLAGS] from the parent */ +static void NPFFollowTrack(AyStar *aystar, OpenListNode *current) +{ + /* We leave src_tile on track src_trackdir in direction src_exitdir */ + Trackdir src_trackdir = current->path.node.direction; + TileIndex src_tile = current->path.node.tile; + DiagDirection src_exitdir = TrackdirToExitdir(src_trackdir); + + /* Is src_tile valid, and can be used? + * When choosing track on a junction src_tile is the tile neighboured to the junction wrt. exitdir. + * But we must not check the validity of this move, as src_tile is totally unrelated to the move, if a roadvehicle reversed on a junction. */ + bool ignore_src_tile = (current->path.parent == NULL && NPFGetFlag(¤t->path.node, NPF_FLAG_IGNORE_START_TILE)); + + /* Information about the vehicle: TransportType (road/rail/water) and SubType (compatible rail/road types) */ + TransportType type = (TransportType)aystar->user_data[NPF_TYPE]; + uint subtype = aystar->user_data[NPF_SUB_TYPE]; + + /* Initialize to 0, so we can jump out (return) somewhere an have no neighbours */ + aystar->num_neighbours = 0; + DEBUG(npf, 4, "Expanding: (%d, %d, %d) [%d]", TileX(src_tile), TileY(src_tile), src_trackdir, src_tile); + + /* We want to determine the tile we arrive, and which choices we have there */ + TileIndex dst_tile; + TrackdirBits trackdirbits; + + /* Find dest tile */ + if (ignore_src_tile) { + /* Do not perform any checks that involve src_tile */ + dst_tile = src_tile + TileOffsByDiagDir(src_exitdir); + trackdirbits = GetDriveableTrackdirBits(dst_tile, src_trackdir, type, subtype); + } else if (IsTileType(src_tile, MP_TUNNELBRIDGE) && GetTunnelBridgeDirection(src_tile) == src_exitdir) { + /* We drive through the wormhole and arrive on the other side */ + dst_tile = GetOtherTunnelBridgeEnd(src_tile); + trackdirbits = TrackdirToTrackdirBits(src_trackdir); + } else if (ForceReverse(src_tile, src_exitdir, type, subtype)) { + /* We can only reverse on this tile */ + dst_tile = src_tile; + src_trackdir = ReverseTrackdir(src_trackdir); + trackdirbits = TrackdirToTrackdirBits(src_trackdir); + } else { + /* We leave src_tile in src_exitdir and reach dst_tile */ + dst_tile = AddTileIndexDiffCWrap(src_tile, TileIndexDiffCByDiagDir(src_exitdir)); + + if (dst_tile != INVALID_TILE && !CanEnterTile(dst_tile, src_exitdir, type, subtype, (RailTypes)aystar->user_data[NPF_RAILTYPES], (Owner)aystar->user_data[NPF_OWNER])) dst_tile = INVALID_TILE; + + if (dst_tile == INVALID_TILE) { + /* We cannot enter the next tile. Road vehicles can reverse, others reach dead end */ + if (type != TRANSPORT_ROAD || HasBit(subtype, ROADTYPE_TRAM)) return; + + dst_tile = src_tile; + src_trackdir = ReverseTrackdir(src_trackdir); + } + + trackdirbits = GetDriveableTrackdirBits(dst_tile, src_trackdir, type, subtype); + + if (trackdirbits == 0) { + /* We cannot enter the next tile. Road vehicles can reverse, others reach dead end */ + if (type != TRANSPORT_ROAD || HasBit(subtype, ROADTYPE_TRAM)) return; + + dst_tile = src_tile; + src_trackdir = ReverseTrackdir(src_trackdir); + + trackdirbits = GetDriveableTrackdirBits(dst_tile, src_trackdir, type, subtype); + } + } + + if (NPFGetFlag(¤t->path.node, NPF_FLAG_IGNORE_RESERVED)) { + /* Mask out any reserved tracks. */ + TrackBits reserved = GetReservedTrackbits(dst_tile); + trackdirbits &= ~TrackBitsToTrackdirBits(reserved); + + uint bits = TrackdirBitsToTrackBits(trackdirbits); + int i; + FOR_EACH_SET_BIT(i, bits) { + if (TracksOverlap(reserved | TrackToTrackBits((Track)i))) trackdirbits &= ~TrackToTrackdirBits((Track)i); + } + } + + /* Enumerate possible track */ + uint i = 0; + while (trackdirbits != 0) { + Trackdir dst_trackdir = RemoveFirstTrackdir(&trackdirbits); + DEBUG(npf, 5, "Expanded into trackdir: %d, remaining trackdirs: 0x%X", dst_trackdir, trackdirbits); + + /* Tile with signals? */ + if (IsTileType(dst_tile, MP_RAILWAY) && GetRailTileType(dst_tile) == RAIL_TILE_SIGNALS) { + if (HasSignalOnTrackdir(dst_tile, ReverseTrackdir(dst_trackdir)) && !HasSignalOnTrackdir(dst_tile, dst_trackdir) && IsOnewaySignal(dst_tile, TrackdirToTrack(dst_trackdir))) + /* If there's a one-way signal not pointing towards us, stop going in this direction. */ + break; + } + { + /* We've found ourselves a neighbour :-) */ + AyStarNode *neighbour = &aystar->neighbours[i]; + neighbour->tile = dst_tile; + neighbour->direction = dst_trackdir; + /* Save user data */ + neighbour->user_data[NPF_NODE_FLAGS] = current->path.node.user_data[NPF_NODE_FLAGS]; + NPFFillTrackdirChoice(neighbour, current); + } + i++; + } + aystar->num_neighbours = i; +} + +/* + * Plan a route to the specified target (which is checked by target_proc), + * from start1 and if not NULL, from start2 as well. The type of transport we + * are checking is in type. reverse_penalty is applied to all routes that + * originate from the second start node. + * When we are looking for one specific target (optionally multiple tiles), we + * should use a good heuristic to perform aystar search. When we search for + * multiple targets that are spread around, we should perform a breadth first + * search by specifiying CalcZero as our heuristic. + */ +static NPFFoundTargetData NPFRouteInternal(AyStarNode *start1, bool ignore_start_tile1, AyStarNode *start2, bool ignore_start_tile2, NPFFindStationOrTileData *target, AyStar_EndNodeCheck target_proc, AyStar_CalculateH heuristic_proc, TransportType type, uint sub_type, Owner owner, RailTypes railtypes, uint reverse_penalty) +{ + int r; + NPFFoundTargetData result; + + /* Initialize procs */ + _npf_aystar.CalculateH = heuristic_proc; + _npf_aystar.EndNodeCheck = target_proc; + _npf_aystar.FoundEndNode = NPFSaveTargetData; + _npf_aystar.GetNeighbours = NPFFollowTrack; + switch (type) { + default: NOT_REACHED(); + case TRANSPORT_RAIL: _npf_aystar.CalculateG = NPFRailPathCost; break; + case TRANSPORT_ROAD: _npf_aystar.CalculateG = NPFRoadPathCost; break; + case TRANSPORT_WATER: _npf_aystar.CalculateG = NPFWaterPathCost; break; + } + + /* Initialize Start Node(s) */ + start1->user_data[NPF_TRACKDIR_CHOICE] = INVALID_TRACKDIR; + start1->user_data[NPF_NODE_FLAGS] = 0; + NPFSetFlag(start1, NPF_FLAG_IGNORE_START_TILE, ignore_start_tile1); + _npf_aystar.addstart(&_npf_aystar, start1, 0); + if (start2) { + start2->user_data[NPF_TRACKDIR_CHOICE] = INVALID_TRACKDIR; + start2->user_data[NPF_NODE_FLAGS] = 0; + NPFSetFlag(start2, NPF_FLAG_IGNORE_START_TILE, ignore_start_tile2); + NPFSetFlag(start2, NPF_FLAG_REVERSE, true); + _npf_aystar.addstart(&_npf_aystar, start2, reverse_penalty); + } + + /* Initialize result */ + result.best_bird_dist = UINT_MAX; + result.best_path_dist = UINT_MAX; + result.best_trackdir = INVALID_TRACKDIR; + result.node.tile = INVALID_TILE; + result.res_okay = false; + _npf_aystar.user_path = &result; + + /* Initialize target */ + _npf_aystar.user_target = target; + + /* Initialize user_data */ + _npf_aystar.user_data[NPF_TYPE] = type; + _npf_aystar.user_data[NPF_SUB_TYPE] = sub_type; + _npf_aystar.user_data[NPF_OWNER] = owner; + _npf_aystar.user_data[NPF_RAILTYPES] = railtypes; + + /* GO! */ + r = AyStarMain_Main(&_npf_aystar); + assert(r != AYSTAR_STILL_BUSY); + + if (result.best_bird_dist != 0) { + if (target != NULL) { + DEBUG(npf, 1, "Could not find route to tile 0x%X from 0x%X.", target->dest_coords, start1->tile); + } else { + /* Assumption: target == NULL, so we are looking for a depot */ + DEBUG(npf, 1, "Could not find route to a depot from tile 0x%X.", start1->tile); + } + + } + return result; +} + +NPFFoundTargetData NPFRouteToStationOrTileTwoWay(TileIndex tile1, Trackdir trackdir1, bool ignore_start_tile1, TileIndex tile2, Trackdir trackdir2, bool ignore_start_tile2, NPFFindStationOrTileData *target, TransportType type, uint sub_type, Owner owner, RailTypes railtypes) +{ + AyStarNode start1; + AyStarNode start2; + + start1.tile = tile1; + start2.tile = tile2; + /* We set this in case the target is also the start tile, we will just + * return a not found then */ + start1.user_data[NPF_TRACKDIR_CHOICE] = INVALID_TRACKDIR; + start1.direction = trackdir1; + start2.direction = trackdir2; + start2.user_data[NPF_TRACKDIR_CHOICE] = INVALID_TRACKDIR; + + return NPFRouteInternal(&start1, ignore_start_tile1, (IsValidTile(tile2) ? &start2 : NULL), ignore_start_tile2, target, NPFFindStationOrTile, NPFCalcStationOrTileHeuristic, type, sub_type, owner, railtypes, 0); +} + +NPFFoundTargetData NPFRouteToStationOrTile(TileIndex tile, Trackdir trackdir, bool ignore_start_tile, NPFFindStationOrTileData *target, TransportType type, uint sub_type, Owner owner, RailTypes railtypes) +{ + return NPFRouteToStationOrTileTwoWay(tile, trackdir, ignore_start_tile, INVALID_TILE, INVALID_TRACKDIR, false, target, type, sub_type, owner, railtypes); +} + +NPFFoundTargetData NPFRouteToDepotBreadthFirstTwoWay(TileIndex tile1, Trackdir trackdir1, bool ignore_start_tile1, TileIndex tile2, Trackdir trackdir2, bool ignore_start_tile2, TransportType type, uint sub_type, Owner owner, RailTypes railtypes, uint reverse_penalty) +{ + AyStarNode start1; + AyStarNode start2; + + start1.tile = tile1; + start2.tile = tile2; + /* We set this in case the target is also the start tile, we will just + * return a not found then */ + start1.user_data[NPF_TRACKDIR_CHOICE] = INVALID_TRACKDIR; + start1.direction = trackdir1; + start2.direction = trackdir2; + start2.user_data[NPF_TRACKDIR_CHOICE] = INVALID_TRACKDIR; + + /* perform a breadth first search. Target is NULL, + * since we are just looking for any depot...*/ + return NPFRouteInternal(&start1, ignore_start_tile1, (IsValidTile(tile2) ? &start2 : NULL), ignore_start_tile2, NULL, NPFFindDepot, NPFCalcZero, type, sub_type, owner, railtypes, reverse_penalty); +} + +NPFFoundTargetData NPFRouteToDepotBreadthFirst(TileIndex tile, Trackdir trackdir, bool ignore_start_tile, TransportType type, uint sub_type, Owner owner, RailTypes railtypes) +{ + return NPFRouteToDepotBreadthFirstTwoWay(tile, trackdir, ignore_start_tile, INVALID_TILE, INVALID_TRACKDIR, false, type, sub_type, owner, railtypes, 0); +} + +NPFFoundTargetData NPFRouteToDepotTrialError(TileIndex tile, Trackdir trackdir, bool ignore_start_tile, TransportType type, uint sub_type, Owner owner, RailTypes railtypes) +{ + /* Okay, what we're gonna do. First, we look at all depots, calculate + * the manhatten distance to get to each depot. We then sort them by + * distance. We start by trying to plan a route to the closest, then + * the next closest, etc. We stop when the best route we have found so + * far, is shorter than the manhattan distance. This will obviously + * always find the closest depot. It will probably be most efficient + * for ships, since the heuristic will not be to far off then. I hope. + */ + Queue depots; + int r; + NPFFoundTargetData best_result = {UINT_MAX, UINT_MAX, INVALID_TRACKDIR, {INVALID_TILE, INVALID_TRACKDIR, {0, 0}}, false}; + NPFFoundTargetData result; + NPFFindStationOrTileData target; + AyStarNode start; + Depot *current; + Depot *depot; + + init_InsSort(&depots); + /* Okay, let's find all depots that we can use first */ + FOR_ALL_DEPOTS(depot) { + /* Check if this is really a valid depot, it is of the needed type and + * owner */ + if (IsDepotTypeTile(depot->xy, type) && IsTileOwner(depot->xy, owner)) + /* If so, let's add it to the queue, sorted by distance */ + depots.push(&depots, depot, DistanceManhattan(tile, depot->xy)); + } + + /* Now, let's initialise the aystar */ + + /* Initialize procs */ + _npf_aystar.CalculateH = NPFCalcStationOrTileHeuristic; + _npf_aystar.EndNodeCheck = NPFFindStationOrTile; + _npf_aystar.FoundEndNode = NPFSaveTargetData; + _npf_aystar.GetNeighbours = NPFFollowTrack; + switch (type) { + default: NOT_REACHED(); + case TRANSPORT_RAIL: _npf_aystar.CalculateG = NPFRailPathCost; break; + case TRANSPORT_ROAD: _npf_aystar.CalculateG = NPFRoadPathCost; break; + case TRANSPORT_WATER: _npf_aystar.CalculateG = NPFWaterPathCost; break; + } + + /* Initialize target */ + target.station_index = INVALID_STATION; // We will initialize dest_coords inside the loop below + _npf_aystar.user_target = ⌖ + + /* Initialize user_data */ + _npf_aystar.user_data[NPF_TYPE] = type; + _npf_aystar.user_data[NPF_SUB_TYPE] = sub_type; + _npf_aystar.user_data[NPF_OWNER] = owner; + + /* Initialize Start Node */ + start.tile = tile; + start.direction = trackdir; // We will initialize user_data inside the loop below + + /* Initialize Result */ + _npf_aystar.user_path = &result; + best_result.best_path_dist = UINT_MAX; + best_result.best_bird_dist = UINT_MAX; + + /* Just iterate the depots in order of increasing distance */ + while ((current = (Depot*)depots.pop(&depots))) { + /* Check to see if we already have a path shorter than this + * depot's manhattan distance. HACK: We call DistanceManhattan + * again, we should probably modify the queue to give us that + * value... */ + if ( DistanceManhattan(tile, current->xy * NPF_TILE_LENGTH) > best_result.best_path_dist) + break; + + /* Initialize Start Node + * We set this in case the target is also the start tile, we will just + * return a not found then */ + start.user_data[NPF_TRACKDIR_CHOICE] = INVALID_TRACKDIR; + start.user_data[NPF_NODE_FLAGS] = 0; + NPFSetFlag(&start, NPF_FLAG_IGNORE_START_TILE, ignore_start_tile); + _npf_aystar.addstart(&_npf_aystar, &start, 0); + + /* Initialize result */ + result.best_bird_dist = UINT_MAX; + result.best_path_dist = UINT_MAX; + result.best_trackdir = INVALID_TRACKDIR; + + /* Initialize target */ + target.dest_coords = current->xy; + + /* GO! */ + r = AyStarMain_Main(&_npf_aystar); + assert(r != AYSTAR_STILL_BUSY); + + /* This depot is closer */ + if (result.best_path_dist < best_result.best_path_dist) + best_result = result; + } + if (result.best_bird_dist != 0) { + DEBUG(npf, 1, "Could not find route to any depot from tile 0x%X.", tile); + } + return best_result; +} + +NPFFoundTargetData NPFRouteToSafeTile(const Train *v, TileIndex tile, Trackdir trackdir, bool override_railtype) +{ + assert(v->type == VEH_TRAIN); + + NPFFindStationOrTileData fstd; + fstd.v = v; + fstd.reserve_path = true; + + AyStarNode start1; + start1.tile = tile; + /* We set this in case the target is also the start tile, we will just + * return a not found then */ + start1.user_data[NPF_TRACKDIR_CHOICE] = INVALID_TRACKDIR; + start1.direction = trackdir; + NPFSetFlag(&start1, NPF_FLAG_IGNORE_RESERVED, true); + + RailTypes railtypes = v->compatible_railtypes; + if (override_railtype) railtypes |= GetRailTypeInfo(v->railtype)->compatible_railtypes; + + /* perform a breadth first search. Target is NULL, + * since we are just looking for any safe tile...*/ + return NPFRouteInternal(&start1, true, NULL, false, &fstd, NPFFindSafeTile, NPFCalcZero, TRANSPORT_RAIL, 0, v->owner, railtypes, 0); +} + +void InitializeNPF() +{ + static bool first_init = true; + if (first_init) { + first_init = false; + init_AyStar(&_npf_aystar, NPFHash, NPF_HASH_SIZE); + } else { + AyStarMain_Clear(&_npf_aystar); + } + _npf_aystar.loops_per_tick = 0; + _npf_aystar.max_path_cost = 0; + //_npf_aystar.max_search_nodes = 0; + /* We will limit the number of nodes for now, until we have a better + * solution to really fix performance */ + _npf_aystar.max_search_nodes = _settings_game.pf.npf.npf_max_search_nodes; +} + +void NPFFillWithOrderData(NPFFindStationOrTileData *fstd, Vehicle *v, bool reserve_path) +{ + /* Ships don't really reach their stations, but the tile in front. So don't + * save the station id for ships. For roadvehs we don't store it either, + * because multistop depends on vehicles actually reaching the exact + * dest_tile, not just any stop of that station. + * So only for train orders to stations we fill fstd->station_index, for all + * others only dest_coords */ + if (v->type == VEH_TRAIN && (v->current_order.IsType(OT_GOTO_STATION) || v->current_order.IsType(OT_GOTO_WAYPOINT))) { + fstd->station_index = v->current_order.GetDestination(); + /* Let's take the closest tile of the station as our target for trains */ + fstd->dest_coords = CalcClosestStationTile(fstd->station_index, v->tile); + } else { + fstd->dest_coords = v->dest_tile; + fstd->station_index = INVALID_STATION; + } + fstd->reserve_path = reserve_path; + fstd->v = v; +} diff --git a/src/pathfinder/npf/npf.h b/src/pathfinder/npf/npf.h new file mode 100644 index 000000000..3beb09391 --- /dev/null +++ b/src/pathfinder/npf/npf.h @@ -0,0 +1,151 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file npf.h New A* pathfinder. */ + +#ifndef NPF_H +#define NPF_H + +#include "aystar.h" +#include "../../station_type.h" +#include "../../rail_type.h" +#include "../../company_type.h" +#include "../../vehicle_type.h" +#include "../../tile_type.h" +#include "../../track_type.h" +#include "../../core/bitmath_func.hpp" +#include "../../transport_type.h" + +/* mowing grass */ +enum { + NPF_HASH_BITS = 12, ///< The size of the hash used in pathfinding. Just changing this value should be sufficient to change the hash size. Should be an even value. + /* Do no change below values */ + NPF_HASH_SIZE = 1 << NPF_HASH_BITS, + NPF_HASH_HALFBITS = NPF_HASH_BITS / 2, + NPF_HASH_HALFMASK = (1 << NPF_HASH_HALFBITS) - 1 +}; + +/* For new pathfinding. Define here so it is globally available without having + * to include npf.h */ +enum { + NPF_TILE_LENGTH = 100 +}; + +enum { + /** This penalty is the equivalent of "inifite", which means that paths that + * get this penalty will be chosen, but only if there is no other route + * without it. Be careful with not applying this penalty to often, or the + * total path cost might overflow.. + * For now, this is just a Very Big Penalty, we might actually implement + * this in a nicer way :-) + */ + NPF_INFINITE_PENALTY = 1000 * NPF_TILE_LENGTH +}; + +/* Meant to be stored in AyStar.targetdata */ +struct NPFFindStationOrTileData { + TileIndex dest_coords; ///< An indication of where the station is, for heuristic purposes, or the target tile + StationID station_index; ///< station index we're heading for, or INVALID_STATION when we're heading for a tile + bool reserve_path; ///< Indicates whether the found path should be reserved + const Vehicle *v; ///< The vehicle we are pathfinding for +}; + +/* Indices into AyStar.userdata[] */ +enum { + NPF_TYPE = 0, ///< Contains a TransportTypes value + NPF_SUB_TYPE, ///< Contains the sub transport type + NPF_OWNER, ///< Contains an Owner value + NPF_RAILTYPES, ///< Contains a bitmask the compatible RailTypes of the engine when NPF_TYPE == TRANSPORT_RAIL. Unused otherwise. +}; + +/* Indices into AyStarNode.userdata[] */ +enum { + NPF_TRACKDIR_CHOICE = 0, ///< The trackdir chosen to get here + NPF_NODE_FLAGS, +}; + +/* Flags for AyStarNode.userdata[NPF_NODE_FLAGS]. Use NPFGetBit() and NPFGetBit() to use them. */ +enum NPFNodeFlag { + NPF_FLAG_SEEN_SIGNAL, ///< Used to mark that a signal was seen on the way, for rail only + NPF_FLAG_2ND_SIGNAL, ///< Used to mark that two signals were seen, rail only + NPF_FLAG_3RD_SIGNAL, ///< Used to mark that three signals were seen, rail only + NPF_FLAG_REVERSE, ///< Used to mark that this node was reached from the second start node, if applicable + NPF_FLAG_LAST_SIGNAL_RED, ///< Used to mark that the last signal on this path was red + NPF_FLAG_IGNORE_START_TILE, ///< Used to mark that the start tile is invalid, and searching should start from the second tile on + NPF_FLAG_TARGET_RESERVED, ///< Used to mark that the possible reservation target is already reserved + NPF_FLAG_IGNORE_RESERVED, ///< Used to mark that reserved tiles should be considered impassable +}; + +/* Meant to be stored in AyStar.userpath */ +struct NPFFoundTargetData { + uint best_bird_dist; ///< The best heuristic found. Is 0 if the target was found + uint best_path_dist; ///< The shortest path. Is UINT_MAX if no path is found + Trackdir best_trackdir; ///< The trackdir that leads to the shortest path/closest birds dist + AyStarNode node; ///< The node within the target the search led us to + bool res_okay; ///< True if a path reservation could be made +}; + +/* These functions below are _not_ re-entrant, in favor of speed! */ + +/* Will search from the given tile and direction, for a route to the given + * station for the given transport type. See the declaration of + * NPFFoundTargetData above for the meaning of the result. */ +NPFFoundTargetData NPFRouteToStationOrTile(TileIndex tile, Trackdir trackdir, bool ignore_start_tile, NPFFindStationOrTileData *target, TransportType type, uint sub_type, Owner owner, RailTypes railtypes); + +/* Will search as above, but with two start nodes, the second being the + * reverse. Look at the NPF_FLAG_REVERSE flag in the result node to see which + * direction was taken (NPFGetBit(result.node, NPF_FLAG_REVERSE)) */ +NPFFoundTargetData NPFRouteToStationOrTileTwoWay(TileIndex tile1, Trackdir trackdir1, bool ignore_start_tile1, TileIndex tile2, Trackdir trackdir2, bool ignore_start_tile2, NPFFindStationOrTileData *target, TransportType type, uint sub_type, Owner owner, RailTypes railtypes); + +/* Will search a route to the closest depot. */ + +/* Search using breadth first. Good for little track choice and inaccurate + * heuristic, such as railway/road.*/ +NPFFoundTargetData NPFRouteToDepotBreadthFirst(TileIndex tile, Trackdir trackdir, bool ignore_start_tile, TransportType type, uint sub_type, Owner owner, RailTypes railtypes); +/* Same as above but with two start nodes, the second being the reverse. Call + * NPFGetBit(result.node, NPF_FLAG_REVERSE) to see from which node the path + * orginated. All pathfs from the second node will have the given + * reverse_penalty applied (NPF_TILE_LENGTH is the equivalent of one full + * tile). + */ +NPFFoundTargetData NPFRouteToDepotBreadthFirstTwoWay(TileIndex tile1, Trackdir trackdir1, bool ignore_start_tile1, TileIndex tile2, Trackdir trackdir2, bool ignore_start_tile2, TransportType type, uint sub_type, Owner owner, RailTypes railtypes, uint reverse_penalty); +/* Search by trying each depot in order of Manhattan Distance. Good for lots + * of choices and accurate heuristics, such as water. */ +NPFFoundTargetData NPFRouteToDepotTrialError(TileIndex tile, Trackdir trackdir, bool ignore_start_tile, TransportType type, uint sub_type, Owner owner, RailTypes railtypes); + +/** + * Search for any safe tile using a breadth first search and try to reserve a path. + */ +NPFFoundTargetData NPFRouteToSafeTile(const struct Train *v, TileIndex tile, Trackdir trackdir, bool override_railtype); + + +void NPFFillWithOrderData(NPFFindStationOrTileData *fstd, Vehicle *v, bool reserve_path = false); + + +/* + * Functions to manipulate the various NPF related flags on an AyStarNode. + */ + +/** + * Returns the current value of the given flag on the given AyStarNode. + */ +static inline bool NPFGetFlag(const AyStarNode *node, NPFNodeFlag flag) +{ + return HasBit(node->user_data[NPF_NODE_FLAGS], flag); +} + +/** + * Sets the given flag on the given AyStarNode to the given value. + */ +static inline void NPFSetFlag(AyStarNode *node, NPFNodeFlag flag, bool value) +{ + SB(node->user_data[NPF_NODE_FLAGS], flag, 1, value); +} + +#endif /* NPF_H */ diff --git a/src/pathfinder/npf/queue.cpp b/src/pathfinder/npf/queue.cpp new file mode 100644 index 000000000..a954876a6 --- /dev/null +++ b/src/pathfinder/npf/queue.cpp @@ -0,0 +1,577 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file queue.cpp Implementation of the Queue/Hash. */ + +#include "../../stdafx.h" +#include "../../core/alloc_func.hpp" +#include "queue.h" + + +/* + * Insertion Sorter + */ + +static void InsSort_Clear(Queue *q, bool free_values) +{ + InsSortNode *node = q->data.inssort.first; + InsSortNode *prev; + + while (node != NULL) { + if (free_values) free(node->item); + prev = node; + node = node->next; + free(prev); + } + q->data.inssort.first = NULL; +} + +static void InsSort_Free(Queue *q, bool free_values) +{ + q->clear(q, free_values); +} + +static bool InsSort_Push(Queue *q, void *item, int priority) +{ + InsSortNode *newnode = MallocT<InsSortNode>(1); + + newnode->item = item; + newnode->priority = priority; + if (q->data.inssort.first == NULL || + q->data.inssort.first->priority >= priority) { + newnode->next = q->data.inssort.first; + q->data.inssort.first = newnode; + } else { + InsSortNode *node = q->data.inssort.first; + while (node != NULL) { + if (node->next == NULL || node->next->priority >= priority) { + newnode->next = node->next; + node->next = newnode; + break; + } + node = node->next; + } + } + return true; +} + +static void *InsSort_Pop(Queue *q) +{ + InsSortNode *node = q->data.inssort.first; + void *result; + + if (node == NULL) return NULL; + result = node->item; + q->data.inssort.first = q->data.inssort.first->next; + assert(q->data.inssort.first == NULL || q->data.inssort.first->priority >= node->priority); + free(node); + return result; +} + +static bool InsSort_Delete(Queue *q, void *item, int priority) +{ + return false; +} + +void init_InsSort(Queue *q) +{ + q->push = InsSort_Push; + q->pop = InsSort_Pop; + q->del = InsSort_Delete; + q->clear = InsSort_Clear; + q->free = InsSort_Free; + q->data.inssort.first = NULL; +} + + +/* + * Binary Heap + * For information, see: http://www.policyalmanac.org/games/binaryHeaps.htm + */ + +#define BINARY_HEAP_BLOCKSIZE (1 << BINARY_HEAP_BLOCKSIZE_BITS) +#define BINARY_HEAP_BLOCKSIZE_MASK (BINARY_HEAP_BLOCKSIZE - 1) + +/* To make our life easy, we make the next define + * Because Binary Heaps works with array from 1 to n, + * and C with array from 0 to n-1, and we don't like typing + * q->data.binaryheap.elements[i - 1] every time, we use this define. */ +#define BIN_HEAP_ARR(i) q->data.binaryheap.elements[((i) - 1) >> BINARY_HEAP_BLOCKSIZE_BITS][((i) - 1) & BINARY_HEAP_BLOCKSIZE_MASK] + +static void BinaryHeap_Clear(Queue *q, bool free_values) +{ + /* Free all items if needed and free all but the first blocks of memory */ + uint i; + uint j; + + for (i = 0; i < q->data.binaryheap.blocks; i++) { + if (q->data.binaryheap.elements[i] == NULL) { + /* No more allocated blocks */ + break; + } + /* For every allocated block */ + if (free_values) { + for (j = 0; j < (1 << BINARY_HEAP_BLOCKSIZE_BITS); j++) { + /* For every element in the block */ + if ((q->data.binaryheap.size >> BINARY_HEAP_BLOCKSIZE_BITS) == i && + (q->data.binaryheap.size & BINARY_HEAP_BLOCKSIZE_MASK) == j) { + break; // We're past the last element + } + free(q->data.binaryheap.elements[i][j].item); + } + } + if (i != 0) { + /* Leave the first block of memory alone */ + free(q->data.binaryheap.elements[i]); + q->data.binaryheap.elements[i] = NULL; + } + } + q->data.binaryheap.size = 0; + q->data.binaryheap.blocks = 1; +} + +static void BinaryHeap_Free(Queue *q, bool free_values) +{ + uint i; + + q->clear(q, free_values); + for (i = 0; i < q->data.binaryheap.blocks; i++) { + if (q->data.binaryheap.elements[i] == NULL) break; + free(q->data.binaryheap.elements[i]); + } + free(q->data.binaryheap.elements); +} + +static bool BinaryHeap_Push(Queue *q, void *item, int priority) +{ +#ifdef QUEUE_DEBUG + printf("[BinaryHeap] Pushing an element. There are %d elements left\n", q->data.binaryheap.size); +#endif + + if (q->data.binaryheap.size == q->data.binaryheap.max_size) return false; + assert(q->data.binaryheap.size < q->data.binaryheap.max_size); + + if (q->data.binaryheap.elements[q->data.binaryheap.size >> BINARY_HEAP_BLOCKSIZE_BITS] == NULL) { + /* The currently allocated blocks are full, allocate a new one */ + assert((q->data.binaryheap.size & BINARY_HEAP_BLOCKSIZE_MASK) == 0); + q->data.binaryheap.elements[q->data.binaryheap.size >> BINARY_HEAP_BLOCKSIZE_BITS] = MallocT<BinaryHeapNode>(BINARY_HEAP_BLOCKSIZE); + q->data.binaryheap.blocks++; +#ifdef QUEUE_DEBUG + printf("[BinaryHeap] Increasing size of elements to %d nodes\n", q->data.binaryheap.blocks * BINARY_HEAP_BLOCKSIZE); +#endif + } + + /* Add the item at the end of the array */ + BIN_HEAP_ARR(q->data.binaryheap.size + 1).priority = priority; + BIN_HEAP_ARR(q->data.binaryheap.size + 1).item = item; + q->data.binaryheap.size++; + + /* Now we are going to check where it belongs. As long as the parent is + * bigger, we switch with the parent */ + { + BinaryHeapNode temp; + int i; + int j; + + i = q->data.binaryheap.size; + while (i > 1) { + /* Get the parent of this object (divide by 2) */ + j = i / 2; + /* Is the parent bigger then the current, switch them */ + if (BIN_HEAP_ARR(i).priority <= BIN_HEAP_ARR(j).priority) { + temp = BIN_HEAP_ARR(j); + BIN_HEAP_ARR(j) = BIN_HEAP_ARR(i); + BIN_HEAP_ARR(i) = temp; + i = j; + } else { + /* It is not, we're done! */ + break; + } + } + } + + return true; +} + +static bool BinaryHeap_Delete(Queue *q, void *item, int priority) +{ + uint i = 0; + +#ifdef QUEUE_DEBUG + printf("[BinaryHeap] Deleting an element. There are %d elements left\n", q->data.binaryheap.size); +#endif + + /* First, we try to find the item.. */ + do { + if (BIN_HEAP_ARR(i + 1).item == item) break; + i++; + } while (i < q->data.binaryheap.size); + /* We did not find the item, so we return false */ + if (i == q->data.binaryheap.size) return false; + + /* Now we put the last item over the current item while decreasing the size of the elements */ + q->data.binaryheap.size--; + BIN_HEAP_ARR(i + 1) = BIN_HEAP_ARR(q->data.binaryheap.size + 1); + + /* Now the only thing we have to do, is resort it.. + * On place i there is the item to be sorted.. let's start there */ + { + uint j; + BinaryHeapNode temp; + /* Because of the fact that Binary Heap uses array from 1 to n, we need to + * increase i by 1 + */ + i++; + + for (;;) { + j = i; + /* Check if we have 2 childs */ + if (2 * j + 1 <= q->data.binaryheap.size) { + /* Is this child smaller than the parent? */ + if (BIN_HEAP_ARR(j).priority >= BIN_HEAP_ARR(2 * j).priority) i = 2 * j; + /* Yes, we _need_ to use i here, not j, because we want to have the smallest child + * This way we get that straight away! */ + if (BIN_HEAP_ARR(i).priority >= BIN_HEAP_ARR(2 * j + 1).priority) i = 2 * j + 1; + /* Do we have one child? */ + } else if (2 * j <= q->data.binaryheap.size) { + if (BIN_HEAP_ARR(j).priority >= BIN_HEAP_ARR(2 * j).priority) i = 2 * j; + } + + /* One of our childs is smaller than we are, switch */ + if (i != j) { + temp = BIN_HEAP_ARR(j); + BIN_HEAP_ARR(j) = BIN_HEAP_ARR(i); + BIN_HEAP_ARR(i) = temp; + } else { + /* None of our childs is smaller, so we stay here.. stop :) */ + break; + } + } + } + + return true; +} + +static void *BinaryHeap_Pop(Queue *q) +{ + void *result; + +#ifdef QUEUE_DEBUG + printf("[BinaryHeap] Popping an element. There are %d elements left\n", q->data.binaryheap.size); +#endif + + if (q->data.binaryheap.size == 0) return NULL; + + /* The best item is always on top, so give that as result */ + result = BIN_HEAP_ARR(1).item; + /* And now we should get rid of this item... */ + BinaryHeap_Delete(q, BIN_HEAP_ARR(1).item, BIN_HEAP_ARR(1).priority); + + return result; +} + +void init_BinaryHeap(Queue *q, uint max_size) +{ + assert(q != NULL); + q->push = BinaryHeap_Push; + q->pop = BinaryHeap_Pop; + q->del = BinaryHeap_Delete; + q->clear = BinaryHeap_Clear; + q->free = BinaryHeap_Free; + q->data.binaryheap.max_size = max_size; + q->data.binaryheap.size = 0; + /* We malloc memory in block of BINARY_HEAP_BLOCKSIZE + * It autosizes when it runs out of memory */ + q->data.binaryheap.elements = CallocT<BinaryHeapNode*>((max_size - 1) / BINARY_HEAP_BLOCKSIZE + 1); + q->data.binaryheap.elements[0] = MallocT<BinaryHeapNode>(BINARY_HEAP_BLOCKSIZE); + q->data.binaryheap.blocks = 1; +#ifdef QUEUE_DEBUG + printf("[BinaryHeap] Initial size of elements is %d nodes\n", BINARY_HEAP_BLOCKSIZE); +#endif +} + +/* Because we don't want anyone else to bother with our defines */ +#undef BIN_HEAP_ARR + +/* + * Hash + */ + +void init_Hash(Hash *h, Hash_HashProc *hash, uint num_buckets) +{ + /* Allocate space for the Hash, the buckets and the bucket flags */ + uint i; + + assert(h != NULL); +#ifdef HASH_DEBUG + debug("Allocated hash: %p", h); +#endif + h->hash = hash; + h->size = 0; + h->num_buckets = num_buckets; + h->buckets = (HashNode*)MallocT<byte>(num_buckets * (sizeof(*h->buckets) + sizeof(*h->buckets_in_use))); +#ifdef HASH_DEBUG + debug("Buckets = %p", h->buckets); +#endif + h->buckets_in_use = (bool*)(h->buckets + num_buckets); + for (i = 0; i < num_buckets; i++) h->buckets_in_use[i] = false; +} + + +void delete_Hash(Hash *h, bool free_values) +{ + uint i; + + /* Iterate all buckets */ + for (i = 0; i < h->num_buckets; i++) { + if (h->buckets_in_use[i]) { + HashNode *node; + + /* Free the first value */ + if (free_values) free(h->buckets[i].value); + node = h->buckets[i].next; + while (node != NULL) { + HashNode *prev = node; + + node = node->next; + /* Free the value */ + if (free_values) free(prev->value); + /* Free the node */ + free(prev); + } + } + } + free(h->buckets); + /* No need to free buckets_in_use, it is always allocated in one + * malloc with buckets */ +#ifdef HASH_DEBUG + debug("Freeing Hash: %p", h); +#endif +} + +#ifdef HASH_STATS +static void stat_Hash(const Hash *h) +{ + uint used_buckets = 0; + uint max_collision = 0; + uint max_usage = 0; + uint usage[200]; + uint i; + + for (i = 0; i < lengthof(usage); i++) usage[i] = 0; + for (i = 0; i < h->num_buckets; i++) { + uint collision = 0; + if (h->buckets_in_use[i]) { + const HashNode *node; + + used_buckets++; + for (node = &h->buckets[i]; node != NULL; node = node->next) collision++; + if (collision > max_collision) max_collision = collision; + } + if (collision >= lengthof(usage)) collision = lengthof(usage) - 1; + usage[collision]++; + if (collision > 0 && usage[collision] >= max_usage) { + max_usage = usage[collision]; + } + } + printf( + "---\n" + "Hash size: %d\n" + "Nodes used: %d\n" + "Non empty buckets: %d\n" + "Max collision: %d\n", + h->num_buckets, h->size, used_buckets, max_collision + ); + printf("{ "); + for (i = 0; i <= max_collision; i++) { + if (usage[i] > 0) { + printf("%d:%d ", i, usage[i]); +#if 0 + if (i > 0) { + uint j; + + for (j = 0; j < usage[i] * 160 / 800; j++) putchar('#'); + } + printf("\n"); +#endif + } + } + printf ("}\n"); +} +#endif + +void clear_Hash(Hash *h, bool free_values) +{ + uint i; + +#ifdef HASH_STATS + if (h->size > 2000) stat_Hash(h); +#endif + + /* Iterate all buckets */ + for (i = 0; i < h->num_buckets; i++) { + if (h->buckets_in_use[i]) { + HashNode *node; + + h->buckets_in_use[i] = false; + /* Free the first value */ + if (free_values) free(h->buckets[i].value); + node = h->buckets[i].next; + while (node != NULL) { + HashNode *prev = node; + + node = node->next; + if (free_values) free(prev->value); + free(prev); + } + } + } + h->size = 0; +} + +/** Finds the node that that saves this key pair. If it is not + * found, returns NULL. If it is found, *prev is set to the + * node before the one found, or if the node found was the first in the bucket + * to NULL. If it is not found, *prev is set to the last HashNode in the + * bucket, or NULL if it is empty. prev can also be NULL, in which case it is + * not used for output. + */ +static HashNode *Hash_FindNode(const Hash *h, uint key1, uint key2, HashNode** prev_out) +{ + uint hash = h->hash(key1, key2); + HashNode *result = NULL; + +#ifdef HASH_DEBUG + debug("Looking for %u, %u", key1, key2); +#endif + /* Check if the bucket is empty */ + if (!h->buckets_in_use[hash]) { + if (prev_out != NULL) *prev_out = NULL; + result = NULL; + /* Check the first node specially */ + } else if (h->buckets[hash].key1 == key1 && h->buckets[hash].key2 == key2) { + /* Save the value */ + result = h->buckets + hash; + if (prev_out != NULL) *prev_out = NULL; +#ifdef HASH_DEBUG + debug("Found in first node: %p", result); +#endif + /* Check all other nodes */ + } else { + HashNode *prev = h->buckets + hash; + HashNode *node; + + for (node = prev->next; node != NULL; node = node->next) { + if (node->key1 == key1 && node->key2 == key2) { + /* Found it */ + result = node; +#ifdef HASH_DEBUG + debug("Found in other node: %p", result); +#endif + break; + } + prev = node; + } + if (prev_out != NULL) *prev_out = prev; + } +#ifdef HASH_DEBUG + if (result == NULL) debug("Not found"); +#endif + return result; +} + +void *Hash_Delete(Hash *h, uint key1, uint key2) +{ + void *result; + HashNode *prev; // Used as output var for below function call + HashNode *node = Hash_FindNode(h, key1, key2, &prev); + + if (node == NULL) { + /* not found */ + result = NULL; + } else if (prev == NULL) { + /* It is in the first node, we can't free that one, so we free + * the next one instead (if there is any)*/ + /* Save the value */ + result = node->value; + if (node->next != NULL) { + HashNode *next = node->next; + /* Copy the second to the first */ + *node = *next; + /* Free the second */ +#ifndef NOFREE + free(next); +#endif + } else { + /* This was the last in this bucket + * Mark it as empty */ + uint hash = h->hash(key1, key2); + h->buckets_in_use[hash] = false; + } + } else { + /* It is in another node + * Save the value */ + result = node->value; + /* Link previous and next nodes */ + prev->next = node->next; + /* Free the node */ +#ifndef NOFREE + free(node); +#endif + } + if (result != NULL) h->size--; + return result; +} + + +void *Hash_Set(Hash *h, uint key1, uint key2, void *value) +{ + HashNode *prev; + HashNode *node = Hash_FindNode(h, key1, key2, &prev); + + if (node != NULL) { + /* Found it */ + void *result = node->value; + + node->value = value; + return result; + } + /* It is not yet present, let's add it */ + if (prev == NULL) { + /* The bucket is still empty */ + uint hash = h->hash(key1, key2); + h->buckets_in_use[hash] = true; + node = h->buckets + hash; + } else { + /* Add it after prev */ + node = MallocT<HashNode>(1); + prev->next = node; + } + node->next = NULL; + node->key1 = key1; + node->key2 = key2; + node->value = value; + h->size++; + return NULL; +} + +void *Hash_Get(const Hash *h, uint key1, uint key2) +{ + HashNode *node = Hash_FindNode(h, key1, key2, NULL); + +#ifdef HASH_DEBUG + debug("Found node: %p", node); +#endif + return (node != NULL) ? node->value : NULL; +} + +uint Hash_Size(const Hash *h) +{ + return h->size; +} diff --git a/src/pathfinder/npf/queue.h b/src/pathfinder/npf/queue.h new file mode 100644 index 000000000..76421e9ac --- /dev/null +++ b/src/pathfinder/npf/queue.h @@ -0,0 +1,167 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file queue.h Simple Queue/Hash implementations. */ + +#ifndef QUEUE_H +#define QUEUE_H + +//#define NOFREE +//#define QUEUE_DEBUG +//#define HASH_DEBUG +//#define HASH_STATS + + +struct Queue; +typedef bool Queue_PushProc(Queue *q, void *item, int priority); +typedef void *Queue_PopProc(Queue *q); +typedef bool Queue_DeleteProc(Queue *q, void *item, int priority); +typedef void Queue_ClearProc(Queue *q, bool free_values); +typedef void Queue_FreeProc(Queue *q, bool free_values); + +struct InsSortNode { + void *item; + int priority; + InsSortNode *next; +}; + +struct BinaryHeapNode { + void *item; + int priority; +}; + + +struct Queue{ + /* + * Pushes an element into the queue, at the appropriate place for the queue. + * Requires the queue pointer to be of an appropriate type, of course. + */ + Queue_PushProc *push; + /* + * Pops the first element from the queue. What exactly is the first element, + * is defined by the exact type of queue. + */ + Queue_PopProc *pop; + /* + * Deletes the item from the queue. priority should be specified if + * known, which speeds up the deleting for some queue's. Should be -1 + * if not known. + */ + Queue_DeleteProc *del; + + /* Clears the queue, by removing all values from it. It's state is + * effectively reset. If free_items is true, each of the items cleared + * in this way are free()'d. + */ + Queue_ClearProc *clear; + /* Frees the queue, by reclaiming all memory allocated by it. After + * this it is no longer usable. If free_items is true, any remaining + * items are free()'d too. + */ + Queue_FreeProc *free; + + union { + struct { + InsSortNode *first; + } inssort; + struct { + uint max_size; + uint size; + uint blocks; ///< The amount of blocks for which space is reserved in elements + BinaryHeapNode **elements; + } binaryheap; + } data; +}; + + +/** + * Insertion Sorter + */ + +/* Initializes a inssort and allocates internal memory. There is no maximum + * size */ +void init_InsSort(Queue *q); + + +/* + * Binary Heap + * For information, see: + * http://www.policyalmanac.org/games/binaryHeaps.htm + */ + +/* The amount of elements that will be malloc'd at a time */ +#define BINARY_HEAP_BLOCKSIZE_BITS 10 + +/** Initializes a binary heap and allocates internal memory for maximum of + * max_size elements */ +void init_BinaryHeap(Queue *q, uint max_size); + + +/* + * Hash + */ +struct HashNode { + uint key1; + uint key2; + void *value; + HashNode *next; +}; +/** + * Generates a hash code from the given key pair. You should make sure that + * the resulting range is clearly defined. + */ +typedef uint Hash_HashProc(uint key1, uint key2); +struct Hash { + /* The hash function used */ + Hash_HashProc *hash; + /* The amount of items in the hash */ + uint size; + /* The number of buckets allocated */ + uint num_buckets; + /* A pointer to an array of num_buckets buckets. */ + HashNode *buckets; + /* A pointer to an array of numbuckets booleans, which will be true if + * there are any Nodes in the bucket */ + bool *buckets_in_use; +}; + +/* Call these function to manipulate a hash */ + +/** Deletes the value with the specified key pair from the hash and returns + * that value. Returns NULL when the value was not present. The value returned + * is _not_ free()'d! */ +void *Hash_Delete(Hash *h, uint key1, uint key2); +/** Sets the value associated with the given key pair to the given value. + * Returns the old value if the value was replaced, NULL when it was not yet present. */ +void *Hash_Set(Hash *h, uint key1, uint key2, void *value); +/** Gets the value associated with the given key pair, or NULL when it is not + * present. */ +void *Hash_Get(const Hash *h, uint key1, uint key2); + +/* Call these function to create/destroy a hash */ + +/** Builds a new hash in an existing struct. Make sure that hash() always + * returns a hash less than num_buckets! Call delete_hash after use */ +void init_Hash(Hash *h, Hash_HashProc *hash, uint num_buckets); +/** + * Deletes the hash and cleans up. Only cleans up memory allocated by new_Hash + * & friends. If free is true, it will call free() on all the values that + * are left in the hash. + */ +void delete_Hash(Hash *h, bool free_values); +/** + * Cleans the hash, but keeps the memory allocated + */ +void clear_Hash(Hash *h, bool free_values); +/** + * Gets the current size of the Hash + */ +uint Hash_Size(const Hash *h); + +#endif /* QUEUE_H */ diff --git a/src/pathfinder/opf/opf_ship.cpp b/src/pathfinder/opf/opf_ship.cpp new file mode 100644 index 000000000..7ee5ec9c3 --- /dev/null +++ b/src/pathfinder/opf/opf_ship.cpp @@ -0,0 +1,107 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file pathfind.cpp Implementation of the oldest supported pathfinder. */ + +#include "../../stdafx.h" +#include "../../debug.h" +#include "../../tunnelbridge_map.h" +#include "../../core/alloc_type.hpp" +#include "../../tunnelbridge.h" +#include "opf_ship.h" + +struct RememberData { + uint16 cur_length; + byte depth; + Track last_choosen_track; +}; + +struct TrackPathFinder { + TPFEnumProc *enum_proc; + void *userdata; + RememberData rd; + TrackdirByte the_dir; +}; + +static void TPFModeShip(TrackPathFinder *tpf, TileIndex tile, DiagDirection direction) +{ + if (IsTileType(tile, MP_TUNNELBRIDGE)) { + /* wrong track type */ + if (GetTunnelBridgeTransportType(tile) != TRANSPORT_WATER) return; + + DiagDirection dir = GetTunnelBridgeDirection(tile); + /* entering tunnel / bridge? */ + if (dir == direction) { + TileIndex endtile = GetOtherTunnelBridgeEnd(tile); + + tpf->rd.cur_length += GetTunnelBridgeLength(tile, endtile) + 1; + + tile = endtile; + } else { + /* leaving tunnel / bridge? */ + if (ReverseDiagDir(dir) != direction) return; + } + } + + /* This addition will sometimes overflow by a single tile. + * The use of TILE_MASK here makes sure that we still point at a valid + * tile, and then this tile will be in the sentinel row/col, so GetTileTrackStatus will fail. */ + tile = TILE_MASK(tile + TileOffsByDiagDir(direction)); + + if (++tpf->rd.cur_length > 50) + return; + + TrackBits bits = TrackStatusToTrackBits(GetTileTrackStatus(tile, TRANSPORT_WATER, 0)) & DiagdirReachesTracks(direction); + if (bits == TRACK_BIT_NONE) return; + + assert(TileX(tile) != MapMaxX() && TileY(tile) != MapMaxY()); + + bool only_one_track = true; + do { + Track track = RemoveFirstTrack(&bits); + if (bits != TRACK_BIT_NONE) only_one_track = false; + RememberData rd = tpf->rd; + + /* Change direction 4 times only */ + if (!only_one_track && track != tpf->rd.last_choosen_track) { + if (++tpf->rd.depth > 4) { + tpf->rd = rd; + return; + } + tpf->rd.last_choosen_track = track; + } + + tpf->the_dir = TrackEnterdirToTrackdir(track, direction); + + if (!tpf->enum_proc(tile, tpf->userdata, tpf->the_dir, tpf->rd.cur_length)) { + TPFModeShip(tpf, tile, TrackdirToExitdir(tpf->the_dir)); + } + + tpf->rd = rd; + } while (bits != TRACK_BIT_NONE); + +} + +void OPFShipFollowTrack(TileIndex tile, DiagDirection direction, TPFEnumProc *enum_proc, void *data) +{ + assert(IsValidDiagDirection(direction)); + + SmallStackSafeStackAlloc<TrackPathFinder, 1> tpf; + + /* initialize path finder variables */ + tpf->userdata = data; + tpf->enum_proc = enum_proc; + + tpf->rd.cur_length = 0; + tpf->rd.depth = 0; + tpf->rd.last_choosen_track = INVALID_TRACK; + + tpf->enum_proc(tile, data, INVALID_TRACKDIR, 0); + TPFModeShip(tpf, tile, direction); +} diff --git a/src/pathfinder/opf/opf_ship.h b/src/pathfinder/opf/opf_ship.h new file mode 100644 index 000000000..6afff227b --- /dev/null +++ b/src/pathfinder/opf/opf_ship.h @@ -0,0 +1,21 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file opf_ship.h Original pathfinder for ships; very simple. */ + +#ifndef OPF_SHIP_H +#define OPF_SHIP_H + +#include "../../direction_type.h" + +typedef bool TPFEnumProc(TileIndex tile, void *data, Trackdir trackdir, uint length); + +void OPFShipFollowTrack(TileIndex tile, DiagDirection direction, TPFEnumProc *enum_proc, void *data); + +#endif /* OPF_SHIP_H */ diff --git a/src/pathfinder/pathfinder_func.h b/src/pathfinder/pathfinder_func.h new file mode 100644 index 000000000..dc79c8024 --- /dev/null +++ b/src/pathfinder/pathfinder_func.h @@ -0,0 +1,49 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file pathfinder_func.h General functions related to pathfinders. */ + +#ifndef PATHFINDER_FUNC_H +#define PATHFINDER_FUNC_H + +#include "../station_base.h" +#include "../waypoint_base.h" + +/** + * Calculates the tile of given station that is closest to a given tile + * for this we assume the station is a rectangle, + * as defined by its tile are (st->train_station) + * @param station The station to calculate the distance to + * @param tile The tile from where to calculate the distance + * @return The closest station tile to the given tile. + */ +static inline TileIndex CalcClosestStationTile(StationID station, TileIndex tile) +{ + const BaseStation *st = BaseStation::Get(station); + + /* If the rail station is (temporarily) not present, use the station sign to drive near the station */ + if (st->train_station.tile == INVALID_TILE) return st->xy; + + uint minx = TileX(st->train_station.tile); // topmost corner of station + uint miny = TileY(st->train_station.tile); + uint maxx = minx + st->train_station.w - 1; // lowermost corner of station + uint maxy = miny + st->train_station.h - 1; + + /* we are going the aim for the x coordinate of the closest corner + * but if we are between those coordinates, we will aim for our own x coordinate */ + uint x = ClampU(TileX(tile), minx, maxx); + + /* same for y coordinate, see above comment */ + uint y = ClampU(TileY(tile), miny, maxy); + + /* return the tile of our target coordinates */ + return TileXY(x, y); +} + +#endif /* PATHFINDER_FUNC_H */ diff --git a/src/pathfinder/yapf/follow_track.hpp b/src/pathfinder/yapf/follow_track.hpp new file mode 100644 index 000000000..a1f769de9 --- /dev/null +++ b/src/pathfinder/yapf/follow_track.hpp @@ -0,0 +1,451 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file follow_track.hpp Template function for track followers */ + +#ifndef FOLLOW_TRACK_HPP +#define FOLLOW_TRACK_HPP + +#include "yapf.hpp" +#include "../../depot_map.h" +#include "../../roadveh.h" +#include "../../train.h" + +/** Track follower helper template class (can serve pathfinders and vehicle + * controllers). See 6 different typedefs below for 3 different transport + * types w/ or w/o 90-deg turns allowed */ +template <TransportType Ttr_type_, bool T90deg_turns_allowed_ = true, bool Tmask_reserved_tracks = false> +struct CFollowTrackT +{ + enum ErrorCode { + EC_NONE, + EC_OWNER, + EC_RAIL_TYPE, + EC_90DEG, + EC_NO_WAY, + EC_RESERVED, + }; + + const Vehicle *m_veh; ///< moving vehicle + Owner m_veh_owner; ///< owner of the vehicle + TileIndex m_old_tile; ///< the origin (vehicle moved from) before move + Trackdir m_old_td; ///< the trackdir (the vehicle was on) before move + TileIndex m_new_tile; ///< the new tile (the vehicle has entered) + TrackdirBits m_new_td_bits; ///< the new set of available trackdirs + DiagDirection m_exitdir; ///< exit direction (leaving the old tile) + bool m_is_tunnel; ///< last turn passed tunnel + bool m_is_bridge; ///< last turn passed bridge ramp + bool m_is_station; ///< last turn passed station + int m_tiles_skipped; ///< number of skipped tunnel or station tiles + ErrorCode m_err; + CPerformanceTimer *m_pPerf; + RailTypes m_railtypes; + + FORCEINLINE CFollowTrackT(const Vehicle *v = NULL, RailTypes railtype_override = INVALID_RAILTYPES, CPerformanceTimer *pPerf = NULL) + { + Init(v, railtype_override, pPerf); + } + + FORCEINLINE CFollowTrackT(Owner o, RailTypes railtype_override = INVALID_RAILTYPES, CPerformanceTimer *pPerf = NULL) + { + m_veh = NULL; + Init(o, railtype_override, pPerf); + } + + FORCEINLINE void Init(const Vehicle *v, RailTypes railtype_override, CPerformanceTimer *pPerf) + { + assert(!IsRailTT() || (v != NULL && v->type == VEH_TRAIN)); + m_veh = v; + Init(v != NULL ? v->owner : INVALID_OWNER, IsRailTT() && railtype_override == INVALID_RAILTYPES ? Train::From(v)->compatible_railtypes : railtype_override, pPerf); + } + + FORCEINLINE void Init(Owner o, RailTypes railtype_override, CPerformanceTimer *pPerf) + { + assert((!IsRoadTT() || m_veh != NULL) && (!IsRailTT() || railtype_override != INVALID_RAILTYPES)); + m_veh_owner = o; + m_pPerf = pPerf; + /* don't worry, all is inlined so compiler should remove unnecessary initializations */ + m_new_tile = INVALID_TILE; + m_new_td_bits = TRACKDIR_BIT_NONE; + m_exitdir = INVALID_DIAGDIR; + m_is_station = m_is_bridge = m_is_tunnel = false; + m_tiles_skipped = 0; + m_err = EC_NONE; + m_railtypes = railtype_override; + } + + FORCEINLINE static TransportType TT() {return Ttr_type_;} + FORCEINLINE static bool IsWaterTT() {return TT() == TRANSPORT_WATER;} + FORCEINLINE static bool IsRailTT() {return TT() == TRANSPORT_RAIL;} + FORCEINLINE bool IsTram() {return IsRoadTT() && HasBit(RoadVehicle::From(m_veh)->compatible_roadtypes, ROADTYPE_TRAM);} + FORCEINLINE static bool IsRoadTT() {return TT() == TRANSPORT_ROAD;} + FORCEINLINE static bool Allow90degTurns() {return T90deg_turns_allowed_;} + FORCEINLINE static bool DoTrackMasking() {return IsRailTT() && Tmask_reserved_tracks;} + + /** Tests if a tile is a road tile with a single tramtrack (tram can reverse) */ + FORCEINLINE DiagDirection GetSingleTramBit(TileIndex tile) + { + assert(IsTram()); // this function shouldn't be called in other cases + + if (IsNormalRoadTile(tile)) { + RoadBits rb = GetRoadBits(tile, ROADTYPE_TRAM); + switch (rb) { + case ROAD_NW: return DIAGDIR_NW; + case ROAD_SW: return DIAGDIR_SW; + case ROAD_SE: return DIAGDIR_SE; + case ROAD_NE: return DIAGDIR_NE; + default: break; + } + } + return INVALID_DIAGDIR; + } + + /** main follower routine. Fills all members and return true on success. + * Otherwise returns false if track can't be followed. */ + inline bool Follow(TileIndex old_tile, Trackdir old_td) + { + m_old_tile = old_tile; + m_old_td = old_td; + m_err = EC_NONE; + assert(((TrackStatusToTrackdirBits(GetTileTrackStatus(m_old_tile, TT(), IsRoadTT() && m_veh != NULL ? RoadVehicle::From(m_veh)->compatible_roadtypes : 0)) & TrackdirToTrackdirBits(m_old_td)) != 0) || + (IsTram() && GetSingleTramBit(m_old_tile) != INVALID_DIAGDIR)); // Disable the assertion for single tram bits + m_exitdir = TrackdirToExitdir(m_old_td); + if (ForcedReverse()) return true; + if (!CanExitOldTile()) return false; + FollowTileExit(); + if (!QueryNewTileTrackStatus()) return TryReverse(); + if (!CanEnterNewTile()) return false; + m_new_td_bits &= DiagdirReachesTrackdirs(m_exitdir); + if (m_new_td_bits == TRACKDIR_BIT_NONE) { + m_err = EC_NO_WAY; + return false; + } + if (!Allow90degTurns()) { + m_new_td_bits &= (TrackdirBits)~(int)TrackdirCrossesTrackdirs(m_old_td); + if (m_new_td_bits == TRACKDIR_BIT_NONE) { + m_err = EC_90DEG; + return false; + } + } + return true; + } + + inline bool MaskReservedTracks() + { + if (!DoTrackMasking()) return true; + + if (m_is_station) { + /* Check skipped station tiles as well. */ + TileIndexDiff diff = TileOffsByDiagDir(m_exitdir); + for (TileIndex tile = m_new_tile - diff * m_tiles_skipped; tile != m_new_tile; tile += diff) { + if (HasStationReservation(tile)) { + m_new_td_bits = TRACKDIR_BIT_NONE; + m_err = EC_RESERVED; + return false; + } + } + } + + TrackBits reserved = GetReservedTrackbits(m_new_tile); + /* Mask already reserved trackdirs. */ + m_new_td_bits &= ~TrackBitsToTrackdirBits(reserved); + /* Mask out all trackdirs that conflict with the reservation. */ + uint bits = (uint)TrackdirBitsToTrackBits(m_new_td_bits); + int i; + FOR_EACH_SET_BIT(i, bits) { + if (TracksOverlap(reserved | TrackToTrackBits((Track)i))) m_new_td_bits &= ~TrackToTrackdirBits((Track)i); + } + if (m_new_td_bits == TRACKDIR_BIT_NONE) { + m_err = EC_RESERVED; + return false; + } + return true; + } + +protected: + /** Follow the m_exitdir from m_old_tile and fill m_new_tile and m_tiles_skipped */ + FORCEINLINE void FollowTileExit() + { + m_is_station = m_is_bridge = m_is_tunnel = false; + m_tiles_skipped = 0; + + /* extra handling for tunnels and bridges in our direction */ + if (IsTileType(m_old_tile, MP_TUNNELBRIDGE)) { + DiagDirection enterdir = GetTunnelBridgeDirection(m_old_tile); + if (enterdir == m_exitdir) { + /* we are entering the tunnel / bridge */ + if (IsTunnel(m_old_tile)) { + m_is_tunnel = true; + m_new_tile = GetOtherTunnelEnd(m_old_tile); + } else { // IsBridge(m_old_tile) + m_is_bridge = true; + m_new_tile = GetOtherBridgeEnd(m_old_tile); + } + m_tiles_skipped = GetTunnelBridgeLength(m_new_tile, m_old_tile); + return; + } + assert(ReverseDiagDir(enterdir) == m_exitdir); + } + + /* normal or station tile, do one step */ + TileIndexDiff diff = TileOffsByDiagDir(m_exitdir); + m_new_tile = TILE_ADD(m_old_tile, diff); + + /* special handling for stations */ + if (IsRailTT() && HasStationTileRail(m_new_tile)) { + m_is_station = true; + } else if (IsRoadTT() && IsRoadStopTile(m_new_tile)) { + m_is_station = true; + } else { + m_is_station = false; + } + } + + /** stores track status (available trackdirs) for the new tile into m_new_td_bits */ + FORCEINLINE bool QueryNewTileTrackStatus() + { + CPerfStart perf(*m_pPerf); + if (IsRailTT() && IsPlainRailTile(m_new_tile)) { + m_new_td_bits = (TrackdirBits)(GetTrackBits(m_new_tile) * 0x101); + } else { + m_new_td_bits = TrackStatusToTrackdirBits(GetTileTrackStatus(m_new_tile, TT(), IsRoadTT() && m_veh != NULL ? RoadVehicle::From(m_veh)->compatible_roadtypes : 0)); + + if (IsTram() && m_new_td_bits == 0) { + /* GetTileTrackStatus() returns 0 for single tram bits. + * As we cannot change it there (easily) without breaking something, change it here */ + switch (GetSingleTramBit(m_new_tile)) { + case DIAGDIR_NE: + case DIAGDIR_SW: + m_new_td_bits = TRACKDIR_BIT_X_NE | TRACKDIR_BIT_X_SW; + break; + + case DIAGDIR_NW: + case DIAGDIR_SE: + m_new_td_bits = TRACKDIR_BIT_Y_NW | TRACKDIR_BIT_Y_SE; + break; + + default: break; + } + } + } + return (m_new_td_bits != TRACKDIR_BIT_NONE); + } + + /** return true if we can leave m_old_tile in m_exitdir */ + FORCEINLINE bool CanExitOldTile() + { + /* road stop can be left at one direction only unless it's a drive-through stop */ + if (IsRoadTT() && IsStandardRoadStopTile(m_old_tile)) { + DiagDirection exitdir = GetRoadStopDir(m_old_tile); + if (exitdir != m_exitdir) { + m_err = EC_NO_WAY; + return false; + } + } + + /* single tram bits can only be left in one direction */ + if (IsTram()) { + DiagDirection single_tram = GetSingleTramBit(m_old_tile); + if (single_tram != INVALID_DIAGDIR && single_tram != m_exitdir) { + m_err = EC_NO_WAY; + return false; + } + } + + /* road depots can be also left in one direction only */ + if (IsRoadTT() && IsDepotTypeTile(m_old_tile, TT())) { + DiagDirection exitdir = GetRoadDepotDirection(m_old_tile); + if (exitdir != m_exitdir) { + m_err = EC_NO_WAY; + return false; + } + } + return true; + } + + /** return true if we can enter m_new_tile from m_exitdir */ + FORCEINLINE bool CanEnterNewTile() + { + if (IsRoadTT() && IsStandardRoadStopTile(m_new_tile)) { + /* road stop can be entered from one direction only unless it's a drive-through stop */ + DiagDirection exitdir = GetRoadStopDir(m_new_tile); + if (ReverseDiagDir(exitdir) != m_exitdir) { + m_err = EC_NO_WAY; + return false; + } + } + + /* single tram bits can only be entered from one direction */ + if (IsTram()) { + DiagDirection single_tram = GetSingleTramBit(m_new_tile); + if (single_tram != INVALID_DIAGDIR && single_tram != ReverseDiagDir(m_exitdir)) { + m_err = EC_NO_WAY; + return false; + } + } + + /* road and rail depots can also be entered from one direction only */ + if (IsRoadTT() && IsDepotTypeTile(m_new_tile, TT())) { + DiagDirection exitdir = GetRoadDepotDirection(m_new_tile); + if (ReverseDiagDir(exitdir) != m_exitdir) { + m_err = EC_NO_WAY; + return false; + } + /* don't try to enter other company's depots */ + if (GetTileOwner(m_new_tile) != m_veh_owner) { + m_err = EC_OWNER; + return false; + } + } + if (IsRailTT() && IsDepotTypeTile(m_new_tile, TT())) { + DiagDirection exitdir = GetRailDepotDirection(m_new_tile); + if (ReverseDiagDir(exitdir) != m_exitdir) { + m_err = EC_NO_WAY; + return false; + } + } + + /* rail transport is possible only on tiles with the same owner as vehicle */ + if (IsRailTT() && GetTileOwner(m_new_tile) != m_veh_owner) { + /* different owner */ + m_err = EC_NO_WAY; + return false; + } + + /* rail transport is possible only on compatible rail types */ + if (IsRailTT()) { + RailType rail_type = GetTileRailType(m_new_tile); + if (!HasBit(m_railtypes, rail_type)) { + /* incompatible rail type */ + m_err = EC_RAIL_TYPE; + return false; + } + } + + /* tunnel holes and bridge ramps can be entered only from proper direction */ + if (IsTileType(m_new_tile, MP_TUNNELBRIDGE)) { + if (IsTunnel(m_new_tile)) { + if (!m_is_tunnel) { + DiagDirection tunnel_enterdir = GetTunnelBridgeDirection(m_new_tile); + if (tunnel_enterdir != m_exitdir) { + m_err = EC_NO_WAY; + return false; + } + } + } else { // IsBridge(m_new_tile) + if (!m_is_bridge) { + DiagDirection ramp_enderdir = GetTunnelBridgeDirection(m_new_tile); + if (ramp_enderdir != m_exitdir) { + m_err = EC_NO_WAY; + return false; + } + } + } + } + + /* special handling for rail stations - get to the end of platform */ + if (IsRailTT() && m_is_station) { + /* entered railway station + * get platform length */ + uint length = BaseStation::GetByTile(m_new_tile)->GetPlatformLength(m_new_tile, TrackdirToExitdir(m_old_td)); + /* how big step we must do to get to the last platform tile; */ + m_tiles_skipped = length - 1; + /* move to the platform end */ + TileIndexDiff diff = TileOffsByDiagDir(m_exitdir); + diff *= m_tiles_skipped; + m_new_tile = TILE_ADD(m_new_tile, diff); + return true; + } + + return true; + } + + /** return true if we must reverse (in depots and single tram bits) */ + FORCEINLINE bool ForcedReverse() + { + /* rail and road depots cause reversing */ + if (!IsWaterTT() && IsDepotTypeTile(m_old_tile, TT())) { + DiagDirection exitdir = IsRailTT() ? GetRailDepotDirection(m_old_tile) : GetRoadDepotDirection(m_old_tile); + if (exitdir != m_exitdir) { + /* reverse */ + m_new_tile = m_old_tile; + m_new_td_bits = TrackdirToTrackdirBits(ReverseTrackdir(m_old_td)); + m_exitdir = exitdir; + m_tiles_skipped = 0; + m_is_tunnel = m_is_bridge = m_is_station = false; + return true; + } + } + + /* single tram bits cause reversing */ + if (IsTram() && GetSingleTramBit(m_old_tile) == ReverseDiagDir(m_exitdir)) { + /* reverse */ + m_new_tile = m_old_tile; + m_new_td_bits = TrackdirToTrackdirBits(ReverseTrackdir(m_old_td)); + m_exitdir = ReverseDiagDir(m_exitdir); + m_tiles_skipped = 0; + m_is_tunnel = m_is_bridge = m_is_station = false; + return true; + } + + return false; + } + + /** return true if we successfully reversed at end of road/track */ + FORCEINLINE bool TryReverse() + { + if (IsRoadTT() && !IsTram()) { + /* if we reached the end of road, we can reverse the RV and continue moving */ + m_exitdir = ReverseDiagDir(m_exitdir); + /* new tile will be the same as old one */ + m_new_tile = m_old_tile; + /* set new trackdir bits to all reachable trackdirs */ + QueryNewTileTrackStatus(); + m_new_td_bits &= DiagdirReachesTrackdirs(m_exitdir); + if (m_new_td_bits != TRACKDIR_BIT_NONE) { + /* we have some trackdirs reachable after reversal */ + return true; + } + } + m_err = EC_NO_WAY; + return false; + } + +public: + /** Helper for pathfinders - get min/max speed on the m_old_tile/m_old_td */ + int GetSpeedLimit(int *pmin_speed = NULL) const + { + int min_speed = 0; + int max_speed = INT_MAX; // no limit + + /* for now we handle only on-bridge speed limit */ + if (!IsWaterTT() && IsBridgeTile(m_old_tile)) { + int spd = GetBridgeSpec(GetBridgeType(m_old_tile))->speed; + if (IsRoadTT()) spd *= 2; + if (max_speed > spd) max_speed = spd; + } + + /* if min speed was requested, return it */ + if (pmin_speed) *pmin_speed = min_speed; + return max_speed; + } +}; + +typedef CFollowTrackT<TRANSPORT_WATER, true > CFollowTrackWater; +typedef CFollowTrackT<TRANSPORT_ROAD , true > CFollowTrackRoad; +typedef CFollowTrackT<TRANSPORT_RAIL , true > CFollowTrackRail; + +typedef CFollowTrackT<TRANSPORT_WATER, false> CFollowTrackWaterNo90; +typedef CFollowTrackT<TRANSPORT_ROAD , false> CFollowTrackRoadNo90; +typedef CFollowTrackT<TRANSPORT_RAIL , false> CFollowTrackRailNo90; + +typedef CFollowTrackT<TRANSPORT_RAIL , true , true> CFollowTrackFreeRail; +typedef CFollowTrackT<TRANSPORT_RAIL , false, true> CFollowTrackFreeRailNo90; + +#endif /* FOLLOW_TRACK_HPP */ diff --git a/src/pathfinder/yapf/nodelist.hpp b/src/pathfinder/yapf/nodelist.hpp new file mode 100644 index 000000000..915342e1d --- /dev/null +++ b/src/pathfinder/yapf/nodelist.hpp @@ -0,0 +1,164 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file nodelist.hpp List of nodes used for the A-star pathfinder. */ + +#ifndef NODELIST_HPP +#define NODELIST_HPP + +#include "../../misc/array.hpp" +#include "../../misc/hashtable.hpp" +#include "../../misc/binaryheap.hpp" + +/** Hash table based node list multi-container class. + * Implements open list, closed list and priority queue for A-star + * path finder. */ +template <class Titem_, int Thash_bits_open_, int Thash_bits_closed_> +class CNodeList_HashTableT { +public: + /** make Titem_ visible from outside of class */ + typedef Titem_ Titem; + /** make Titem_::Key a property of HashTable */ + typedef typename Titem_::Key Key; + /** type that we will use as item container */ + typedef CArrayT<Titem_, 65536, 256> CItemArray; + /** how pointers to open nodes will be stored */ + typedef CHashTableT<Titem_, Thash_bits_open_ > COpenList; + /** how pointers to closed nodes will be stored */ + typedef CHashTableT<Titem_, Thash_bits_closed_> CClosedList; + /** how the priority queue will be managed */ + typedef CBinaryHeapT<Titem_> CPriorityQueue; + +protected: + /** here we store full item data (Titem_) */ + CItemArray m_arr; + /** hash table of pointers to open item data */ + COpenList m_open; + /** hash table of pointers to closed item data */ + CClosedList m_closed; + /** priority queue of pointers to open item data */ + CPriorityQueue m_open_queue; + /** new open node under construction */ + Titem *m_new_node; +public: + /** default constructor */ + CNodeList_HashTableT() + : m_open_queue(204800) + { + m_new_node = NULL; + } + + /** destructor */ + ~CNodeList_HashTableT() + { + } + + /** return number of open nodes */ + FORCEINLINE int OpenCount() + { + return m_open.Count(); + } + + /** return number of closed nodes */ + FORCEINLINE int ClosedCount() + { + return m_closed.Count(); + } + + /** allocate new data item from m_arr */ + FORCEINLINE Titem_ *CreateNewNode() + { + if (m_new_node == NULL) m_new_node = &m_arr.Add(); + return m_new_node; + } + + /** notify the nodelist, that we don't want to discard the given node */ + FORCEINLINE void FoundBestNode(Titem_& item) + { + /* for now it is enough to invalidate m_new_node if it is our given node */ + if (&item == m_new_node) { + m_new_node = NULL; + } + /* TODO: do we need to store best nodes found in some extra list/array? Probably not now. */ + } + + /** insert given item as open node (into m_open and m_open_queue) */ + FORCEINLINE void InsertOpenNode(Titem_& item) + { + assert(m_closed.Find(item.GetKey()) == NULL); + m_open.Push(item); + /* TODO: check if m_open_queue is not full */ + assert(!m_open_queue.IsFull()); + m_open_queue.Push(item); + if (&item == m_new_node) { + m_new_node = NULL; + } + } + + /** return the best open node */ + FORCEINLINE Titem_ *GetBestOpenNode() + { + if (!m_open_queue.IsEmpty()) { + Titem_& item = m_open_queue.GetHead(); + return &item; + } + return NULL; + } + + /** remove and return the best open node */ + FORCEINLINE Titem_ *PopBestOpenNode() + { + if (!m_open_queue.IsEmpty()) { + Titem_& item = m_open_queue.PopHead(); + m_open.Pop(item); + return &item; + } + return NULL; + } + + /** return the open node specified by a key or NULL if not found */ + FORCEINLINE Titem_ *FindOpenNode(const Key& key) + { + Titem_ *item = m_open.Find(key); + return item; + } + + /** remove and return the open node specified by a key */ + FORCEINLINE Titem_& PopOpenNode(const Key& key) + { + Titem_& item = m_open.Pop(key); + int idxPop = m_open_queue.FindLinear(item); + m_open_queue.RemoveByIdx(idxPop); + return item; + } + + /** close node */ + FORCEINLINE void InsertClosedNode(Titem_& item) + { + assert(m_open.Find(item.GetKey()) == NULL); + m_closed.Push(item); + } + + /** return the closed node specified by a key or NULL if not found */ + FORCEINLINE Titem_ *FindClosedNode(const Key& key) + { + Titem_ *item = m_closed.Find(key); + return item; + } + + FORCEINLINE int TotalCount() {return m_arr.Size();} + FORCEINLINE Titem_& ItemAt(int idx) {return m_arr[idx];} + + template <class D> void Dump(D &dmp) const + { + dmp.WriteStructT("m_arr", &m_arr); + } +}; + +#endif /* NODELIST_HPP */ diff --git a/src/pathfinder/yapf/yapf.h b/src/pathfinder/yapf/yapf.h new file mode 100644 index 000000000..d55a4f372 --- /dev/null +++ b/src/pathfinder/yapf/yapf.h @@ -0,0 +1,119 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf.h Entry point for OpenTTD to YAPF. */ + +#ifndef YAPF_H +#define YAPF_H + +#include "../../debug.h" +#include "../../depot_type.h" +#include "../../direction_type.h" +#include "../../station_type.h" +#include "../../pbs.h" + +/** Finds the best path for given ship. + * @param v the ship that needs to find a path + * @param tile the tile to find the path from (should be next tile the ship is about to enter) + * @param enterdir diagonal direction which the ship will enter this new tile from + * @param tracks available tracks on the new tile (to choose from) + * @return the best trackdir for next turn or INVALID_TRACKDIR if the path could not be found + */ +Trackdir YapfChooseShipTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir, TrackBits tracks); + +/** Finds the best path for given road vehicle. + * @param v the RV that needs to find a path + * @param tile the tile to find the path from (should be next tile the RV is about to enter) + * @param enterdir diagonal direction which the RV will enter this new tile from + * @return the best trackdir for next turn or INVALID_TRACKDIR if the path could not be found + */ +Trackdir YapfChooseRoadTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir); + +/** Finds the best path for given train. + * @param v the train that needs to find a path + * @param tile the tile to find the path from (should be next tile the train is about to enter) + * @param enterdir diagonal direction which the RV will enter this new tile from + * @param tracks available trackdirs on the new tile (to choose from) + * @param path_not_found [out] true is returned if no path can be found (returned Trackdir is only a 'guess') + * @param reserve_track indicates whether YAPF should try to reserve the found path + * @param target [out] the target tile of the reservation, free is set to true if path was reserved + * @return the best trackdir for next turn or INVALID_TRACKDIR if the path could not be found + */ +Trackdir YapfChooseRailTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir, TrackBits tracks, bool *path_not_found, bool reserve_track, PBSTileInfo *target); + +/** Used by RV multistop feature to find the nearest road stop that has a free slot. + * @param v RV (its current tile will be the origin) + * @param tile destination tile + * @return distance from origin tile to the destination (number of road tiles) or UINT_MAX if path not found + */ +uint YapfRoadVehDistanceToTile(const Vehicle *v, TileIndex tile); + +/** Used to determinine the closest reachable compatible road stop for a given vehicle. + * @param v vehicle that needs to go to the road stop + * @param station the station the road stop must belong to + * @param stop_tile receives the stop tile if a stop was found + * @return true if stop was found. + */ +bool YapfFindNearestRoadVehicleCompatibleStop(const RoadVehicle *v, StationID station, TileIndex *stop_tile); + +/** Used when user sends road vehicle to the nearest depot or if road vehicle needs servicing. + * @param v vehicle that needs to go to some depot + * @param max_distance max distance (number of track tiles) from the current vehicle position + * (used also as optimization - the pathfinder can stop path finding if max_distance + * was reached and no depot was seen) + * @param depot_tile receives the depot tile if depot was found + * @return true if depot was found. + */ +bool YapfFindNearestRoadDepot(const Vehicle *v, int max_distance, TileIndex *depot_tile); + +/** Used when user sends train to the nearest depot or if train needs servicing. + * @param v train that needs to go to some depot + * @param max_distance max distance (number of track tiles) from the current train position + * (used also as optimization - the pathfinder can stop path finding if max_distance + * was reached and no depot was seen) + * @param reverse_penalty penalty that should be added for the path that requires reversing the train first + * @param depot_tile receives the depot tile if depot was found + * @param reversed receives true if train needs to reversed first + * @return true if depot was found. + */ +bool YapfFindNearestRailDepotTwoWay(const Vehicle *v, int max_distance, int reverse_penalty, TileIndex *depot_tile, bool *reversed); + +/** Returns true if it is better to reverse the train before leaving station */ +bool YapfCheckReverseTrain(const Vehicle *v); + +/** + * Try to extend the reserved path of a train to the nearest safe tile. + * + * @param v The train that needs to find a safe tile. + * @param tile Last tile of the current reserved path. + * @param td Last trackdir of the current reserved path. + * @param override_railtype Should all physically compabtible railtypes be searched, even if the vehicle can't on them on it own? + * @return True if the path could be extended to a safe tile. + */ +bool YapfRailFindNearestSafeTile(const Vehicle *v, TileIndex tile, Trackdir td, bool override_railtype); + +/** Use this function to notify YAPF that track layout (or signal configuration) has change */ +void YapfNotifyTrackLayoutChange(TileIndex tile, Track track); + +/** performance measurement helpers */ +void *NpfBeginInterval(); +int NpfEndInterval(void *perf); + + +extern int _aystar_stats_open_size; +extern int _aystar_stats_closed_size; + + +/** Base tile length units */ +enum { + YAPF_TILE_LENGTH = 100, + YAPF_TILE_CORNER_LENGTH = 71 +}; + +#endif /* YAPF_H */ diff --git a/src/pathfinder/yapf/yapf.hpp b/src/pathfinder/yapf/yapf.hpp new file mode 100644 index 000000000..a353902c5 --- /dev/null +++ b/src/pathfinder/yapf/yapf.hpp @@ -0,0 +1,129 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf.hpp Base includes/functions for YAPF. */ + +#ifndef YAPF_HPP +#define YAPF_HPP + +#include "../../openttd.h" +#include "../../vehicle_base.h" +#include "../../road_map.h" +#include "../../tunnel_map.h" +#include "../../bridge_map.h" +#include "../../tunnelbridge_map.h" +#include "../../bridge.h" +#include "../../station_map.h" +#include "../../tile_cmd.h" +#include "../../landscape.h" +#include "yapf.h" +#include "../pathfinder_func.h" +#include "../../waypoint_base.h" +#include "../../debug.h" +#include "../../settings_type.h" +#include "../../tunnelbridge.h" + +extern uint64 ottd_rdtsc(); + +#include <limits.h> +#include <new> + +#if defined(_WIN32) || defined(_WIN64) +# include <windows.h> +#else +# include <time.h> +#endif + +struct CPerformanceTimer +{ + int64 m_start; + int64 m_acc; + + CPerformanceTimer() : m_start(0), m_acc(0) {} + + FORCEINLINE void Start() + { + m_start = QueryTime(); + } + + FORCEINLINE void Stop() + { + m_acc += QueryTime() - m_start; + } + + FORCEINLINE int Get(int64 coef) + { + return (int)(m_acc * coef / QueryFrequency()); + } + + FORCEINLINE int64 QueryTime() + { + return ottd_rdtsc(); + } + + FORCEINLINE int64 QueryFrequency() + { + return ((int64)2200 * 1000000); + } +}; + +struct CPerfStartReal +{ + CPerformanceTimer *m_pperf; + + FORCEINLINE CPerfStartReal(CPerformanceTimer& perf) : m_pperf(&perf) + { + if (m_pperf != NULL) m_pperf->Start(); + } + + FORCEINLINE ~CPerfStartReal() + { + Stop(); + } + + FORCEINLINE void Stop() + { + if (m_pperf != NULL) { + m_pperf->Stop(); + m_pperf = NULL; + } + } +}; + +struct CPerfStartFake +{ + FORCEINLINE CPerfStartFake(CPerformanceTimer& perf) {} + FORCEINLINE ~CPerfStartFake() {} + FORCEINLINE void Stop() {} +}; + +typedef CPerfStartFake CPerfStart; + + +//#undef FORCEINLINE +//#define FORCEINLINE inline + +#include "../../misc/crc32.hpp" +#include "../../misc/blob.hpp" +#include "../../misc/str.hpp" +#include "../../misc/fixedsizearray.hpp" +#include "../../misc/array.hpp" +#include "../../misc/hashtable.hpp" +#include "../../misc/binaryheap.hpp" +#include "../../misc/dbg_helpers.h" +#include "nodelist.hpp" +#include "follow_track.hpp" +#include "yapf_base.hpp" +#include "yapf_node.hpp" +#include "yapf_common.hpp" +#include "yapf_costbase.hpp" +#include "yapf_costcache.hpp" + + +#endif /* YAPF_HPP */ diff --git a/src/pathfinder/yapf/yapf_base.hpp b/src/pathfinder/yapf/yapf_base.hpp new file mode 100644 index 000000000..0011e0802 --- /dev/null +++ b/src/pathfinder/yapf/yapf_base.hpp @@ -0,0 +1,361 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_base.hpp Base classes for YAPF. */ + +#ifndef YAPF_BASE_HPP +#define YAPF_BASE_HPP + +#include "../../debug.h" +#include "../../settings_type.h" + +extern int _total_pf_time_us; + +/** CYapfBaseT - A-star type path finder base class. + * Derive your own pathfinder from it. You must provide the following template argument: + * Types - used as collection of local types used in pathfinder + * + * Requirements for the Types struct: + * ---------------------------------- + * The following types must be defined in the 'Types' argument: + * - Types::Tpf - your pathfinder derived from CYapfBaseT + * - Types::NodeList - open/closed node list (look at CNodeList_HashTableT) + * NodeList needs to have defined local type Titem - defines the pathfinder node type. + * Node needs to define local type Key - the node key in the collection () + * + * For node list you can use template class CNodeList_HashTableT, for which + * you need to declare only your node type. Look at test_yapf.h for an example. + * + * + * Requrements to your pathfinder class derived from CYapfBaseT: + * ------------------------------------------------------------- + * Your pathfinder derived class needs to implement following methods: + * FORCEINLINE void PfSetStartupNodes() + * FORCEINLINE void PfFollowNode(Node& org) + * FORCEINLINE bool PfCalcCost(Node& n) + * FORCEINLINE bool PfCalcEstimate(Node& n) + * FORCEINLINE bool PfDetectDestination(Node& n) + * + * For more details about those methods, look at the end of CYapfBaseT + * declaration. There are some examples. For another example look at + * test_yapf.h (part or unittest project). + */ +template <class Types> +class CYapfBaseT { +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList NodeList; ///< our node list + typedef typename NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + + + NodeList m_nodes; ///< node list multi-container +protected: + Node *m_pBestDestNode; ///< pointer to the destination node found at last round + Node *m_pBestIntermediateNode; ///< here should be node closest to the destination if path not found + const YAPFSettings *m_settings; ///< current settings (_settings_game.yapf) + int m_max_search_nodes; ///< maximum number of nodes we are allowed to visit before we give up + const Vehicle *m_veh; ///< vehicle that we are trying to drive + + int m_stats_cost_calcs; ///< stats - how many node's costs were calculated + int m_stats_cache_hits; ///< stats - how many node's costs were reused from cache + +public: + CPerformanceTimer m_perf_cost; ///< stats - total CPU time of this run + CPerformanceTimer m_perf_slope_cost; ///< stats - slope calculation CPU time + CPerformanceTimer m_perf_ts_cost; ///< stats - GetTrackStatus() CPU time + CPerformanceTimer m_perf_other_cost; ///< stats - other CPU time + +public: + int m_num_steps; ///< this is there for debugging purposes (hope it doesn't hurt) + +public: + /** default constructor */ + FORCEINLINE CYapfBaseT() + : m_pBestDestNode(NULL) + , m_pBestIntermediateNode(NULL) + , m_settings(&_settings_game.pf.yapf) + , m_max_search_nodes(PfGetSettings().max_search_nodes) + , m_veh(NULL) + , m_stats_cost_calcs(0) + , m_stats_cache_hits(0) + , m_num_steps(0) + { + } + + /** default destructor */ + ~CYapfBaseT() {} + +protected: + /** to access inherited path finder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** return current settings (can be custom - company based - but later) */ + FORCEINLINE const YAPFSettings& PfGetSettings() const + { + return *m_settings; + } + + /** Main pathfinder routine: + * - set startup node(s) + * - main loop that stops if: + * - the destination was found + * - or the open list is empty (no route to destination). + * - or the maximum amount of loops reached - m_max_search_nodes (default = 10000) + * @return true if the path was found */ + inline bool FindPath(const Vehicle *v) + { + m_veh = v; + +#ifndef NO_DEBUG_MESSAGES + CPerformanceTimer perf; + perf.Start(); +#endif /* !NO_DEBUG_MESSAGES */ + + Yapf().PfSetStartupNodes(); + + while (true) { + m_num_steps++; + Node *n = m_nodes.GetBestOpenNode(); + if (n == NULL) { + break; + } + + /* if the best open node was worse than the best path found, we can finish */ + if (m_pBestDestNode != NULL && m_pBestDestNode->GetCost() < n->GetCostEstimate()) { + break; + } + + Yapf().PfFollowNode(*n); + if (m_max_search_nodes == 0 || m_nodes.ClosedCount() < m_max_search_nodes) { + m_nodes.PopOpenNode(n->GetKey()); + m_nodes.InsertClosedNode(*n); + } else { + m_pBestDestNode = m_pBestIntermediateNode; + break; + } + } + + bool bDestFound = (m_pBestDestNode != NULL) && (m_pBestDestNode != m_pBestIntermediateNode); + +#ifndef NO_DEBUG_MESSAGES + perf.Stop(); + if (_debug_yapf_level >= 2) { + int t = perf.Get(1000000); + _total_pf_time_us += t; + + if (_debug_yapf_level >= 3) { + UnitID veh_idx = (m_veh != NULL) ? m_veh->unitnumber : 0; + char ttc = Yapf().TransportTypeChar(); + float cache_hit_ratio = (m_stats_cache_hits == 0) ? 0.0f : ((float)m_stats_cache_hits / (float)(m_stats_cache_hits + m_stats_cost_calcs) * 100.0f); + int cost = bDestFound ? m_pBestDestNode->m_cost : -1; + int dist = bDestFound ? m_pBestDestNode->m_estimate - m_pBestDestNode->m_cost : -1; + + DEBUG(yapf, 3, "[YAPF%c]%c%4d- %d us - %d rounds - %d open - %d closed - CHR %4.1f%% - C %d D %d - c%d(sc%d, ts%d, o%d) -- ", + ttc, bDestFound ? '-' : '!', veh_idx, t, m_num_steps, m_nodes.OpenCount(), m_nodes.ClosedCount(), + cache_hit_ratio, cost, dist, m_perf_cost.Get(1000000), m_perf_slope_cost.Get(1000000), + m_perf_ts_cost.Get(1000000), m_perf_other_cost.Get(1000000) + ); + } + } +#endif /* !NO_DEBUG_MESSAGES */ + return bDestFound; + } + + /** If path was found return the best node that has reached the destination. Otherwise + * return the best visited node (which was nearest to the destination). + */ + FORCEINLINE Node *GetBestNode() + { + return (m_pBestDestNode != NULL) ? m_pBestDestNode : m_pBestIntermediateNode; + } + + /** Calls NodeList::CreateNewNode() - allocates new node that can be filled and used + * as argument for AddStartupNode() or AddNewNode() + */ + FORCEINLINE Node& CreateNewNode() + { + Node& node = *m_nodes.CreateNewNode(); + return node; + } + + /** Add new node (created by CreateNewNode and filled with data) into open list */ + FORCEINLINE void AddStartupNode(Node& n) + { + Yapf().PfNodeCacheFetch(n); + /* insert the new node only if it is not there */ + if (m_nodes.FindOpenNode(n.m_key) == NULL) { + m_nodes.InsertOpenNode(n); + } else { + /* if we are here, it means that node is already there - how it is possible? + * probably the train is in the position that both its ends point to the same tile/exit-dir + * very unlikely, but it happened */ + } + } + + /** add multiple nodes - direct children of the given node */ + FORCEINLINE void AddMultipleNodes(Node *parent, const TrackFollower &tf) + { + bool is_choice = (KillFirstBit(tf.m_new_td_bits) != TRACKDIR_BIT_NONE); + for (TrackdirBits rtds = tf.m_new_td_bits; rtds != TRACKDIR_BIT_NONE; rtds = KillFirstBit(rtds)) { + Trackdir td = (Trackdir)FindFirstBit2x64(rtds); + Node& n = Yapf().CreateNewNode(); + n.Set(parent, tf.m_new_tile, td, is_choice); + Yapf().AddNewNode(n, tf); + } + } + + /** AddNewNode() - called by Tderived::PfFollowNode() for each child node. + * Nodes are evaluated here and added into open list */ + void AddNewNode(Node &n, const TrackFollower &tf) + { + /* evaluate the node */ + bool bCached = Yapf().PfNodeCacheFetch(n); + if (!bCached) { + m_stats_cost_calcs++; + } else { + m_stats_cache_hits++; + } + + bool bValid = Yapf().PfCalcCost(n, &tf); + + if (bCached) { + Yapf().PfNodeCacheFlush(n); + } + + if (bValid) bValid = Yapf().PfCalcEstimate(n); + + /* have the cost or estimate callbacks marked this node as invalid? */ + if (!bValid) return; + + /* detect the destination */ + bool bDestination = Yapf().PfDetectDestination(n); + if (bDestination) { + if (m_pBestDestNode == NULL || n < *m_pBestDestNode) { + m_pBestDestNode = &n; + } + m_nodes.FoundBestNode(n); + return; + } + + if (m_max_search_nodes > 0 && (m_pBestIntermediateNode == NULL || (m_pBestIntermediateNode->GetCostEstimate() - m_pBestIntermediateNode->GetCost()) > (n.GetCostEstimate() - n.GetCost()))) { + m_pBestIntermediateNode = &n; + } + + /* check new node against open list */ + Node *openNode = m_nodes.FindOpenNode(n.GetKey()); + if (openNode != NULL) { + /* another node exists with the same key in the open list + * is it better than new one? */ + if (n.GetCostEstimate() < openNode->GetCostEstimate()) { + /* update the old node by value from new one */ + m_nodes.PopOpenNode(n.GetKey()); + *openNode = n; + /* add the updated old node back to open list */ + m_nodes.InsertOpenNode(*openNode); + } + return; + } + + /* check new node against closed list */ + Node *closedNode = m_nodes.FindClosedNode(n.GetKey()); + if (closedNode != NULL) { + /* another node exists with the same key in the closed list + * is it better than new one? */ + int node_est = n.GetCostEstimate(); + int closed_est = closedNode->GetCostEstimate(); + if (node_est < closed_est) { + /* If this assert occurs, you have probably problem in + * your Tderived::PfCalcCost() or Tderived::PfCalcEstimate(). + * The problem could be: + * - PfCalcEstimate() gives too large numbers + * - PfCalcCost() gives too small numbers + * - You have used negative cost penalty in some cases (cost bonus) */ + NOT_REACHED(); + } + return; + } + /* the new node is really new + * add it to the open list */ + m_nodes.InsertOpenNode(n); + } + + const Vehicle * GetVehicle() const + { + return m_veh; + } + + void DumpBase(DumpTarget &dmp) const + { + dmp.WriteStructT("m_nodes", &m_nodes); + dmp.WriteLine("m_num_steps = %d", m_num_steps); + } + + /* methods that should be implemented at derived class Types::Tpf (derived from CYapfBaseT) */ + +#if 0 + /** Example: PfSetStartupNodes() - set source (origin) nodes */ + FORCEINLINE void PfSetStartupNodes() + { + /* example: */ + Node& n1 = *base::m_nodes.CreateNewNode(); + . + . // setup node members here + . + base::m_nodes.InsertOpenNode(n1); + } + + /** Example: PfFollowNode() - set following (child) nodes of the given node */ + FORCEINLINE void PfFollowNode(Node& org) + { + for (each follower of node org) { + Node& n = *base::m_nodes.CreateNewNode(); + . + . // setup node members here + . + n.m_parent = &org; // set node's parent to allow back tracking + AddNewNode(n); + } + } + + /** Example: PfCalcCost() - set path cost from origin to the given node */ + FORCEINLINE bool PfCalcCost(Node& n) + { + /* evaluate last step cost */ + int cost = ...; + /* set the node cost as sum of parent's cost and last step cost */ + n.m_cost = n.m_parent->m_cost + cost; + return true; // true if node is valid follower (i.e. no obstacle was found) + } + + /** Example: PfCalcEstimate() - set path cost estimate from origin to the target through given node */ + FORCEINLINE bool PfCalcEstimate(Node& n) + { + /* evaluate the distance to our destination */ + int distance = ...; + /* set estimate as sum of cost from origin + distance to the target */ + n.m_estimate = n.m_cost + distance; + return true; // true if node is valid (i.e. not too far away :) + } + + /** Example: PfDetectDestination() - return true if the given node is our destination */ + FORCEINLINE bool PfDetectDestination(Node& n) + { + bool bDest = (n.m_key.m_x == m_x2) && (n.m_key.m_y == m_y2); + return bDest; + } +#endif +}; + +#endif /* YAPF_BASE_HPP */ diff --git a/src/pathfinder/yapf/yapf_common.hpp b/src/pathfinder/yapf/yapf_common.hpp new file mode 100644 index 000000000..51c2fc12a --- /dev/null +++ b/src/pathfinder/yapf/yapf_common.hpp @@ -0,0 +1,194 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_common.hpp Commonly used classes for YAPF. */ + +#ifndef YAPF_COMMON_HPP +#define YAPF_COMMON_HPP + +/** YAPF origin provider base class - used when origin is one tile / multiple trackdirs */ +template <class Types> +class CYapfOriginTileT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + TileIndex m_orgTile; ///< origin tile + TrackdirBits m_orgTrackdirs; ///< origin trackdir mask + + /** to access inherited path finder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** Set origin tile / trackdir mask */ + void SetOrigin(TileIndex tile, TrackdirBits trackdirs) + { + m_orgTile = tile; + m_orgTrackdirs = trackdirs; + } + + /** Called when YAPF needs to place origin nodes into open list */ + void PfSetStartupNodes() + { + bool is_choice = (KillFirstBit(m_orgTrackdirs) != TRACKDIR_BIT_NONE); + for (TrackdirBits tdb = m_orgTrackdirs; tdb != TRACKDIR_BIT_NONE; tdb = KillFirstBit(tdb)) { + Trackdir td = (Trackdir)FindFirstBit2x64(tdb); + Node& n1 = Yapf().CreateNewNode(); + n1.Set(NULL, m_orgTile, td, is_choice); + Yapf().AddStartupNode(n1); + } + } +}; + +/** YAPF origin provider base class - used when there are two tile/trackdir origins */ +template <class Types> +class CYapfOriginTileTwoWayT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + TileIndex m_orgTile; ///< first origin tile + Trackdir m_orgTd; ///< first origin trackdir + TileIndex m_revTile; ///< second (reversed) origin tile + Trackdir m_revTd; ///< second (reversed) origin trackdir + int m_reverse_penalty; ///< penalty to be added for using the reversed origin + bool m_treat_first_red_two_way_signal_as_eol; ///< in some cases (leaving station) we need to handle first two-way signal differently + + /** to access inherited path finder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** set origin (tiles, trackdirs, etc.) */ + void SetOrigin(TileIndex tile, Trackdir td, TileIndex tiler = INVALID_TILE, Trackdir tdr = INVALID_TRACKDIR, int reverse_penalty = 0, bool treat_first_red_two_way_signal_as_eol = true) + { + m_orgTile = tile; + m_orgTd = td; + m_revTile = tiler; + m_revTd = tdr; + m_reverse_penalty = reverse_penalty; + m_treat_first_red_two_way_signal_as_eol = treat_first_red_two_way_signal_as_eol; + } + + /** Called when YAPF needs to place origin nodes into open list */ + void PfSetStartupNodes() + { + if (m_orgTile != INVALID_TILE && m_orgTd != INVALID_TRACKDIR) { + Node& n1 = Yapf().CreateNewNode(); + n1.Set(NULL, m_orgTile, m_orgTd, false); + Yapf().AddStartupNode(n1); + } + if (m_revTile != INVALID_TILE && m_revTd != INVALID_TRACKDIR) { + Node& n2 = Yapf().CreateNewNode(); + n2.Set(NULL, m_revTile, m_revTd, false); + n2.m_cost = m_reverse_penalty; + Yapf().AddStartupNode(n2); + } + } + + /** return true if first two-way signal should be treated as dead end */ + FORCEINLINE bool TreatFirstRedTwoWaySignalAsEOL() + { + return Yapf().PfGetSettings().rail_firstred_twoway_eol && m_treat_first_red_two_way_signal_as_eol; + } +}; + +/** YAPF destination provider base class - used when destination is single tile / multiple trackdirs */ +template <class Types> +class CYapfDestinationTileT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + TileIndex m_destTile; ///< destination tile + TrackdirBits m_destTrackdirs; ///< destination trackdir mask + +public: + /** set the destination tile / more trackdirs */ + void SetDestination(TileIndex tile, TrackdirBits trackdirs) + { + m_destTile = tile; + m_destTrackdirs = trackdirs; + } + +protected: + /** to access inherited path finder */ + Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** Called by YAPF to detect if node ends in the desired destination */ + FORCEINLINE bool PfDetectDestination(Node& n) + { + bool bDest = (n.m_key.m_tile == m_destTile) && ((m_destTrackdirs & TrackdirToTrackdirBits(n.GetTrackdir())) != TRACKDIR_BIT_NONE); + return bDest; + } + + /** Called by YAPF to calculate cost estimate. Calculates distance to the destination + * adds it to the actual cost from origin and stores the sum to the Node::m_estimate */ + inline bool PfCalcEstimate(Node& n) + { + static const int dg_dir_to_x_offs[] = {-1, 0, 1, 0}; + static const int dg_dir_to_y_offs[] = {0, 1, 0, -1}; + if (PfDetectDestination(n)) { + n.m_estimate = n.m_cost; + return true; + } + + TileIndex tile = n.GetTile(); + DiagDirection exitdir = TrackdirToExitdir(n.GetTrackdir()); + int x1 = 2 * TileX(tile) + dg_dir_to_x_offs[(int)exitdir]; + int y1 = 2 * TileY(tile) + dg_dir_to_y_offs[(int)exitdir]; + int x2 = 2 * TileX(m_destTile); + int y2 = 2 * TileY(m_destTile); + int dx = abs(x1 - x2); + int dy = abs(y1 - y2); + int dmin = min(dx, dy); + int dxy = abs(dx - dy); + int d = dmin * YAPF_TILE_CORNER_LENGTH + (dxy - 1) * (YAPF_TILE_LENGTH / 2); + n.m_estimate = n.m_cost + d; + assert(n.m_estimate >= n.m_parent->m_estimate); + return true; + } +}; + +/** YAPF template that uses Ttypes template argument to determine all YAPF + * components (base classes) from which the actual YAPF is composed. + * For example classes consult: CYapfRail_TypesT template and its instantiations: + * CYapfRail1, CYapfRail2, CYapfRail3, CYapfAnyDepotRail1, CYapfAnyDepotRail2, CYapfAnyDepotRail3 */ +template <class Ttypes> +class CYapfT + : public Ttypes::PfBase ///< Instance of CYapfBaseT - main YAPF loop and support base class + , public Ttypes::PfCost ///< Cost calculation provider base class + , public Ttypes::PfCache ///< Segment cost cache provider + , public Ttypes::PfOrigin ///< Origin (tile or two-tile origin) + , public Ttypes::PfDestination ///< Destination detector and distance (estimate) calculation provider + , public Ttypes::PfFollow ///< Node follower (stepping provider) +{ +}; + + + +#endif /* YAPF_COMMON_HPP */ diff --git a/src/pathfinder/yapf/yapf_costbase.hpp b/src/pathfinder/yapf/yapf_costbase.hpp new file mode 100644 index 000000000..39424dc10 --- /dev/null +++ b/src/pathfinder/yapf/yapf_costbase.hpp @@ -0,0 +1,42 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_costbase.hpp Handling of cost determination. */ + +#ifndef YAPF_COSTBASE_HPP +#define YAPF_COSTBASE_HPP + +struct CYapfCostBase { + FORCEINLINE static bool stSlopeCost(TileIndex tile, Trackdir td) + { + if (IsDiagonalTrackdir(td)) { + if (IsBridgeTile(tile)) { + /* it is bridge ramp, check if we are entering the bridge */ + if (GetTunnelBridgeDirection(tile) != TrackdirToExitdir(td)) return false; // no, we are leaving it, no penalty + /* we are entering the bridge */ + Slope tile_slope = GetTileSlope(tile, NULL); + Axis axis = DiagDirToAxis(GetTunnelBridgeDirection(tile)); + return !HasBridgeFlatRamp(tile_slope, axis); + } else { + /* not bridge ramp */ + if (IsTunnelTile(tile)) return false; // tunnel entry/exit doesn't slope + Slope tile_slope = GetTileSlope(tile, NULL); + return IsUphillTrackdir(tile_slope, td); // slopes uphill => apply penalty + } + } + return false; + } +}; + +struct CostRailSettings { + /* look-ahead signal penalty */ +}; + + +#endif /* YAPF_COSTBASE_HPP */ diff --git a/src/pathfinder/yapf/yapf_costcache.hpp b/src/pathfinder/yapf/yapf_costcache.hpp new file mode 100644 index 000000000..810579120 --- /dev/null +++ b/src/pathfinder/yapf/yapf_costcache.hpp @@ -0,0 +1,216 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_costcache.hpp Caching of segment costs. */ + +#ifndef YAPF_COSTCACHE_HPP +#define YAPF_COSTCACHE_HPP + +#include "../../date_func.h" + +/** CYapfSegmentCostCacheNoneT - the formal only yapf cost cache provider that implements + * PfNodeCacheFetch() and PfNodeCacheFlush() callbacks. Used when nodes don't have CachedData + * defined (they don't count with any segment cost caching). + */ +template <class Types> +class CYapfSegmentCostCacheNoneT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + + /** Called by YAPF to attach cached or local segment cost data to the given node. + * @return true if globally cached data were used or false if local data was used */ + FORCEINLINE bool PfNodeCacheFetch(Node& n) + { + return false; + } + + /** Called by YAPF to flush the cached segment cost data back into cache storage. + * Current cache implementation doesn't use that. */ + FORCEINLINE void PfNodeCacheFlush(Node& n) + { + } +}; + + +/** CYapfSegmentCostCacheLocalT - the yapf cost cache provider that implements fake segment + * cost caching functionality for yapf. Used when node needs caching, but you don't want to + * cache the segment costs. + */ +template <class Types> +class CYapfSegmentCostCacheLocalT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + typedef typename Node::CachedData CachedData; + typedef typename CachedData::Key CacheKey; + typedef CArrayT<CachedData> LocalCache; + +protected: + LocalCache m_local_cache; + + /** to access inherited path finder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** Called by YAPF to attach cached or local segment cost data to the given node. + * @return true if globally cached data were used or false if local data was used */ + FORCEINLINE bool PfNodeCacheFetch(Node& n) + { + CacheKey key(n.GetKey()); + Yapf().ConnectNodeToCachedData(n, *new (&m_local_cache.AddNC()) CachedData(key)); + return false; + } + + /** Called by YAPF to flush the cached segment cost data back into cache storage. + * Current cache implementation doesn't use that. */ + FORCEINLINE void PfNodeCacheFlush(Node& n) + { + } +}; + + +/** Base class for segment cost cache providers. Contains global counter + * of track layout changes and static notification function called whenever + * the track layout changes. It is implemented as base class because it needs + * to be shared between all rail YAPF types (one shared counter, one notification + * function. */ +struct CSegmentCostCacheBase +{ + static int s_rail_change_counter; + + static void NotifyTrackLayoutChange(TileIndex tile, Track track) + { + s_rail_change_counter++; + } +}; + + +/** CSegmentCostCacheT - template class providing hash-map and storage (heap) + * of Tsegment structures. Each rail node contains pointer to the segment + * that contains cached (or non-cached) segment cost information. Nodes can + * differ by key type, but they use the same segment type. Segment key should + * be always the same (TileIndex + DiagDirection) that represent the beginning + * of the segment (origin tile and exit-dir from this tile). + * Different CYapfCachedCostT types can share the same type of CSegmentCostCacheT. + * Look at CYapfRailSegment (yapf_node_rail.hpp) for the segment example */ +template <class Tsegment> +struct CSegmentCostCacheT + : public CSegmentCostCacheBase +{ + enum {c_hash_bits = 14}; + + typedef CHashTableT<Tsegment, c_hash_bits> HashTable; + typedef CArrayT<Tsegment> Heap; + typedef typename Tsegment::Key Key; ///< key to hash table + + HashTable m_map; + Heap m_heap; + + FORCEINLINE CSegmentCostCacheT() {} + + /** flush (clear) the cache */ + FORCEINLINE void Flush() + { + m_map.Clear(); + m_heap.Clear(); + } + + FORCEINLINE Tsegment& Get(Key& key, bool *found) + { + Tsegment *item = m_map.Find(key); + if (item == NULL) { + *found = false; + item = new (&m_heap.AddNC()) Tsegment(key); + m_map.Push(*item); + } else { + *found = true; + } + return *item; + } +}; + +/** CYapfSegmentCostCacheGlobalT - the yapf cost cache provider that adds the segment cost + * caching functionality to yapf. Using this class as base of your will provide the global + * segment cost caching services for your Nodes. + */ +template <class Types> +class CYapfSegmentCostCacheGlobalT + : public CYapfSegmentCostCacheLocalT<Types> +{ +public: + typedef CYapfSegmentCostCacheLocalT<Types> Tlocal; + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + typedef typename Node::CachedData CachedData; + typedef typename CachedData::Key CacheKey; + typedef CSegmentCostCacheT<CachedData> Cache; + +protected: + Cache& m_global_cache; + + FORCEINLINE CYapfSegmentCostCacheGlobalT() : m_global_cache(stGetGlobalCache()) {}; + + /** to access inherited path finder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + + FORCEINLINE static Cache& stGetGlobalCache() + { + static int last_rail_change_counter = 0; + static Date last_date = 0; + static Cache C; + + /* some statistics */ + if (last_date != _date) { + last_date = _date; + DEBUG(yapf, 2, "Pf time today: %5d ms", _total_pf_time_us / 1000); + _total_pf_time_us = 0; + } + + /* delete the cache sometimes... */ + if (last_rail_change_counter != Cache::s_rail_change_counter) { + last_rail_change_counter = Cache::s_rail_change_counter; + C.Flush(); + } + return C; + } + +public: + /** Called by YAPF to attach cached or local segment cost data to the given node. + * @return true if globally cached data were used or false if local data was used */ + FORCEINLINE bool PfNodeCacheFetch(Node& n) + { + if (!Yapf().CanUseGlobalCache(n)) { + return Tlocal::PfNodeCacheFetch(n); + } + CacheKey key(n.GetKey()); + bool found; + CachedData& item = m_global_cache.Get(key, &found); + Yapf().ConnectNodeToCachedData(n, item); + return found; + } + + /** Called by YAPF to flush the cached segment cost data back into cache storage. + * Current cache implementation doesn't use that. */ + FORCEINLINE void PfNodeCacheFlush(Node& n) + { + } +}; + +#endif /* YAPF_COSTCACHE_HPP */ diff --git a/src/pathfinder/yapf/yapf_costrail.hpp b/src/pathfinder/yapf/yapf_costrail.hpp new file mode 100644 index 000000000..51c8e3920 --- /dev/null +++ b/src/pathfinder/yapf/yapf_costrail.hpp @@ -0,0 +1,591 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_costrail.hpp Cost determination for rails. */ + +#ifndef YAPF_COSTRAIL_HPP +#define YAPF_COSTRAIL_HPP + +#include "../../pbs.h" + +template <class Types> +class CYapfCostRailT + : public CYapfCostBase + , public CostRailSettings +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + typedef typename Node::CachedData CachedData; + +protected: + + /* Structure used inside PfCalcCost() to keep basic tile information. */ + struct TILE { + TileIndex tile; + Trackdir td; + TileType tile_type; + RailType rail_type; + + TILE() + { + tile = INVALID_TILE; + td = INVALID_TRACKDIR; + tile_type = MP_VOID; + rail_type = INVALID_RAILTYPE; + } + + TILE(TileIndex tile, Trackdir td) + { + this->tile = tile; + this->td = td; + this->tile_type = GetTileType(tile); + this->rail_type = GetTileRailType(tile); + } + + TILE(const TILE &src) + { + tile = src.tile; + td = src.td; + tile_type = src.tile_type; + rail_type = src.rail_type; + } + }; + +protected: + /** + * @note maximum cost doesn't work with caching enabled + * @todo fix maximum cost failing with caching (e.g. FS#2900) + */ + int m_max_cost; + CBlobT<int> m_sig_look_ahead_costs; + bool m_disable_cache; + +public: + bool m_stopped_on_first_two_way_signal; +protected: + + static const int s_max_segment_cost = 10000; + + CYapfCostRailT() + : m_max_cost(0) + , m_disable_cache(false) + , m_stopped_on_first_two_way_signal(false) + { + /* pre-compute look-ahead penalties into array */ + int p0 = Yapf().PfGetSettings().rail_look_ahead_signal_p0; + int p1 = Yapf().PfGetSettings().rail_look_ahead_signal_p1; + int p2 = Yapf().PfGetSettings().rail_look_ahead_signal_p2; + int *pen = m_sig_look_ahead_costs.GrowSizeNC(Yapf().PfGetSettings().rail_look_ahead_max_signals); + for (uint i = 0; i < Yapf().PfGetSettings().rail_look_ahead_max_signals; i++) { + pen[i] = p0 + i * (p1 + i * p2); + } + } + + /** to access inherited path finder */ + Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + FORCEINLINE int SlopeCost(TileIndex tile, Trackdir td) + { + CPerfStart perf_cost(Yapf().m_perf_slope_cost); + if (!stSlopeCost(tile, td)) return 0; + return Yapf().PfGetSettings().rail_slope_penalty; + } + + FORCEINLINE int CurveCost(Trackdir td1, Trackdir td2) + { + assert(IsValidTrackdir(td1)); + assert(IsValidTrackdir(td2)); + int cost = 0; + if (TrackFollower::Allow90degTurns() + && ((TrackdirToTrackdirBits(td2) & (TrackdirBits)TrackdirCrossesTrackdirs(td1)) != 0)) { + /* 90-deg curve penalty */ + cost += Yapf().PfGetSettings().rail_curve90_penalty; + } else if (td2 != NextTrackdir(td1)) { + /* 45-deg curve penalty */ + cost += Yapf().PfGetSettings().rail_curve45_penalty; + } + return cost; + } + + FORCEINLINE int SwitchCost(TileIndex tile1, TileIndex tile2, DiagDirection exitdir) + { + if (IsPlainRailTile(tile1) && IsPlainRailTile(tile2)) { + bool t1 = KillFirstBit(GetTrackBits(tile1) & DiagdirReachesTracks(ReverseDiagDir(exitdir))) != TRACK_BIT_NONE; + bool t2 = KillFirstBit(GetTrackBits(tile2) & DiagdirReachesTracks(exitdir)) != TRACK_BIT_NONE; + if (t1 && t2) return Yapf().PfGetSettings().rail_doubleslip_penalty; + } + return 0; + } + + /** Return one tile cost (base cost + level crossing penalty). */ + FORCEINLINE int OneTileCost(TileIndex& tile, Trackdir trackdir) + { + int cost = 0; + /* set base cost */ + if (IsDiagonalTrackdir(trackdir)) { + cost += YAPF_TILE_LENGTH; + switch (GetTileType(tile)) { + case MP_ROAD: + /* Increase the cost for level crossings */ + if (IsLevelCrossing(tile)) { + cost += Yapf().PfGetSettings().rail_crossing_penalty; + } + break; + + default: + break; + } + } else { + /* non-diagonal trackdir */ + cost = YAPF_TILE_CORNER_LENGTH; + } + return cost; + } + + /** Check for a reserved station platform. */ + FORCEINLINE bool IsAnyStationTileReserved(TileIndex tile, Trackdir trackdir, int skipped) + { + TileIndexDiff diff = TileOffsByDiagDir(TrackdirToExitdir(ReverseTrackdir(trackdir))); + for (; skipped >= 0; skipped--, tile += diff) { + if (HasStationReservation(tile)) return true; + } + return false; + } + + /** The cost for reserved tiles, including skipped ones. */ + FORCEINLINE int ReservationCost(Node& n, TileIndex tile, Trackdir trackdir, int skipped) + { + if (n.m_num_signals_passed >= m_sig_look_ahead_costs.Size() / 2) return 0; + + if (IsRailStationTile(tile) && IsAnyStationTileReserved(tile, trackdir, skipped)) { + return Yapf().PfGetSettings().rail_pbs_station_penalty * (skipped + 1); + } else if (TrackOverlapsTracks(GetReservedTrackbits(tile), TrackdirToTrack(trackdir))) { + int cost = Yapf().PfGetSettings().rail_pbs_cross_penalty; + if (!IsDiagonalTrackdir(trackdir)) cost = (cost * YAPF_TILE_CORNER_LENGTH) / YAPF_TILE_LENGTH; + return cost * (skipped + 1); + } + return 0; + } + + int SignalCost(Node& n, TileIndex tile, Trackdir trackdir) + { + int cost = 0; + /* if there is one-way signal in the opposite direction, then it is not our way */ + CPerfStart perf_cost(Yapf().m_perf_other_cost); + if (IsTileType(tile, MP_RAILWAY)) { + bool has_signal_against = HasSignalOnTrackdir(tile, ReverseTrackdir(trackdir)); + bool has_signal_along = HasSignalOnTrackdir(tile, trackdir); + if (has_signal_against && !has_signal_along && IsOnewaySignal(tile, TrackdirToTrack(trackdir))) { + /* one-way signal in opposite direction */ + n.m_segment->m_end_segment_reason |= ESRB_DEAD_END; + } else { + if (has_signal_along) { + SignalState sig_state = GetSignalStateByTrackdir(tile, trackdir); + /* cache the look-ahead polynomial constant only if we didn't pass more signals than the look-ahead limit is */ + int look_ahead_cost = (n.m_num_signals_passed < m_sig_look_ahead_costs.Size()) ? m_sig_look_ahead_costs.Data()[n.m_num_signals_passed] : 0; + if (sig_state != SIGNAL_STATE_RED) { + /* green signal */ + n.flags_u.flags_s.m_last_signal_was_red = false; + /* negative look-ahead red-signal penalties would cause problems later, so use them as positive penalties for green signal */ + if (look_ahead_cost < 0) { + /* add its negation to the cost */ + cost -= look_ahead_cost; + } + } else { + SignalType sig_type = GetSignalType(tile, TrackdirToTrack(trackdir)); + /* we have a red signal in our direction + * was it first signal which is two-way? */ + if (!IsPbsSignal(sig_type) && Yapf().TreatFirstRedTwoWaySignalAsEOL() && n.flags_u.flags_s.m_choice_seen && has_signal_against && n.m_num_signals_passed == 0) { + /* yes, the first signal is two-way red signal => DEAD END */ + n.m_segment->m_end_segment_reason |= ESRB_DEAD_END; + Yapf().m_stopped_on_first_two_way_signal = true; + return -1; + } + n.m_last_red_signal_type = sig_type; + n.flags_u.flags_s.m_last_signal_was_red = true; + + /* look-ahead signal penalty */ + if (!IsPbsSignal(sig_type) && look_ahead_cost > 0) { + /* add the look ahead penalty only if it is positive */ + cost += look_ahead_cost; + } + + /* special signal penalties */ + if (n.m_num_signals_passed == 0) { + switch (sig_type) { + case SIGTYPE_COMBO: + case SIGTYPE_EXIT: cost += Yapf().PfGetSettings().rail_firstred_exit_penalty; break; // first signal is red pre-signal-exit + case SIGTYPE_NORMAL: + case SIGTYPE_ENTRY: cost += Yapf().PfGetSettings().rail_firstred_penalty; break; + default: break; + } + } + } + + n.m_num_signals_passed++; + n.m_segment->m_last_signal_tile = tile; + n.m_segment->m_last_signal_td = trackdir; + } + + if (has_signal_against && IsPbsSignal(GetSignalType(tile, TrackdirToTrack(trackdir)))) { + cost += n.m_num_signals_passed < Yapf().PfGetSettings().rail_look_ahead_max_signals ? Yapf().PfGetSettings().rail_pbs_signal_back_penalty : 0; + } + } + } + return cost; + } + + FORCEINLINE int PlatformLengthPenalty(int platform_length) + { + int cost = 0; + const Vehicle *v = Yapf().GetVehicle(); + assert(v != NULL); + assert(v->type == VEH_TRAIN); + assert(Train::From(v)->tcache.cached_total_length != 0); + int missing_platform_length = (Train::From(v)->tcache.cached_total_length + TILE_SIZE - 1) / TILE_SIZE - platform_length; + if (missing_platform_length < 0) { + /* apply penalty for longer platform than needed */ + cost += Yapf().PfGetSettings().rail_longer_platform_penalty + Yapf().PfGetSettings().rail_longer_platform_per_tile_penalty * -missing_platform_length; + } else if (missing_platform_length > 0) { + /* apply penalty for shorter platform than needed */ + cost += Yapf().PfGetSettings().rail_shorter_platform_penalty + Yapf().PfGetSettings().rail_shorter_platform_per_tile_penalty * missing_platform_length; + } + return cost; + } + +public: + FORCEINLINE void SetMaxCost(int max_cost) + { + m_max_cost = max_cost; + } + + /** Called by YAPF to calculate the cost from the origin to the given node. + * Calculates only the cost of given node, adds it to the parent node cost + * and stores the result into Node::m_cost member */ + FORCEINLINE bool PfCalcCost(Node &n, const TrackFollower *tf) + { + assert(!n.flags_u.flags_s.m_targed_seen); + assert(tf->m_new_tile == n.m_key.m_tile); + assert((TrackdirToTrackdirBits(n.m_key.m_td) & tf->m_new_td_bits) != TRACKDIR_BIT_NONE); + + CPerfStart perf_cost(Yapf().m_perf_cost); + + /* Does the node have some parent node? */ + bool has_parent = (n.m_parent != NULL); + + /* Do we already have a cached segment? */ + CachedData &segment = *n.m_segment; + bool is_cached_segment = (segment.m_cost >= 0); + + int parent_cost = has_parent ? n.m_parent->m_cost : 0; + + /* Each node cost contains 2 or 3 main components: + * 1. Transition cost - cost of the move from previous node (tile): + * - curve cost (or zero for straight move) + * 2. Tile cost: + * - base tile cost + * - YAPF_TILE_LENGTH for diagonal tiles + * - YAPF_TILE_CORNER_LENGTH for non-diagonal tiles + * - tile penalties + * - tile slope penalty (upward slopes) + * - red signal penalty + * - level crossing penalty + * - speed-limit penalty (bridges) + * - station platform penalty + * - penalty for reversing in the depot + * - etc. + * 3. Extra cost (applies to the last node only) + * - last red signal penalty + * - penalty for too long or too short platform on the destination station + */ + int transition_cost = 0; + int extra_cost = 0; + + /* Segment: one or more tiles connected by contiguous tracks of the same type. + * Each segment cost includes 'Tile cost' for all its tiles (including the first + * and last), and the 'Transition cost' between its tiles. The first transition + * cost of segment entry (move from the 'parent' node) is not included! + */ + int segment_entry_cost = 0; + int segment_cost = 0; + + const Vehicle *v = Yapf().GetVehicle(); + + /* start at n.m_key.m_tile / n.m_key.m_td and walk to the end of segment */ + TILE cur(n.m_key.m_tile, n.m_key.m_td); + + /* the previous tile will be needed for transition cost calculations */ + TILE prev = !has_parent ? TILE() : TILE(n.m_parent->GetLastTile(), n.m_parent->GetLastTrackdir()); + + EndSegmentReasonBits end_segment_reason = ESRB_NONE; + + TrackFollower tf_local(v, Yapf().GetCompatibleRailTypes(), &Yapf().m_perf_ts_cost); + + if (!has_parent) { + /* We will jump to the middle of the cost calculator assuming that segment cache is not used. */ + assert(!is_cached_segment); + /* Skip the first transition cost calculation. */ + goto no_entry_cost; + } + + for (;;) { + /* Transition cost (cost of the move from previous tile) */ + transition_cost = Yapf().CurveCost(prev.td, cur.td); + transition_cost += Yapf().SwitchCost(prev.tile, cur.tile, TrackdirToExitdir(prev.td)); + + /* First transition cost counts against segment entry cost, other transitions + * inside segment will come to segment cost (and will be cached) */ + if (segment_cost == 0) { + /* We just entered the loop. First transition cost goes to segment entry cost)*/ + segment_entry_cost = transition_cost; + transition_cost = 0; + + /* It is the right time now to look if we can reuse the cached segment cost. */ + if (is_cached_segment) { + /* Yes, we already know the segment cost. */ + segment_cost = segment.m_cost; + /* We know also the reason why the segment ends. */ + end_segment_reason = segment.m_end_segment_reason; + /* We will need also some information about the last signal (if it was red). */ + if (segment.m_last_signal_tile != INVALID_TILE) { + assert(HasSignalOnTrackdir(segment.m_last_signal_tile, segment.m_last_signal_td)); + SignalState sig_state = GetSignalStateByTrackdir(segment.m_last_signal_tile, segment.m_last_signal_td); + bool is_red = (sig_state == SIGNAL_STATE_RED); + n.flags_u.flags_s.m_last_signal_was_red = is_red; + if (is_red) { + n.m_last_red_signal_type = GetSignalType(segment.m_last_signal_tile, TrackdirToTrack(segment.m_last_signal_td)); + } + } + /* No further calculation needed. */ + cur = TILE(n.GetLastTile(), n.GetLastTrackdir()); + break; + } + } else { + /* Other than first transition cost count as the regular segment cost. */ + segment_cost += transition_cost; + } + +no_entry_cost: // jump here at the beginning if the node has no parent (it is the first node) + + /* All other tile costs will be calculated here. */ + segment_cost += Yapf().OneTileCost(cur.tile, cur.td); + + /* If we skipped some tunnel/bridge/station tiles, add their base cost */ + segment_cost += YAPF_TILE_LENGTH * tf->m_tiles_skipped; + + /* Slope cost. */ + segment_cost += Yapf().SlopeCost(cur.tile, cur.td); + + /* Reserved tiles. */ + segment_cost += Yapf().ReservationCost(n, cur.tile, cur.td, tf->m_tiles_skipped); + + /* Signal cost (routine can modify segment data). */ + segment_cost += Yapf().SignalCost(n, cur.tile, cur.td); + end_segment_reason = segment.m_end_segment_reason; + + /* Tests for 'potential target' reasons to close the segment. */ + if (cur.tile == prev.tile) { + /* Penalty for reversing in a depot. */ + assert(IsRailDepot(cur.tile)); + segment_cost += Yapf().PfGetSettings().rail_depot_reverse_penalty; + /* We will end in this pass (depot is possible target) */ + end_segment_reason |= ESRB_DEPOT; + + } else if (tf->m_is_station) { + /* Station penalties. */ + uint platform_length = tf->m_tiles_skipped + 1; + /* We don't know yet if the station is our target or not. Act like + * if it is pass-through station (not our destination). */ + segment_cost += Yapf().PfGetSettings().rail_station_penalty * platform_length; + /* We will end in this pass (station is possible target) */ + end_segment_reason |= ESRB_STATION; + + } else if (cur.tile_type == MP_STATION && IsRailWaypoint(cur.tile)) { + /* Waypoint is also a good reason to finish. */ + end_segment_reason |= ESRB_WAYPOINT; + } else if (TrackFollower::DoTrackMasking() && cur.tile_type == MP_RAILWAY) { + /* Searching for a safe tile? */ + if (HasSignalOnTrackdir(cur.tile, cur.td) && !IsPbsSignal(GetSignalType(cur.tile, TrackdirToTrack(cur.td)))) { + end_segment_reason |= ESRB_SAFE_TILE; + } + } + + /* Apply min/max speed penalties only when inside the look-ahead radius. Otherwise + * it would cause desync in MP. */ + if (n.m_num_signals_passed < m_sig_look_ahead_costs.Size()) + { + int min_speed = 0; + int max_speed = tf->GetSpeedLimit(&min_speed); + if (max_speed < v->max_speed) { + extra_cost += YAPF_TILE_LENGTH * (v->max_speed - max_speed) * (4 + tf->m_tiles_skipped) / v->max_speed; + } + if (min_speed > v->max_speed) { + extra_cost += YAPF_TILE_LENGTH * (min_speed - v->max_speed); + } + } + + /* Finish if we already exceeded the maximum path cost (i.e. when + * searching for the nearest depot). */ + if (m_max_cost > 0 && (parent_cost + segment_entry_cost + segment_cost) > m_max_cost) { + end_segment_reason |= ESRB_PATH_TOO_LONG; + } + + /* Move to the next tile/trackdir. */ + tf = &tf_local; + tf_local.Init(v, Yapf().GetCompatibleRailTypes(), &Yapf().m_perf_ts_cost); + + if (!tf_local.Follow(cur.tile, cur.td)) { + assert(tf_local.m_err != TrackFollower::EC_NONE); + /* Can't move to the next tile (EOL?). */ + if (tf_local.m_err == TrackFollower::EC_RAIL_TYPE) { + end_segment_reason |= ESRB_RAIL_TYPE; + } else { + end_segment_reason |= ESRB_DEAD_END; + } + + if (TrackFollower::DoTrackMasking() && !HasOnewaySignalBlockingTrackdir(cur.tile, cur.td)) { + end_segment_reason |= ESRB_SAFE_TILE; + } + break; + } + + /* Check if the next tile is not a choice. */ + if (KillFirstBit(tf_local.m_new_td_bits) != TRACKDIR_BIT_NONE) { + /* More than one segment will follow. Close this one. */ + end_segment_reason |= ESRB_CHOICE_FOLLOWS; + break; + } + + /* Gather the next tile/trackdir/tile_type/rail_type. */ + TILE next(tf_local.m_new_tile, (Trackdir)FindFirstBit2x64(tf_local.m_new_td_bits)); + + if (TrackFollower::DoTrackMasking() && HasPbsSignalOnTrackdir(next.tile, next.td)) { + /* Possible safe tile. */ + end_segment_reason |= ESRB_SAFE_TILE; + } + + /* Check the next tile for the rail type. */ + if (next.rail_type != cur.rail_type) { + /* Segment must consist from the same rail_type tiles. */ + end_segment_reason |= ESRB_RAIL_TYPE; + break; + } + + /* Avoid infinite looping. */ + if (next.tile == n.m_key.m_tile && next.td == n.m_key.m_td) { + end_segment_reason |= ESRB_INFINITE_LOOP; + break; + } + + if (segment_cost > s_max_segment_cost) { + /* Potentially in the infinite loop (or only very long segment?). We should + * not force it to finish prematurely unless we are on a regular tile. */ + if (IsTileType(tf->m_new_tile, MP_RAILWAY)) { + end_segment_reason |= ESRB_SEGMENT_TOO_LONG; + break; + } + } + + /* Any other reason bit set? */ + if (end_segment_reason != ESRB_NONE) { + break; + } + + /* For the next loop set new prev and cur tile info. */ + prev = cur; + cur = next; + + } // for (;;) + + bool target_seen = false; + if ((end_segment_reason & ESRB_POSSIBLE_TARGET) != ESRB_NONE) { + /* Depot, station or waypoint. */ + if (Yapf().PfDetectDestination(cur.tile, cur.td)) { + /* Destination found. */ + target_seen = true; + } + } + + /* Update the segment if needed. */ + if (!is_cached_segment) { + /* Write back the segment information so it can be reused the next time. */ + segment.m_cost = segment_cost; + segment.m_end_segment_reason = end_segment_reason & ESRB_CACHED_MASK; + /* Save end of segment back to the node. */ + n.SetLastTileTrackdir(cur.tile, cur.td); + } + + /* Do we have an excuse why not to continue pathfinding in this direction? */ + if (!target_seen && (end_segment_reason & ESRB_ABORT_PF_MASK) != ESRB_NONE) { + /* Reason to not continue. Stop this PF branch. */ + return false; + } + + /* Special costs for the case we have reached our target. */ + if (target_seen) { + n.flags_u.flags_s.m_targed_seen = true; + /* Last-red and last-red-exit penalties. */ + if (n.flags_u.flags_s.m_last_signal_was_red) { + if (n.m_last_red_signal_type == SIGTYPE_EXIT) { + /* last signal was red pre-signal-exit */ + extra_cost += Yapf().PfGetSettings().rail_lastred_exit_penalty; + } else { + /* last signal was red, but not exit */ + extra_cost += Yapf().PfGetSettings().rail_lastred_penalty; + } + } + + /* Station platform-length penalty. */ + if ((end_segment_reason & ESRB_STATION) != ESRB_NONE) { + const BaseStation *st = BaseStation::GetByTile(n.GetLastTile()); + assert(st != NULL); + uint platform_length = st->GetPlatformLength(n.GetLastTile(), ReverseDiagDir(TrackdirToExitdir(n.GetLastTrackdir()))); + /* Reduce the extra cost caused by passing-station penalty (each station receives it in the segment cost). */ + extra_cost -= Yapf().PfGetSettings().rail_station_penalty * platform_length; + /* Add penalty for the inappropriate platform length. */ + extra_cost += PlatformLengthPenalty(platform_length); + } + } + + /* total node cost */ + n.m_cost = parent_cost + segment_entry_cost + segment_cost + extra_cost; + + return true; + } + + FORCEINLINE bool CanUseGlobalCache(Node& n) const + { + return !m_disable_cache + && (n.m_parent != NULL) + && (n.m_parent->m_num_signals_passed >= m_sig_look_ahead_costs.Size()); + } + + FORCEINLINE void ConnectNodeToCachedData(Node& n, CachedData& ci) + { + n.m_segment = &ci; + if (n.m_segment->m_cost < 0) { + n.m_segment->m_last_tile = n.m_key.m_tile; + n.m_segment->m_last_td = n.m_key.m_td; + } + } + + void DisableCache(bool disable) + { + m_disable_cache = disable; + } +}; + +#endif /* YAPF_COSTRAIL_HPP */ diff --git a/src/pathfinder/yapf/yapf_destrail.hpp b/src/pathfinder/yapf/yapf_destrail.hpp new file mode 100644 index 000000000..acdc68244 --- /dev/null +++ b/src/pathfinder/yapf/yapf_destrail.hpp @@ -0,0 +1,203 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_destrail.hpp Determining the destination for rail vehicles. */ + +#ifndef YAPF_DESTRAIL_HPP +#define YAPF_DESTRAIL_HPP + +class CYapfDestinationRailBase +{ +protected: + RailTypes m_compatible_railtypes; + +public: + void SetDestination(const Vehicle *v, bool override_rail_type = false) + { + m_compatible_railtypes = Train::From(v)->compatible_railtypes; + if (override_rail_type) m_compatible_railtypes |= GetRailTypeInfo(Train::From(v)->railtype)->compatible_railtypes; + } + + bool IsCompatibleRailType(RailType rt) + { + return HasBit(m_compatible_railtypes, rt); + } + + RailTypes GetCompatibleRailTypes() const + { + return m_compatible_railtypes; + } +}; + +template <class Types> +class CYapfDestinationAnyDepotRailT + : public CYapfDestinationRailBase +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + + /** to access inherited path finder */ + Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + + /** Called by YAPF to detect if node ends in the desired destination */ + FORCEINLINE bool PfDetectDestination(Node& n) + { + return PfDetectDestination(n.GetLastTile(), n.GetLastTrackdir()); + } + + /** Called by YAPF to detect if node ends in the desired destination */ + FORCEINLINE bool PfDetectDestination(TileIndex tile, Trackdir td) + { + bool bDest = IsRailDepotTile(tile); + return bDest; + } + + /** Called by YAPF to calculate cost estimate. Calculates distance to the destination + * adds it to the actual cost from origin and stores the sum to the Node::m_estimate */ + FORCEINLINE bool PfCalcEstimate(Node& n) + { + n.m_estimate = n.m_cost; + return true; + } +}; + +template <class Types> +class CYapfDestinationAnySafeTileRailT + : public CYapfDestinationRailBase +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + typedef typename Types::TrackFollower TrackFollower; ///< TrackFollower. Need to typedef for gcc 2.95 + + /** to access inherited path finder */ + Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + + /** Called by YAPF to detect if node ends in the desired destination */ + FORCEINLINE bool PfDetectDestination(Node& n) + { + return PfDetectDestination(n.GetLastTile(), n.GetLastTrackdir()); + } + + /** Called by YAPF to detect if node ends in the desired destination */ + FORCEINLINE bool PfDetectDestination(TileIndex tile, Trackdir td) + { + return + IsSafeWaitingPosition(Train::From(Yapf().GetVehicle()), tile, td, true, !TrackFollower::Allow90degTurns()) && + IsWaitingPositionFree(Train::From(Yapf().GetVehicle()), tile, td, !TrackFollower::Allow90degTurns()); + } + + /** Called by YAPF to calculate cost estimate. Calculates distance to the destination + * adds it to the actual cost from origin and stores the sum to the Node::m_estimate. */ + FORCEINLINE bool PfCalcEstimate(Node& n) + { + n.m_estimate = n.m_cost; + return true; + } +}; + +template <class Types> +class CYapfDestinationTileOrStationRailT + : public CYapfDestinationRailBase +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + TileIndex m_destTile; + TrackdirBits m_destTrackdirs; + StationID m_dest_station_id; + + /** to access inherited path finder */ + Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + void SetDestination(const Vehicle *v) + { + switch (v->current_order.GetType()) { + case OT_GOTO_STATION: + case OT_GOTO_WAYPOINT: + m_destTile = CalcClosestStationTile(v->current_order.GetDestination(), v->tile); + m_dest_station_id = v->current_order.GetDestination(); + m_destTrackdirs = INVALID_TRACKDIR_BIT; + break; + + default: + m_destTile = v->dest_tile; + m_dest_station_id = INVALID_STATION; + m_destTrackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(v->dest_tile, TRANSPORT_RAIL, 0)); + break; + } + CYapfDestinationRailBase::SetDestination(v); + } + + /** Called by YAPF to detect if node ends in the desired destination */ + FORCEINLINE bool PfDetectDestination(Node& n) + { + return PfDetectDestination(n.GetLastTile(), n.GetLastTrackdir()); + } + + /** Called by YAPF to detect if node ends in the desired destination */ + FORCEINLINE bool PfDetectDestination(TileIndex tile, Trackdir td) + { + bool bDest; + if (m_dest_station_id != INVALID_STATION) { + bDest = HasStationTileRail(tile) + && (GetStationIndex(tile) == m_dest_station_id) + && (GetRailStationTrack(tile) == TrackdirToTrack(td)); + } else { + bDest = (tile == m_destTile) + && ((m_destTrackdirs & TrackdirToTrackdirBits(td)) != TRACKDIR_BIT_NONE); + } + return bDest; + } + + /** Called by YAPF to calculate cost estimate. Calculates distance to the destination + * adds it to the actual cost from origin and stores the sum to the Node::m_estimate */ + FORCEINLINE bool PfCalcEstimate(Node& n) + { + static const int dg_dir_to_x_offs[] = {-1, 0, 1, 0}; + static const int dg_dir_to_y_offs[] = {0, 1, 0, -1}; + if (PfDetectDestination(n)) { + n.m_estimate = n.m_cost; + return true; + } + + TileIndex tile = n.GetLastTile(); + DiagDirection exitdir = TrackdirToExitdir(n.GetLastTrackdir()); + int x1 = 2 * TileX(tile) + dg_dir_to_x_offs[(int)exitdir]; + int y1 = 2 * TileY(tile) + dg_dir_to_y_offs[(int)exitdir]; + int x2 = 2 * TileX(m_destTile); + int y2 = 2 * TileY(m_destTile); + int dx = abs(x1 - x2); + int dy = abs(y1 - y2); + int dmin = min(dx, dy); + int dxy = abs(dx - dy); + int d = dmin * YAPF_TILE_CORNER_LENGTH + (dxy - 1) * (YAPF_TILE_LENGTH / 2); + n.m_estimate = n.m_cost + d; + assert(n.m_estimate >= n.m_parent->m_estimate); + return true; + } +}; + +#endif /* YAPF_DESTRAIL_HPP */ diff --git a/src/pathfinder/yapf/yapf_node.hpp b/src/pathfinder/yapf/yapf_node.hpp new file mode 100644 index 000000000..47312e27f --- /dev/null +++ b/src/pathfinder/yapf/yapf_node.hpp @@ -0,0 +1,101 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_node.hpp Node in the pathfinder's graph. */ + +#ifndef YAPF_NODE_HPP +#define YAPF_NODE_HPP + +/** Yapf Node Key that evaluates hash from (and compares) tile & exit dir. */ +struct CYapfNodeKeyExitDir { + TileIndex m_tile; + Trackdir m_td; + DiagDirection m_exitdir; + + FORCEINLINE void Set(TileIndex tile, Trackdir td) + { + m_tile = tile; + m_td = td; + m_exitdir = (m_td == INVALID_TRACKDIR) ? INVALID_DIAGDIR : TrackdirToExitdir(m_td); + } + + FORCEINLINE int CalcHash() const {return m_exitdir | (m_tile << 2);} + FORCEINLINE bool operator == (const CYapfNodeKeyExitDir& other) const {return (m_tile == other.m_tile) && (m_exitdir == other.m_exitdir);} + + void Dump(DumpTarget &dmp) const + { + dmp.WriteTile("m_tile", m_tile); + dmp.WriteEnumT("m_td", m_td); + dmp.WriteEnumT("m_exitdir", m_exitdir); + } +}; + +struct CYapfNodeKeyTrackDir : public CYapfNodeKeyExitDir +{ + FORCEINLINE int CalcHash() const {return m_td | (m_tile << 4);} + FORCEINLINE bool operator == (const CYapfNodeKeyTrackDir& other) const {return (m_tile == other.m_tile) && (m_td == other.m_td);} +}; + +/** Yapf Node base */ +template <class Tkey_, class Tnode> +struct CYapfNodeT { + typedef Tkey_ Key; + typedef Tnode Node; + + Tkey_ m_key; + Node *m_hash_next; + Node *m_parent; + int m_cost; + int m_estimate; + + FORCEINLINE void Set(Node *parent, TileIndex tile, Trackdir td, bool is_choice) + { + m_key.Set(tile, td); + m_hash_next = NULL; + m_parent = parent; + m_cost = 0; + m_estimate = 0; + } + + FORCEINLINE Node *GetHashNext() {return m_hash_next;} + FORCEINLINE void SetHashNext(Node *pNext) {m_hash_next = pNext;} + FORCEINLINE TileIndex GetTile() const {return m_key.m_tile;} + FORCEINLINE Trackdir GetTrackdir() const {return m_key.m_td;} + FORCEINLINE const Tkey_& GetKey() const {return m_key;} + FORCEINLINE int GetCost() const {return m_cost;} + FORCEINLINE int GetCostEstimate() const {return m_estimate;} + FORCEINLINE bool operator < (const Node& other) const {return m_estimate < other.m_estimate;} + + void Dump(DumpTarget &dmp) const + { + dmp.WriteStructT("m_key", &m_key); + dmp.WriteStructT("m_parent", m_parent); + dmp.WriteLine("m_cost = %d", m_cost); + dmp.WriteLine("m_estimate = %d", m_estimate); + } +}; + +/** Yapf Node for ships */ +template <class Tkey_> +struct CYapfShipNodeT + : CYapfNodeT<Tkey_, CYapfShipNodeT<Tkey_> > +{ + +}; + +/* now define two major node types (that differ by key type) */ +typedef CYapfShipNodeT<CYapfNodeKeyExitDir> CYapfShipNodeExitDir; +typedef CYapfShipNodeT<CYapfNodeKeyTrackDir> CYapfShipNodeTrackDir; + +/* Default NodeList types */ +typedef CNodeList_HashTableT<CYapfShipNodeExitDir , 14, 16> CShipNodeListExitDir; +typedef CNodeList_HashTableT<CYapfShipNodeTrackDir, 16, 20> CShipNodeListTrackDir; + + +#endif /* YAPF_NODE_HPP */ diff --git a/src/pathfinder/yapf/yapf_node_rail.hpp b/src/pathfinder/yapf/yapf_node_rail.hpp new file mode 100644 index 000000000..597424e0a --- /dev/null +++ b/src/pathfinder/yapf/yapf_node_rail.hpp @@ -0,0 +1,284 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_node_rail.hpp Node tailored for rail pathfinding. */ + +#ifndef YAPF_NODE_RAIL_HPP +#define YAPF_NODE_RAIL_HPP + +/** key for cached segment cost for rail YAPF */ +struct CYapfRailSegmentKey +{ + uint32 m_value; + + FORCEINLINE CYapfRailSegmentKey(const CYapfRailSegmentKey& src) : m_value(src.m_value) {} + + FORCEINLINE CYapfRailSegmentKey(const CYapfNodeKeyTrackDir& node_key) + { + Set(node_key); + } + + FORCEINLINE void Set(const CYapfRailSegmentKey& src) + { + m_value = src.m_value; + } + + FORCEINLINE void Set(const CYapfNodeKeyTrackDir& node_key) + { + m_value = (((int)node_key.m_tile) << 4) | node_key.m_td; + } + + FORCEINLINE int32 CalcHash() const + { + return m_value; + } + + FORCEINLINE TileIndex GetTile() const + { + return (TileIndex)(m_value >> 4); + } + + FORCEINLINE Trackdir GetTrackdir() const + { + return (Trackdir)(m_value & 0x0F); + } + + FORCEINLINE bool operator == (const CYapfRailSegmentKey& other) const + { + return m_value == other.m_value; + } + + void Dump(DumpTarget &dmp) const + { + dmp.WriteTile("tile", GetTile()); + dmp.WriteEnumT("td", GetTrackdir()); + } +}; + +/* Enum used in PfCalcCost() to see why was the segment closed. */ +enum EndSegmentReason { + /* The following reasons can be saved into cached segment */ + ESR_DEAD_END = 0, ///< track ends here + ESR_RAIL_TYPE, ///< the next tile has a different rail type than our tiles + ESR_INFINITE_LOOP, ///< infinite loop detected + ESR_SEGMENT_TOO_LONG, ///< the segment is too long (possible infinite loop) + ESR_CHOICE_FOLLOWS, ///< the next tile contains a choice (the track splits to more than one segments) + ESR_DEPOT, ///< stop in the depot (could be a target next time) + ESR_WAYPOINT, ///< waypoint encountered (could be a target next time) + ESR_STATION, ///< station encountered (could be a target next time) + ESR_SAFE_TILE, ///< safe waiting position found (could be a target) + + /* The following reasons are used only internally by PfCalcCost(). + * They should not be found in the cached segment. */ + ESR_PATH_TOO_LONG, ///< the path is too long (searching for the nearest depot in the given radius) + ESR_FIRST_TWO_WAY_RED, ///< first signal was 2-way and it was red + ESR_LOOK_AHEAD_END, ///< we have just passed the last look-ahead signal + ESR_TARGET_REACHED, ///< we have just reached the destination + + /* Special values */ + ESR_NONE = 0xFF, ///< no reason to end the segment here +}; + +enum EndSegmentReasonBits { + ESRB_NONE = 0, + + ESRB_DEAD_END = 1 << ESR_DEAD_END, + ESRB_RAIL_TYPE = 1 << ESR_RAIL_TYPE, + ESRB_INFINITE_LOOP = 1 << ESR_INFINITE_LOOP, + ESRB_SEGMENT_TOO_LONG = 1 << ESR_SEGMENT_TOO_LONG, + ESRB_CHOICE_FOLLOWS = 1 << ESR_CHOICE_FOLLOWS, + ESRB_DEPOT = 1 << ESR_DEPOT, + ESRB_WAYPOINT = 1 << ESR_WAYPOINT, + ESRB_STATION = 1 << ESR_STATION, + ESRB_SAFE_TILE = 1 << ESR_SAFE_TILE, + + ESRB_PATH_TOO_LONG = 1 << ESR_PATH_TOO_LONG, + ESRB_FIRST_TWO_WAY_RED = 1 << ESR_FIRST_TWO_WAY_RED, + ESRB_LOOK_AHEAD_END = 1 << ESR_LOOK_AHEAD_END, + ESRB_TARGET_REACHED = 1 << ESR_TARGET_REACHED, + + /* Additional (composite) values. */ + + /* What reasons mean that the target can be found and needs to be detected. */ + ESRB_POSSIBLE_TARGET = ESRB_DEPOT | ESRB_WAYPOINT | ESRB_STATION | ESRB_SAFE_TILE, + + /* What reasons can be stored back into cached segment. */ + ESRB_CACHED_MASK = ESRB_DEAD_END | ESRB_RAIL_TYPE | ESRB_INFINITE_LOOP | ESRB_SEGMENT_TOO_LONG | ESRB_CHOICE_FOLLOWS | ESRB_DEPOT | ESRB_WAYPOINT | ESRB_STATION | ESRB_SAFE_TILE, + + /* Reasons to abort pathfinding in this direction. */ + ESRB_ABORT_PF_MASK = ESRB_DEAD_END | ESRB_PATH_TOO_LONG | ESRB_INFINITE_LOOP | ESRB_FIRST_TWO_WAY_RED, +}; + +DECLARE_ENUM_AS_BIT_SET(EndSegmentReasonBits); + +inline CStrA ValueStr(EndSegmentReasonBits bits) +{ + static const char * const end_segment_reason_names[] = { + "DEAD_END", "RAIL_TYPE", "INFINITE_LOOP", "SEGMENT_TOO_LONG", "CHOICE_FOLLOWS", + "DEPOT", "WAYPOINT", "STATION", "SAFE_TILE", + "PATH_TOO_LONG", "FIRST_TWO_WAY_RED", "LOOK_AHEAD_END", "TARGET_REACHED" + }; + + CStrA out; + out.Format("0x%04X (%s)", bits, ComposeNameT(bits, end_segment_reason_names, "UNK", ESRB_NONE, "NONE").Data()); + return out.Transfer(); +} + +/** cached segment cost for rail YAPF */ +struct CYapfRailSegment +{ + typedef CYapfRailSegmentKey Key; + + CYapfRailSegmentKey m_key; + TileIndex m_last_tile; + Trackdir m_last_td; + int m_cost; + TileIndex m_last_signal_tile; + Trackdir m_last_signal_td; + EndSegmentReasonBits m_end_segment_reason; + CYapfRailSegment *m_hash_next; + + FORCEINLINE CYapfRailSegment(const CYapfRailSegmentKey& key) + : m_key(key) + , m_last_tile(INVALID_TILE) + , m_last_td(INVALID_TRACKDIR) + , m_cost(-1) + , m_last_signal_tile(INVALID_TILE) + , m_last_signal_td(INVALID_TRACKDIR) + , m_end_segment_reason(ESRB_NONE) + , m_hash_next(NULL) + {} + + FORCEINLINE const Key& GetKey() const + { + return m_key; + } + + FORCEINLINE TileIndex GetTile() const + { + return m_key.GetTile(); + } + + FORCEINLINE CYapfRailSegment *GetHashNext() + { + return m_hash_next; + } + + FORCEINLINE void SetHashNext(CYapfRailSegment *next) + { + m_hash_next = next; + } + + void Dump(DumpTarget &dmp) const + { + dmp.WriteStructT("m_key", &m_key); + dmp.WriteTile("m_last_tile", m_last_tile); + dmp.WriteEnumT("m_last_td", m_last_td); + dmp.WriteLine("m_cost = %d", m_cost); + dmp.WriteTile("m_last_signal_tile", m_last_signal_tile); + dmp.WriteEnumT("m_last_signal_td", m_last_signal_td); + dmp.WriteEnumT("m_end_segment_reason", m_end_segment_reason); + } +}; + +/** Yapf Node for rail YAPF */ +template <class Tkey_> +struct CYapfRailNodeT + : CYapfNodeT<Tkey_, CYapfRailNodeT<Tkey_> > +{ + typedef CYapfNodeT<Tkey_, CYapfRailNodeT<Tkey_> > base; + typedef CYapfRailSegment CachedData; + + CYapfRailSegment *m_segment; + uint16 m_num_signals_passed; + union { + uint32 m_inherited_flags; + struct { + bool m_targed_seen : 1; + bool m_choice_seen : 1; + bool m_last_signal_was_red : 1; + } flags_s; + } flags_u; + SignalType m_last_red_signal_type; + + FORCEINLINE void Set(CYapfRailNodeT *parent, TileIndex tile, Trackdir td, bool is_choice) + { + base::Set(parent, tile, td, is_choice); + m_segment = NULL; + if (parent == NULL) { + m_num_signals_passed = 0; + flags_u.m_inherited_flags = 0; + m_last_red_signal_type = SIGTYPE_NORMAL; + } else { + m_num_signals_passed = parent->m_num_signals_passed; + flags_u.m_inherited_flags = parent->flags_u.m_inherited_flags; + m_last_red_signal_type = parent->m_last_red_signal_type; + } + flags_u.flags_s.m_choice_seen |= is_choice; + } + + FORCEINLINE TileIndex GetLastTile() const + { + assert(m_segment != NULL); + return m_segment->m_last_tile; + } + + FORCEINLINE Trackdir GetLastTrackdir() const + { + assert(m_segment != NULL); + return m_segment->m_last_td; + } + + FORCEINLINE void SetLastTileTrackdir(TileIndex tile, Trackdir td) + { + assert(m_segment != NULL); + m_segment->m_last_tile = tile; + m_segment->m_last_td = td; + } + + template <class Tbase, class Tfunc, class Tpf> + bool IterateTiles(const Vehicle *v, Tpf &yapf, Tbase &obj, bool (Tfunc::*func)(TileIndex, Trackdir)) const + { + typename Tbase::TrackFollower ft(v, yapf.GetCompatibleRailTypes()); + TileIndex cur = base::GetTile(); + Trackdir cur_td = base::GetTrackdir(); + + while (cur != GetLastTile() || cur_td != GetLastTrackdir()) { + if (!((obj.*func)(cur, cur_td))) return false; + + ft.Follow(cur, cur_td); + cur = ft.m_new_tile; + assert(KillFirstBit(ft.m_new_td_bits) == TRACKDIR_BIT_NONE); + cur_td = FindFirstTrackdir(ft.m_new_td_bits); + } + + return (obj.*func)(cur, cur_td); + } + + void Dump(DumpTarget &dmp) const + { + base::Dump(dmp); + dmp.WriteStructT("m_segment", m_segment); + dmp.WriteLine("m_num_signals_passed = %d", m_num_signals_passed); + dmp.WriteLine("m_targed_seen = %s", flags_u.flags_s.m_targed_seen ? "Yes" : "No"); + dmp.WriteLine("m_choice_seen = %s", flags_u.flags_s.m_choice_seen ? "Yes" : "No"); + dmp.WriteLine("m_last_signal_was_red = %s", flags_u.flags_s.m_last_signal_was_red ? "Yes" : "No"); + dmp.WriteEnumT("m_last_red_signal_type", m_last_red_signal_type); + } +}; + +/* now define two major node types (that differ by key type) */ +typedef CYapfRailNodeT<CYapfNodeKeyExitDir> CYapfRailNodeExitDir; +typedef CYapfRailNodeT<CYapfNodeKeyTrackDir> CYapfRailNodeTrackDir; + +/* Default NodeList types */ +typedef CNodeList_HashTableT<CYapfRailNodeExitDir , 10, 12> CRailNodeListExitDir; +typedef CNodeList_HashTableT<CYapfRailNodeTrackDir, 12, 16> CRailNodeListTrackDir; + +#endif /* YAPF_NODE_RAIL_HPP */ diff --git a/src/pathfinder/yapf/yapf_node_road.hpp b/src/pathfinder/yapf/yapf_node_road.hpp new file mode 100644 index 000000000..c84755b5f --- /dev/null +++ b/src/pathfinder/yapf/yapf_node_road.hpp @@ -0,0 +1,41 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_node_road.hpp Node tailored for road pathfinding. */ + +#ifndef YAPF_NODE_ROAD_HPP +#define YAPF_NODE_ROAD_HPP + +/** Yapf Node for road YAPF */ +template <class Tkey_> +struct CYapfRoadNodeT + : CYapfNodeT<Tkey_, CYapfRoadNodeT<Tkey_> > +{ + typedef CYapfNodeT<Tkey_, CYapfRoadNodeT<Tkey_> > base; + + TileIndex m_segment_last_tile; + Trackdir m_segment_last_td; + + void Set(CYapfRoadNodeT *parent, TileIndex tile, Trackdir td, bool is_choice) + { + base::Set(parent, tile, td, is_choice); + m_segment_last_tile = tile; + m_segment_last_td = td; + } +}; + +/* now define two major node types (that differ by key type) */ +typedef CYapfRoadNodeT<CYapfNodeKeyExitDir> CYapfRoadNodeExitDir; +typedef CYapfRoadNodeT<CYapfNodeKeyTrackDir> CYapfRoadNodeTrackDir; + +/* Default NodeList types */ +typedef CNodeList_HashTableT<CYapfRoadNodeExitDir , 8, 12> CRoadNodeListExitDir; +typedef CNodeList_HashTableT<CYapfRoadNodeTrackDir, 10, 14> CRoadNodeListTrackDir; + +#endif /* YAPF_NODE_ROAD_HPP */ diff --git a/src/pathfinder/yapf/yapf_rail.cpp b/src/pathfinder/yapf/yapf_rail.cpp new file mode 100644 index 000000000..f17b90262 --- /dev/null +++ b/src/pathfinder/yapf/yapf_rail.cpp @@ -0,0 +1,645 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_rail.cpp The rail pathfinding. */ + +#include "../../stdafx.h" + +#include "yapf.hpp" +#include "yapf_node_rail.hpp" +#include "yapf_costrail.hpp" +#include "yapf_destrail.hpp" +#include "../../functions.h" + +#define DEBUG_YAPF_CACHE 0 + +#if DEBUG_YAPF_CACHE +template <typename Tpf> void DumpState(Tpf &pf1, Tpf &pf2) +{ + DumpTarget dmp1, dmp2; + pf1.DumpBase(dmp1); + pf2.DumpBase(dmp2); + FILE *f1 = fopen("yapf1.txt", "wt"); + FILE *f2 = fopen("yapf2.txt", "wt"); + fwrite(dmp1.m_out.Data(), 1, dmp1.m_out.Size(), f1); + fwrite(dmp2.m_out.Data(), 1, dmp2.m_out.Size(), f2); + fclose(f1); + fclose(f2); +} +#endif + +int _total_pf_time_us = 0; + +template <class Types> +class CYapfReserveTrack +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + +protected: + /** to access inherited pathfinder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +private: + TileIndex m_res_dest; ///< The reservation target tile + Trackdir m_res_dest_td; ///< The reservation target trackdir + Node *m_res_node; ///< The reservation target node + TileIndex m_res_fail_tile; ///< The tile where the reservation failed + Trackdir m_res_fail_td; ///< The trackdir where the reservation failed + + bool FindSafePositionProc(TileIndex tile, Trackdir td) + { + if (IsSafeWaitingPosition(Train::From(Yapf().GetVehicle()), tile, td, true, !TrackFollower::Allow90degTurns())) { + m_res_dest = tile; + m_res_dest_td = td; + return false; // Stop iterating segment + } + return true; + } + + /** Reserve a railway platform. Tile contains the failed tile on abort. */ + bool ReserveRailStationPlatform(TileIndex &tile, DiagDirection dir) + { + TileIndex start = tile; + TileIndexDiff diff = TileOffsByDiagDir(dir); + + do { + if (HasStationReservation(tile)) return false; + SetRailStationReservation(tile, true); + MarkTileDirtyByTile(tile); + tile = TILE_ADD(tile, diff); + } while (IsCompatibleTrainStationTile(tile, start)); + + return true; + } + + /** Try to reserve a single track/platform. */ + bool ReserveSingleTrack(TileIndex tile, Trackdir td) + { + if (IsRailStationTile(tile)) { + if (!ReserveRailStationPlatform(tile, TrackdirToExitdir(ReverseTrackdir(td)))) { + /* Platform could not be reserved, undo. */ + m_res_fail_tile = tile; + m_res_fail_td = td; + } + } else { + if (!TryReserveRailTrack(tile, TrackdirToTrack(td))) { + /* Tile couldn't be reserved, undo. */ + m_res_fail_tile = tile; + m_res_fail_td = td; + return false; + } + } + + return tile != m_res_dest || td != m_res_dest_td; + } + + /** Unreserve a single track/platform. Stops when the previous failer is reached. */ + bool UnreserveSingleTrack(TileIndex tile, Trackdir td) + { + if (IsRailStationTile(tile)) { + TileIndex start = tile; + TileIndexDiff diff = TileOffsByDiagDir(TrackdirToExitdir(ReverseTrackdir(td))); + while ((tile != m_res_fail_tile || td != m_res_fail_td) && IsCompatibleTrainStationTile(tile, start)) { + SetRailStationReservation(tile, false); + tile = TILE_ADD(tile, diff); + } + } else if (tile != m_res_fail_tile || td != m_res_fail_td) { + UnreserveRailTrack(tile, TrackdirToTrack(td)); + } + return (tile != m_res_dest || td != m_res_dest_td) && (tile != m_res_fail_tile || td != m_res_fail_td); + } + +public: + /** Set the target to where the reservation should be extended. */ + inline void SetReservationTarget(Node *node, TileIndex tile, Trackdir td) + { + m_res_node = node; + m_res_dest = tile; + m_res_dest_td = td; + } + + /** Check the node for a possible reservation target. */ + inline void FindSafePositionOnNode(Node *node) + { + assert(node->m_parent != NULL); + + /* We will never pass more than two signals, no need to check for a safe tile. */ + if (node->m_parent->m_num_signals_passed >= 2) return; + + if (!node->IterateTiles(Yapf().GetVehicle(), Yapf(), *this, &CYapfReserveTrack<Types>::FindSafePositionProc)) { + m_res_node = node; + } + } + + /** Try to reserve the path till the reservation target. */ + bool TryReservePath(PBSTileInfo *target) + { + m_res_fail_tile = INVALID_TILE; + + if (target != NULL) { + target->tile = m_res_dest; + target->trackdir = m_res_dest_td; + target->okay = false; + } + + /* Don't bother if the target is reserved. */ + if (!IsWaitingPositionFree(Train::From(Yapf().GetVehicle()), m_res_dest, m_res_dest_td)) return false; + + for (Node *node = m_res_node; node->m_parent != NULL; node = node->m_parent) { + node->IterateTiles(Yapf().GetVehicle(), Yapf(), *this, &CYapfReserveTrack<Types>::ReserveSingleTrack); + if (m_res_fail_tile != INVALID_TILE) { + /* Reservation failed, undo. */ + Node *fail_node = m_res_node; + TileIndex stop_tile = m_res_fail_tile; + do { + /* If this is the node that failed, stop at the failed tile. */ + m_res_fail_tile = fail_node == node ? stop_tile : INVALID_TILE; + fail_node->IterateTiles(Yapf().GetVehicle(), Yapf(), *this, &CYapfReserveTrack<Types>::UnreserveSingleTrack); + } while (fail_node != node && (fail_node = fail_node->m_parent) != NULL); + + return false; + } + } + + if (target != NULL) target->okay = true; + + if (Yapf().CanUseGlobalCache(*m_res_node)) + YapfNotifyTrackLayoutChange(INVALID_TILE, INVALID_TRACK); + + return true; + } +}; + +template <class Types> +class CYapfFollowAnyDepotRailT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + /** to access inherited path finder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** Called by YAPF to move from the given node to the next tile. For each + * reachable trackdir on the new tile creates new node, initializes it + * and adds it to the open list by calling Yapf().AddNewNode(n) */ + inline void PfFollowNode(Node& old_node) + { + TrackFollower F(Yapf().GetVehicle()); + if (F.Follow(old_node.GetLastTile(), old_node.GetLastTrackdir())) { + Yapf().AddMultipleNodes(&old_node, F); + } + } + + /** return debug report character to identify the transportation type */ + FORCEINLINE char TransportTypeChar() const + { + return 't'; + } + + static bool stFindNearestDepotTwoWay(const Vehicle *v, TileIndex t1, Trackdir td1, TileIndex t2, Trackdir td2, int max_distance, int reverse_penalty, TileIndex *depot_tile, bool *reversed) + { + Tpf pf1; + /* + * With caching enabled it simply cannot get a reliable result when you + * have limited the distance a train may travel. This means that the + * cached result does not match uncached result in all cases and that + * causes desyncs. So disable caching when finding for a depot that is + * nearby. This only happens with automatic servicing of vehicles, + * so it will only impact performance when you do not manually set + * depot orders and you do not disable automatic servicing. + */ + if (max_distance != 0) pf1.DisableCache(true); + bool result1 = pf1.FindNearestDepotTwoWay(v, t1, td1, t2, td2, max_distance, reverse_penalty, depot_tile, reversed); + +#if DEBUG_YAPF_CACHE + Tpf pf2; + TileIndex depot_tile2 = INVALID_TILE; + bool reversed2 = false; + pf2.DisableCache(true); + bool result2 = pf2.FindNearestDepotTwoWay(v, t1, td1, t2, td2, max_distance, reverse_penalty, &depot_tile2, &reversed2); + if (result1 != result2 || (result1 && (*depot_tile != depot_tile2 || *reversed != reversed2))) { + DEBUG(yapf, 0, "CACHE ERROR: FindNearestDepotTwoWay() = [%s, %s]", result1 ? "T" : "F", result2 ? "T" : "F"); + DumpState(pf1, pf2); + } +#endif + + return result1; + } + + FORCEINLINE bool FindNearestDepotTwoWay(const Vehicle *v, TileIndex t1, Trackdir td1, TileIndex t2, Trackdir td2, int max_distance, int reverse_penalty, TileIndex *depot_tile, bool *reversed) + { + /* set origin and destination nodes */ + Yapf().SetOrigin(t1, td1, t2, td2, reverse_penalty, true); + Yapf().SetDestination(v); + Yapf().SetMaxCost(YAPF_TILE_LENGTH * max_distance); + + /* find the best path */ + bool bFound = Yapf().FindPath(v); + if (!bFound) return false; + + /* some path found + * get found depot tile */ + Node *n = Yapf().GetBestNode(); + *depot_tile = n->GetLastTile(); + + /* walk through the path back to the origin */ + Node *pNode = n; + while (pNode->m_parent != NULL) { + pNode = pNode->m_parent; + } + + /* if the origin node is our front vehicle tile/Trackdir then we didn't reverse + * but we can also look at the cost (== 0 -> not reversed, == reverse_penalty -> reversed) */ + *reversed = (pNode->m_cost != 0); + + return true; + } +}; + +template <class Types> +class CYapfFollowAnySafeTileRailT : public CYapfReserveTrack<Types> +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + /** to access inherited path finder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** Called by YAPF to move from the given node to the next tile. For each + * reachable trackdir on the new tile creates new node, initializes it + * and adds it to the open list by calling Yapf().AddNewNode(n) */ + inline void PfFollowNode(Node& old_node) + { + TrackFollower F(Yapf().GetVehicle(), Yapf().GetCompatibleRailTypes()); + if (F.Follow(old_node.GetLastTile(), old_node.GetLastTrackdir()) && F.MaskReservedTracks()) { + Yapf().AddMultipleNodes(&old_node, F); + } + } + + /** Return debug report character to identify the transportation type */ + FORCEINLINE char TransportTypeChar() const + { + return 't'; + } + + static bool stFindNearestSafeTile(const Vehicle *v, TileIndex t1, Trackdir td, bool override_railtype) + { + /* Create pathfinder instance */ + Tpf pf1; +#if !DEBUG_YAPF_CACHE + bool result1 = pf1.FindNearestSafeTile(v, t1, td, override_railtype, false); + +#else + bool result2 = pf1.FindNearestSafeTile(v, t1, td, override_railtype, true); + Tpf pf2; + pf2.DisableCache(true); + bool result1 = pf2.FindNearestSafeTile(v, t1, td, override_railtype, false); + if (result1 != result2) { + DEBUG(yapf, 0, "CACHE ERROR: FindSafeTile() = [%s, %s]", result2 ? "T" : "F", result1 ? "T" : "F"); + DumpState(pf1, pf2); + } +#endif + + return result1; + } + + bool FindNearestSafeTile(const Vehicle *v, TileIndex t1, Trackdir td, bool override_railtype, bool dont_reserve) + { + /* Set origin and destination. */ + Yapf().SetOrigin(t1, td); + Yapf().SetDestination(v, override_railtype); + + bool bFound = Yapf().FindPath(v); + if (!bFound) return false; + + /* Found a destination, set as reservation target. */ + Node *pNode = Yapf().GetBestNode(); + this->SetReservationTarget(pNode, pNode->GetLastTile(), pNode->GetLastTrackdir()); + + /* Walk through the path back to the origin. */ + Node *pPrev = NULL; + while (pNode->m_parent != NULL) { + pPrev = pNode; + pNode = pNode->m_parent; + + this->FindSafePositionOnNode(pPrev); + } + + return dont_reserve || this->TryReservePath(NULL); + } +}; + +template <class Types> +class CYapfFollowRailT : public CYapfReserveTrack<Types> +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + /** to access inherited path finder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** Called by YAPF to move from the given node to the next tile. For each + * reachable trackdir on the new tile creates new node, initializes it + * and adds it to the open list by calling Yapf().AddNewNode(n) */ + inline void PfFollowNode(Node& old_node) + { + TrackFollower F(Yapf().GetVehicle()); + if (F.Follow(old_node.GetLastTile(), old_node.GetLastTrackdir())) { + Yapf().AddMultipleNodes(&old_node, F); + } + } + + /** return debug report character to identify the transportation type */ + FORCEINLINE char TransportTypeChar() const + { + return 't'; + } + + static Trackdir stChooseRailTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir, TrackBits tracks, bool *path_not_found, bool reserve_track, PBSTileInfo *target) + { + /* create pathfinder instance */ + Tpf pf1; +#if !DEBUG_YAPF_CACHE + Trackdir result1 = pf1.ChooseRailTrack(v, tile, enterdir, tracks, path_not_found, reserve_track, target); + +#else + Trackdir result1 = pf1.ChooseRailTrack(v, tile, enterdir, tracks, path_not_found, false, NULL); + Tpf pf2; + pf2.DisableCache(true); + Trackdir result2 = pf2.ChooseRailTrack(v, tile, enterdir, tracks, path_not_found, reserve_track, target); + if (result1 != result2) { + DEBUG(yapf, 0, "CACHE ERROR: ChooseRailTrack() = [%d, %d]", result1, result2); + DumpState(pf1, pf2); + } +#endif + + return result1; + } + + FORCEINLINE Trackdir ChooseRailTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir, TrackBits tracks, bool *path_not_found, bool reserve_track, PBSTileInfo *target) + { + if (target != NULL) target->tile = INVALID_TILE; + + /* set origin and destination nodes */ + PBSTileInfo origin = FollowTrainReservation(Train::From(v)); + Yapf().SetOrigin(origin.tile, origin.trackdir, INVALID_TILE, INVALID_TRACKDIR, 1, true); + Yapf().SetDestination(v); + + /* find the best path */ + bool path_found = Yapf().FindPath(v); + if (path_not_found != NULL) { + /* tell controller that the path was only 'guessed' + * treat the path as found if stopped on the first two way signal(s) */ + *path_not_found = !(path_found || Yapf().m_stopped_on_first_two_way_signal); + } + + /* if path not found - return INVALID_TRACKDIR */ + Trackdir next_trackdir = INVALID_TRACKDIR; + Node *pNode = Yapf().GetBestNode(); + if (pNode != NULL) { + /* reserve till end of path */ + this->SetReservationTarget(pNode, pNode->GetLastTile(), pNode->GetLastTrackdir()); + + /* path was found or at least suggested + * walk through the path back to the origin */ + Node *pPrev = NULL; + while (pNode->m_parent != NULL) { + pPrev = pNode; + pNode = pNode->m_parent; + + this->FindSafePositionOnNode(pPrev); + } + /* return trackdir from the best origin node (one of start nodes) */ + Node& best_next_node = *pPrev; + next_trackdir = best_next_node.GetTrackdir(); + + if (reserve_track && path_found) this->TryReservePath(target); + } + return next_trackdir; + } + + static bool stCheckReverseTrain(const Vehicle *v, TileIndex t1, Trackdir td1, TileIndex t2, Trackdir td2, int reverse_penalty) + { + Tpf pf1; + bool result1 = pf1.CheckReverseTrain(v, t1, td1, t2, td2, reverse_penalty); + +#if DEBUG_YAPF_CACHE + Tpf pf2; + pf2.DisableCache(true); + bool result2 = pf2.CheckReverseTrain(v, t1, td1, t2, td2, reverse_penalty); + if (result1 != result2) { + DEBUG(yapf, 0, "CACHE ERROR: CheckReverseTrain() = [%s, %s]", result1 ? "T" : "F", result2 ? "T" : "F"); + DumpState(pf1, pf2); + } +#endif + + return result1; + } + + FORCEINLINE bool CheckReverseTrain(const Vehicle *v, TileIndex t1, Trackdir td1, TileIndex t2, Trackdir td2, int reverse_penalty) + { + /* create pathfinder instance + * set origin and destination nodes */ + Yapf().SetOrigin(t1, td1, t2, td2, reverse_penalty, false); + Yapf().SetDestination(v); + + /* find the best path */ + bool bFound = Yapf().FindPath(v); + + if (!bFound) return false; + + /* path was found + * walk through the path back to the origin */ + Node *pNode = Yapf().GetBestNode(); + while (pNode->m_parent != NULL) { + pNode = pNode->m_parent; + } + + /* check if it was reversed origin */ + Node& best_org_node = *pNode; + bool reversed = (best_org_node.m_cost != 0); + return reversed; + } +}; + +template <class Tpf_, class Ttrack_follower, class Tnode_list, template <class Types> class TdestinationT, template <class Types> class TfollowT> +struct CYapfRail_TypesT +{ + typedef CYapfRail_TypesT<Tpf_, Ttrack_follower, Tnode_list, TdestinationT, TfollowT> Types; + + typedef Tpf_ Tpf; + typedef Ttrack_follower TrackFollower; + typedef Tnode_list NodeList; + typedef CYapfBaseT<Types> PfBase; + typedef TfollowT<Types> PfFollow; + typedef CYapfOriginTileTwoWayT<Types> PfOrigin; + typedef TdestinationT<Types> PfDestination; + typedef CYapfSegmentCostCacheGlobalT<Types> PfCache; + typedef CYapfCostRailT<Types> PfCost; +}; + +struct CYapfRail1 : CYapfT<CYapfRail_TypesT<CYapfRail1 , CFollowTrackRail , CRailNodeListTrackDir, CYapfDestinationTileOrStationRailT, CYapfFollowRailT> > {}; +struct CYapfRail2 : CYapfT<CYapfRail_TypesT<CYapfRail2 , CFollowTrackRailNo90, CRailNodeListTrackDir, CYapfDestinationTileOrStationRailT, CYapfFollowRailT> > {}; + +struct CYapfAnyDepotRail1 : CYapfT<CYapfRail_TypesT<CYapfAnyDepotRail1, CFollowTrackRail , CRailNodeListTrackDir, CYapfDestinationAnyDepotRailT , CYapfFollowAnyDepotRailT> > {}; +struct CYapfAnyDepotRail2 : CYapfT<CYapfRail_TypesT<CYapfAnyDepotRail2, CFollowTrackRailNo90, CRailNodeListTrackDir, CYapfDestinationAnyDepotRailT , CYapfFollowAnyDepotRailT> > {}; + +struct CYapfAnySafeTileRail1 : CYapfT<CYapfRail_TypesT<CYapfAnySafeTileRail1, CFollowTrackFreeRail , CRailNodeListTrackDir, CYapfDestinationAnySafeTileRailT , CYapfFollowAnySafeTileRailT> > {}; +struct CYapfAnySafeTileRail2 : CYapfT<CYapfRail_TypesT<CYapfAnySafeTileRail2, CFollowTrackFreeRailNo90, CRailNodeListTrackDir, CYapfDestinationAnySafeTileRailT , CYapfFollowAnySafeTileRailT> > {}; + + +Trackdir YapfChooseRailTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir, TrackBits tracks, bool *path_not_found, bool reserve_track, PBSTileInfo *target) +{ + /* default is YAPF type 2 */ + typedef Trackdir (*PfnChooseRailTrack)(const Vehicle*, TileIndex, DiagDirection, TrackBits, bool*, bool, PBSTileInfo*); + PfnChooseRailTrack pfnChooseRailTrack = &CYapfRail1::stChooseRailTrack; + + /* check if non-default YAPF type needed */ + if (_settings_game.pf.forbid_90_deg) { + pfnChooseRailTrack = &CYapfRail2::stChooseRailTrack; // Trackdir, forbid 90-deg + } + + Trackdir td_ret = pfnChooseRailTrack(v, tile, enterdir, tracks, path_not_found, reserve_track, target); + + return td_ret; +} + +bool YapfCheckReverseTrain(const Vehicle *vt) +{ + const Train *v = Train::From(vt); + const Train *last_veh = v->Last(); + + /* get trackdirs of both ends */ + Trackdir td = v->GetVehicleTrackdir(); + Trackdir td_rev = ReverseTrackdir(last_veh->GetVehicleTrackdir()); + + /* tiles where front and back are */ + TileIndex tile = v->tile; + TileIndex tile_rev = last_veh->tile; + + int reverse_penalty = 0; + + if (v->track == TRACK_BIT_WORMHOLE) { + /* front in tunnel / on bridge */ + DiagDirection dir_into_wormhole = GetTunnelBridgeDirection(tile); + + if (TrackdirToExitdir(td) == dir_into_wormhole) tile = GetOtherTunnelBridgeEnd(tile); + /* Now 'tile' is the tunnel entry/bridge ramp the train will reach when driving forward */ + + /* Current position of the train in the wormhole */ + TileIndex cur_tile = TileVirtXY(v->x_pos, v->y_pos); + + /* Add distance to drive in the wormhole as penalty for the forward path, i.e. bonus for the reverse path + * Note: Negative penalties are ok for the start tile. */ + reverse_penalty -= DistanceManhattan(cur_tile, tile) * YAPF_TILE_LENGTH; + } + + if (last_veh->track == TRACK_BIT_WORMHOLE) { + /* back in tunnel / on bridge */ + DiagDirection dir_into_wormhole = GetTunnelBridgeDirection(tile_rev); + + if (TrackdirToExitdir(td_rev) == dir_into_wormhole) tile_rev = GetOtherTunnelBridgeEnd(tile_rev); + /* Now 'tile_rev' is the tunnel entry/bridge ramp the train will reach when reversing */ + + /* Current position of the last wagon in the wormhole */ + TileIndex cur_tile = TileVirtXY(last_veh->x_pos, last_veh->y_pos); + + /* Add distance to drive in the wormhole as penalty for the revere path. */ + reverse_penalty += DistanceManhattan(cur_tile, tile_rev) * YAPF_TILE_LENGTH; + } + + typedef bool (*PfnCheckReverseTrain)(const Vehicle*, TileIndex, Trackdir, TileIndex, Trackdir, int); + PfnCheckReverseTrain pfnCheckReverseTrain = CYapfRail1::stCheckReverseTrain; + + /* check if non-default YAPF type needed */ + if (_settings_game.pf.forbid_90_deg) { + pfnCheckReverseTrain = &CYapfRail2::stCheckReverseTrain; // Trackdir, forbid 90-deg + } + + /* slightly hackish: If the pathfinders finds a path, the cost of the first node is tested to distinguish between forward- and reverse-path. */ + if (reverse_penalty == 0) reverse_penalty = 1; + + bool reverse = pfnCheckReverseTrain(v, tile, td, tile_rev, td_rev, reverse_penalty); + + return reverse; +} + +bool YapfFindNearestRailDepotTwoWay(const Vehicle *v, int max_distance, int reverse_penalty, TileIndex *depot_tile, bool *reversed) +{ + *depot_tile = INVALID_TILE; + *reversed = false; + + const Vehicle *last_veh = v->Last(); + + PBSTileInfo origin = FollowTrainReservation(Train::From(v)); + TileIndex last_tile = last_veh->tile; + Trackdir td_rev = ReverseTrackdir(last_veh->GetVehicleTrackdir()); + + typedef bool (*PfnFindNearestDepotTwoWay)(const Vehicle*, TileIndex, Trackdir, TileIndex, Trackdir, int, int, TileIndex*, bool*); + PfnFindNearestDepotTwoWay pfnFindNearestDepotTwoWay = &CYapfAnyDepotRail1::stFindNearestDepotTwoWay; + + /* check if non-default YAPF type needed */ + if (_settings_game.pf.forbid_90_deg) { + pfnFindNearestDepotTwoWay = &CYapfAnyDepotRail2::stFindNearestDepotTwoWay; // Trackdir, forbid 90-deg + } + + bool ret = pfnFindNearestDepotTwoWay(v, origin.tile, origin.trackdir, last_tile, td_rev, max_distance, reverse_penalty, depot_tile, reversed); + return ret; +} + +bool YapfRailFindNearestSafeTile(const Vehicle *v, TileIndex tile, Trackdir td, bool override_railtype) +{ + typedef bool (*PfnFindNearestSafeTile)(const Vehicle*, TileIndex, Trackdir, bool); + PfnFindNearestSafeTile pfnFindNearestSafeTile = CYapfAnySafeTileRail1::stFindNearestSafeTile; + + /* check if non-default YAPF type needed */ + if (_settings_game.pf.forbid_90_deg) { + pfnFindNearestSafeTile = &CYapfAnySafeTileRail2::stFindNearestSafeTile; + } + + return pfnFindNearestSafeTile(v, tile, td, override_railtype); +} + +/** if any track changes, this counter is incremented - that will invalidate segment cost cache */ +int CSegmentCostCacheBase::s_rail_change_counter = 0; + +void YapfNotifyTrackLayoutChange(TileIndex tile, Track track) +{ + CSegmentCostCacheBase::NotifyTrackLayoutChange(tile, track); +} diff --git a/src/pathfinder/yapf/yapf_road.cpp b/src/pathfinder/yapf/yapf_road.cpp new file mode 100644 index 000000000..3a5f80717 --- /dev/null +++ b/src/pathfinder/yapf/yapf_road.cpp @@ -0,0 +1,631 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_road.cpp The road pathfinding. */ + +#include "../../stdafx.h" +#include "../../roadstop_base.h" +#include "../../cargotype.h" + +#include "yapf.hpp" +#include "yapf_node_road.hpp" + + +template <class Types> +class CYapfCostRoadT +{ +public: + typedef typename Types::Tpf Tpf; ///< pathfinder (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; ///< track follower helper + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + /** to access inherited path finder */ + Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + + int SlopeCost(TileIndex tile, TileIndex next_tile, Trackdir trackdir) + { + /* height of the center of the current tile */ + int x1 = TileX(tile) * TILE_SIZE; + int y1 = TileY(tile) * TILE_SIZE; + int z1 = GetSlopeZ(x1 + TILE_SIZE / 2, y1 + TILE_SIZE / 2); + + /* height of the center of the next tile */ + int x2 = TileX(next_tile) * TILE_SIZE; + int y2 = TileY(next_tile) * TILE_SIZE; + int z2 = GetSlopeZ(x2 + TILE_SIZE / 2, y2 + TILE_SIZE / 2); + + if (z2 - z1 > 1) { + /* Slope up */ + return Yapf().PfGetSettings().road_slope_penalty; + } + return 0; + } + + /** return one tile cost */ + FORCEINLINE int OneTileCost(TileIndex tile, Trackdir trackdir) + { + int cost = 0; + /* set base cost */ + if (IsDiagonalTrackdir(trackdir)) { + cost += YAPF_TILE_LENGTH; + switch (GetTileType(tile)) { + case MP_ROAD: + /* Increase the cost for level crossings */ + if (IsLevelCrossing(tile)) { + cost += Yapf().PfGetSettings().road_crossing_penalty; + } + break; + + case MP_STATION: + if (IsDriveThroughStopTile(tile)) { + cost += Yapf().PfGetSettings().road_stop_penalty; + } + break; + + default: + break; + } + } else { + /* non-diagonal trackdir */ + cost = YAPF_TILE_CORNER_LENGTH + Yapf().PfGetSettings().road_curve_penalty; + } + return cost; + } + +public: + /** Called by YAPF to calculate the cost from the origin to the given node. + * Calculates only the cost of given node, adds it to the parent node cost + * and stores the result into Node::m_cost member */ + FORCEINLINE bool PfCalcCost(Node& n, const TrackFollower *tf) + { + int segment_cost = 0; + /* start at n.m_key.m_tile / n.m_key.m_td and walk to the end of segment */ + TileIndex tile = n.m_key.m_tile; + Trackdir trackdir = n.m_key.m_td; + while (true) { + /* base tile cost depending on distance between edges */ + segment_cost += Yapf().OneTileCost(tile, trackdir); + + const Vehicle *v = Yapf().GetVehicle(); + /* we have reached the vehicle's destination - segment should end here to avoid target skipping */ + if (Yapf().PfDetectDestinationTile(tile, trackdir)) break; + + /* stop if we have just entered the depot */ + if (IsRoadDepotTile(tile) && trackdir == DiagDirToDiagTrackdir(ReverseDiagDir(GetRoadDepotDirection(tile)))) { + /* next time we will reverse and leave the depot */ + break; + } + + /* if there are no reachable trackdirs on new tile, we have end of road */ + TrackFollower F(Yapf().GetVehicle()); + if (!F.Follow(tile, trackdir)) break; + + /* if there are more trackdirs available & reachable, we are at the end of segment */ + if (KillFirstBit(F.m_new_td_bits) != TRACKDIR_BIT_NONE) break; + + Trackdir new_td = (Trackdir)FindFirstBit2x64(F.m_new_td_bits); + + /* stop if RV is on simple loop with no junctions */ + if (F.m_new_tile == n.m_key.m_tile && new_td == n.m_key.m_td) return false; + + /* if we skipped some tunnel tiles, add their cost */ + segment_cost += F.m_tiles_skipped * YAPF_TILE_LENGTH; + + /* add hilly terrain penalty */ + segment_cost += Yapf().SlopeCost(tile, F.m_new_tile, trackdir); + + /* add min/max speed penalties */ + int min_speed = 0; + int max_speed = F.GetSpeedLimit(&min_speed); + if (max_speed < v->max_speed) segment_cost += 1 * (v->max_speed - max_speed); + if (min_speed > v->max_speed) segment_cost += 10 * (min_speed - v->max_speed); + + /* move to the next tile */ + tile = F.m_new_tile; + trackdir = new_td; + }; + + /* save end of segment back to the node */ + n.m_segment_last_tile = tile; + n.m_segment_last_td = trackdir; + + /* save also tile cost */ + int parent_cost = (n.m_parent != NULL) ? n.m_parent->m_cost : 0; + n.m_cost = parent_cost + segment_cost; + return true; + } +}; + + +template <class Types> +class CYapfDestinationAnyDepotRoadT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + + /** to access inherited path finder */ + Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + + /** Called by YAPF to detect if node ends in the desired destination */ + FORCEINLINE bool PfDetectDestination(Node& n) + { + bool bDest = IsRoadDepotTile(n.m_segment_last_tile); + return bDest; + } + + FORCEINLINE bool PfDetectDestinationTile(TileIndex tile, Trackdir trackdir) + { + return IsRoadDepotTile(tile); + } + + /** Called by YAPF to calculate cost estimate. Calculates distance to the destination + * adds it to the actual cost from origin and stores the sum to the Node::m_estimate */ + FORCEINLINE bool PfCalcEstimate(Node& n) + { + n.m_estimate = n.m_cost; + return true; + } +}; + + +template <class Types> +class CYapfDestinationAnyRoadVehicleCompatibleStopOfGivenStationT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + + TileIndex m_destTile; + StationID m_dest_station; + bool m_bus; + bool m_non_artic; + + /** to access inherited path finder */ + Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + + void SetDestination(const RoadVehicle *v, StationID sid, TileIndex destTile) + { + m_dest_station = sid; + m_destTile = destTile; + m_bus = IsCargoInClass(v->cargo_type, CC_PASSENGERS); + m_non_artic = !v->HasArticulatedPart(); + } + + /** Called by YAPF to detect if node ends in the desired destination */ + FORCEINLINE bool PfDetectDestination(Node& n) + { + return PfDetectDestinationTile(n.m_segment_last_tile, INVALID_TRACKDIR); + } + + FORCEINLINE bool PfDetectDestinationTile(TileIndex tile, Trackdir trackdir) + { + return + IsTileType(tile, MP_STATION) && + GetStationIndex(tile) == m_dest_station && + (m_bus ? IsBusStop(tile) : IsTruckStop(tile)) && + (m_non_artic || IsDriveThroughStopTile(tile)); + } + + /** Called by YAPF to calculate cost estimate. Calculates distance to the destination + * adds it to the actual cost from origin and stores the sum to the Node::m_estimate */ + FORCEINLINE bool PfCalcEstimate(Node& n) + { + static const int dg_dir_to_x_offs[] = {-1, 0, 1, 0}; + static const int dg_dir_to_y_offs[] = {0, 1, 0, -1}; + if (PfDetectDestination(n)) { + n.m_estimate = n.m_cost; + return true; + } + + TileIndex tile = n.m_segment_last_tile; + DiagDirection exitdir = TrackdirToExitdir(n.m_segment_last_td); + int x1 = 2 * TileX(tile) + dg_dir_to_x_offs[(int)exitdir]; + int y1 = 2 * TileY(tile) + dg_dir_to_y_offs[(int)exitdir]; + int x2 = 2 * TileX(m_destTile); + int y2 = 2 * TileY(m_destTile); + int dx = abs(x1 - x2); + int dy = abs(y1 - y2); + int dmin = min(dx, dy); + int dxy = abs(dx - dy); + int d = dmin * YAPF_TILE_CORNER_LENGTH + (dxy - 1) * (YAPF_TILE_LENGTH / 2); + n.m_estimate = n.m_cost + d; + assert(n.m_estimate >= n.m_parent->m_estimate); + return true; + } +}; + + +template <class Types> +class CYapfDestinationTileRoadT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + TileIndex m_destTile; + TrackdirBits m_destTrackdirs; + +public: + void SetDestination(TileIndex tile, TrackdirBits trackdirs) + { + m_destTile = tile; + m_destTrackdirs = trackdirs; + } + +protected: + /** to access inherited path finder */ + Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** Called by YAPF to detect if node ends in the desired destination */ + FORCEINLINE bool PfDetectDestination(Node& n) + { + bool bDest = (n.m_segment_last_tile == m_destTile) && ((m_destTrackdirs & TrackdirToTrackdirBits(n.m_segment_last_td)) != TRACKDIR_BIT_NONE); + return bDest; + } + + FORCEINLINE bool PfDetectDestinationTile(TileIndex tile, Trackdir trackdir) + { + return tile == m_destTile && ((m_destTrackdirs & TrackdirToTrackdirBits(trackdir)) != TRACKDIR_BIT_NONE); + } + + /** Called by YAPF to calculate cost estimate. Calculates distance to the destination + * adds it to the actual cost from origin and stores the sum to the Node::m_estimate */ + inline bool PfCalcEstimate(Node& n) + { + static const int dg_dir_to_x_offs[] = {-1, 0, 1, 0}; + static const int dg_dir_to_y_offs[] = {0, 1, 0, -1}; + if (PfDetectDestination(n)) { + n.m_estimate = n.m_cost; + return true; + } + + TileIndex tile = n.m_segment_last_tile; + DiagDirection exitdir = TrackdirToExitdir(n.m_segment_last_td); + int x1 = 2 * TileX(tile) + dg_dir_to_x_offs[(int)exitdir]; + int y1 = 2 * TileY(tile) + dg_dir_to_y_offs[(int)exitdir]; + int x2 = 2 * TileX(m_destTile); + int y2 = 2 * TileY(m_destTile); + int dx = abs(x1 - x2); + int dy = abs(y1 - y2); + int dmin = min(dx, dy); + int dxy = abs(dx - dy); + int d = dmin * YAPF_TILE_CORNER_LENGTH + (dxy - 1) * (YAPF_TILE_LENGTH / 2); + n.m_estimate = n.m_cost + d; + assert(n.m_estimate >= n.m_parent->m_estimate); + return true; + } +}; + + + +template <class Types> +class CYapfFollowRoadT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + /** to access inherited path finder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + + /** Called by YAPF to move from the given node to the next tile. For each + * reachable trackdir on the new tile creates new node, initializes it + * and adds it to the open list by calling Yapf().AddNewNode(n) */ + inline void PfFollowNode(Node& old_node) + { + TrackFollower F(Yapf().GetVehicle()); + if (F.Follow(old_node.m_segment_last_tile, old_node.m_segment_last_td)) { + Yapf().AddMultipleNodes(&old_node, F); + } + } + + /** return debug report character to identify the transportation type */ + FORCEINLINE char TransportTypeChar() const + { + return 'r'; + } + + static Trackdir stChooseRoadTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir) + { + Tpf pf; + return pf.ChooseRoadTrack(v, tile, enterdir); + } + + FORCEINLINE Trackdir ChooseRoadTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir) + { + /* handle special case - when next tile is destination tile */ + if (tile == v->dest_tile) { + /* choose diagonal trackdir reachable from enterdir */ + return DiagDirToDiagTrackdir(enterdir); + } + /* our source tile will be the next vehicle tile (should be the given one) */ + TileIndex src_tile = tile; + /* get available trackdirs on the start tile */ + TrackdirBits src_trackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, RoadVehicle::From(v)->compatible_roadtypes)); + /* select reachable trackdirs only */ + src_trackdirs &= DiagdirReachesTrackdirs(enterdir); + + /* get available trackdirs on the destination tile */ + TileIndex dest_tile = v->dest_tile; + TrackdirBits dest_trackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(dest_tile, TRANSPORT_ROAD, RoadVehicle::From(v)->compatible_roadtypes)); + + /* set origin and destination nodes */ + Yapf().SetOrigin(src_tile, src_trackdirs); + Yapf().SetDestination(dest_tile, dest_trackdirs); + + /* find the best path */ + Yapf().FindPath(v); + + /* if path not found - return INVALID_TRACKDIR */ + Trackdir next_trackdir = INVALID_TRACKDIR; + Node *pNode = Yapf().GetBestNode(); + if (pNode != NULL) { + /* path was found or at least suggested + * walk through the path back to its origin */ + while (pNode->m_parent != NULL) { + pNode = pNode->m_parent; + } + /* return trackdir from the best origin node (one of start nodes) */ + Node& best_next_node = *pNode; + assert(best_next_node.GetTile() == tile); + next_trackdir = best_next_node.GetTrackdir(); + } + return next_trackdir; + } + + static uint stDistanceToTile(const Vehicle *v, TileIndex tile) + { + Tpf pf; + return pf.DistanceToTile(v, tile); + } + + FORCEINLINE uint DistanceToTile(const Vehicle *v, TileIndex dst_tile) + { + /* handle special case - when current tile is the destination tile */ + if (dst_tile == v->tile) { + /* distance is zero in this case */ + return 0; + } + + if (!SetOriginFromVehiclePos(v)) return UINT_MAX; + + /* set destination tile, trackdir + * get available trackdirs on the destination tile */ + TrackdirBits dst_td_bits = TrackStatusToTrackdirBits(GetTileTrackStatus(dst_tile, TRANSPORT_ROAD, RoadVehicle::From(v)->compatible_roadtypes)); + Yapf().SetDestination(dst_tile, dst_td_bits); + + /* if path not found - return distance = UINT_MAX */ + uint dist = UINT_MAX; + + /* find the best path */ + if (!Yapf().FindPath(v)) return dist; + + Node *pNode = Yapf().GetBestNode(); + if (pNode != NULL) { + /* path was found + * get the path cost estimate */ + dist = pNode->GetCostEstimate(); + } + + return dist; + } + + /** Return true if the valid origin (tile/trackdir) was set from the current vehicle position. */ + FORCEINLINE bool SetOriginFromVehiclePos(const Vehicle *v) + { + /* set origin (tile, trackdir) */ + TileIndex src_tile = v->tile; + Trackdir src_td = v->GetVehicleTrackdir(); + if ((TrackStatusToTrackdirBits(GetTileTrackStatus(src_tile, TRANSPORT_ROAD, RoadVehicle::From(v)->compatible_roadtypes)) & TrackdirToTrackdirBits(src_td)) == 0) { + /* sometimes the roadveh is not on the road (it resides on non-existing track) + * how should we handle that situation? */ + return false; + } + Yapf().SetOrigin(src_tile, TrackdirToTrackdirBits(src_td)); + return true; + } + + static bool stFindNearestDepot(const Vehicle *v, TileIndex tile, Trackdir td, int max_distance, TileIndex *depot_tile) + { + Tpf pf; + return pf.FindNearestDepot(v, tile, td, max_distance, depot_tile); + } + + FORCEINLINE bool FindNearestDepot(const Vehicle *v, TileIndex tile, Trackdir td, int max_distance, TileIndex *depot_tile) + { + /* set origin and destination nodes */ + Yapf().SetOrigin(tile, TrackdirToTrackdirBits(td)); + + /* find the best path */ + bool bFound = Yapf().FindPath(v); + if (!bFound) return false; + + /* some path found + * get found depot tile */ + Node *n = Yapf().GetBestNode(); + + if (max_distance > 0 && n->m_cost > max_distance * YAPF_TILE_LENGTH) return false; + + *depot_tile = n->m_segment_last_tile; + return true; + } + + static bool stFindNearestRoadVehicleCompatibleStop(const RoadVehicle *v, TileIndex tile, TileIndex destTile, Trackdir td, StationID sid, TileIndex *stop_tile) + { + Tpf pf; + return pf.FindNearestRoadVehicleCompatibleStop(v, tile, destTile, td, sid, stop_tile); + } + + FORCEINLINE bool FindNearestRoadVehicleCompatibleStop(const RoadVehicle *v, TileIndex tile, TileIndex destTile, Trackdir td, StationID sid, TileIndex *stop_tile) + { + /* set origin and destination nodes */ + Yapf().SetOrigin(tile, TrackdirToTrackdirBits(td)); + Yapf().SetDestination(v, sid, destTile); + + /* find the best path */ + bool bFound = Yapf().FindPath(v); + if (!bFound) return false; + + /* some path found + * get found depot tile */ + const Node *n = Yapf().GetBestNode(); + + *stop_tile = n->m_segment_last_tile; + return true; + } +}; + +template <class Tpf_, class Tnode_list, template <class Types> class Tdestination> +struct CYapfRoad_TypesT +{ + typedef CYapfRoad_TypesT<Tpf_, Tnode_list, Tdestination> Types; + + typedef Tpf_ Tpf; + typedef CFollowTrackRoad TrackFollower; + typedef Tnode_list NodeList; + typedef CYapfBaseT<Types> PfBase; + typedef CYapfFollowRoadT<Types> PfFollow; + typedef CYapfOriginTileT<Types> PfOrigin; + typedef Tdestination<Types> PfDestination; + typedef CYapfSegmentCostCacheNoneT<Types> PfCache; + typedef CYapfCostRoadT<Types> PfCost; +}; + +struct CYapfRoad1 : CYapfT<CYapfRoad_TypesT<CYapfRoad1 , CRoadNodeListTrackDir, CYapfDestinationTileRoadT > > {}; +struct CYapfRoad2 : CYapfT<CYapfRoad_TypesT<CYapfRoad2 , CRoadNodeListExitDir , CYapfDestinationTileRoadT > > {}; + +struct CYapfRoadAnyDepot1 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot1, CRoadNodeListTrackDir, CYapfDestinationAnyDepotRoadT> > {}; +struct CYapfRoadAnyDepot2 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyDepot2, CRoadNodeListExitDir , CYapfDestinationAnyDepotRoadT> > {}; + +struct CYapfRoadAnyRoadVehicleCompatibleStopOfGivenStation1 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyRoadVehicleCompatibleStopOfGivenStation1, CRoadNodeListTrackDir, CYapfDestinationAnyRoadVehicleCompatibleStopOfGivenStationT> > {}; +struct CYapfRoadAnyRoadVehicleCompatibleStopOfGivenStation2 : CYapfT<CYapfRoad_TypesT<CYapfRoadAnyRoadVehicleCompatibleStopOfGivenStation2, CRoadNodeListExitDir , CYapfDestinationAnyRoadVehicleCompatibleStopOfGivenStationT> > {}; + + +Trackdir YapfChooseRoadTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir) +{ + /* default is YAPF type 2 */ + typedef Trackdir (*PfnChooseRoadTrack)(const Vehicle*, TileIndex, DiagDirection); + PfnChooseRoadTrack pfnChooseRoadTrack = &CYapfRoad2::stChooseRoadTrack; // default: ExitDir, allow 90-deg + + /* check if non-default YAPF type should be used */ + if (_settings_game.pf.yapf.disable_node_optimization) { + pfnChooseRoadTrack = &CYapfRoad1::stChooseRoadTrack; // Trackdir, allow 90-deg + } + + Trackdir td_ret = pfnChooseRoadTrack(v, tile, enterdir); + return td_ret; +} + +uint YapfRoadVehDistanceToTile(const Vehicle *v, TileIndex tile) +{ + /* default is YAPF type 2 */ + typedef uint (*PfnDistanceToTile)(const Vehicle*, TileIndex); + PfnDistanceToTile pfnDistanceToTile = &CYapfRoad2::stDistanceToTile; // default: ExitDir, allow 90-deg + + /* check if non-default YAPF type should be used */ + if (_settings_game.pf.yapf.disable_node_optimization) { + pfnDistanceToTile = &CYapfRoad1::stDistanceToTile; // Trackdir, allow 90-deg + } + + /* measure distance in YAPF units */ + uint dist = pfnDistanceToTile(v, tile); + /* convert distance to tiles */ + if (dist != UINT_MAX) { + dist = (dist + YAPF_TILE_LENGTH - 1) / YAPF_TILE_LENGTH; + } + + return dist; +} + +bool YapfFindNearestRoadDepot(const Vehicle *v, int max_distance, TileIndex *depot_tile) +{ + *depot_tile = INVALID_TILE; + + TileIndex tile = v->tile; + Trackdir trackdir = v->GetVehicleTrackdir(); + if ((TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, RoadVehicle::From(v)->compatible_roadtypes)) & TrackdirToTrackdirBits(trackdir)) == 0) { + return false; + } + + /* handle the case when our vehicle is already in the depot tile */ + if (IsRoadDepotTile(tile)) { + /* only what we need to return is the Depot* */ + *depot_tile = tile; + return true; + } + + /* default is YAPF type 2 */ + typedef bool (*PfnFindNearestDepot)(const Vehicle*, TileIndex, Trackdir, int, TileIndex*); + PfnFindNearestDepot pfnFindNearestDepot = &CYapfRoadAnyDepot2::stFindNearestDepot; + + /* check if non-default YAPF type should be used */ + if (_settings_game.pf.yapf.disable_node_optimization) { + pfnFindNearestDepot = &CYapfRoadAnyDepot1::stFindNearestDepot; // Trackdir, allow 90-deg + } + + bool ret = pfnFindNearestDepot(v, tile, trackdir, max_distance, depot_tile); + return ret; +} + +bool YapfFindNearestRoadVehicleCompatibleStop(const RoadVehicle *v, StationID station, TileIndex *stop_tile) +{ + *stop_tile = INVALID_TILE; + + const RoadStop *rs = Station::Get(station)->GetPrimaryRoadStop(v); + if (rs == NULL) return false; + + TileIndex tile = v->tile; + Trackdir trackdir = v->GetVehicleTrackdir(); + if ((TrackStatusToTrackdirBits(GetTileTrackStatus(tile, TRANSPORT_ROAD, RoadVehicle::From(v)->compatible_roadtypes)) & TrackdirToTrackdirBits(trackdir)) == 0) { + return false; + } + + /* default is YAPF type 2 */ + typedef bool (*PfnFindNearestRoadVehicleCompatibleStop)(const RoadVehicle*, TileIndex, TileIndex, Trackdir, StationID, TileIndex*); + PfnFindNearestRoadVehicleCompatibleStop pfnFindNearestRoadVehicleCompatibleStop = &CYapfRoadAnyRoadVehicleCompatibleStopOfGivenStation2::stFindNearestRoadVehicleCompatibleStop; + + /* check if non-default YAPF type should be used */ + if (_settings_game.pf.yapf.disable_node_optimization) { + pfnFindNearestRoadVehicleCompatibleStop = &CYapfRoadAnyRoadVehicleCompatibleStopOfGivenStation1::stFindNearestRoadVehicleCompatibleStop; // Trackdir, allow 90-deg + } + + bool ret = pfnFindNearestRoadVehicleCompatibleStop(v, tile, rs->xy, trackdir, station, stop_tile); + return ret; +} diff --git a/src/pathfinder/yapf/yapf_ship.cpp b/src/pathfinder/yapf/yapf_ship.cpp new file mode 100644 index 000000000..3ae152bd6 --- /dev/null +++ b/src/pathfinder/yapf/yapf_ship.cpp @@ -0,0 +1,203 @@ +/* $Id$ */ + +/* + * This file is part of OpenTTD. + * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2. + * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>. + */ + +/** @file yapf_ship.cpp Implementation of YAPF for ships. */ + +#include "../../stdafx.h" + +#include "yapf.hpp" + +/** Node Follower module of YAPF for ships */ +template <class Types> +class CYapfFollowShipT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + /** to access inherited path finder */ + FORCEINLINE Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** Called by YAPF to move from the given node to the next tile. For each + * reachable trackdir on the new tile creates new node, initializes it + * and adds it to the open list by calling Yapf().AddNewNode(n) */ + inline void PfFollowNode(Node& old_node) + { + TrackFollower F(Yapf().GetVehicle()); + if (F.Follow(old_node.m_key.m_tile, old_node.m_key.m_td)) { + Yapf().AddMultipleNodes(&old_node, F); + } + } + + /** return debug report character to identify the transportation type */ + FORCEINLINE char TransportTypeChar() const + { + return 'w'; + } + + static Trackdir ChooseShipTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir, TrackBits tracks) + { + /* handle special case - when next tile is destination tile */ + if (tile == v->dest_tile) { + /* convert tracks to trackdirs */ + TrackdirBits trackdirs = (TrackdirBits)(tracks | ((int)tracks << 8)); + /* choose any trackdir reachable from enterdir */ + trackdirs &= DiagdirReachesTrackdirs(enterdir); + return (Trackdir)FindFirstBit2x64(trackdirs); + } + + /* move back to the old tile/trackdir (where ship is coming from) */ + TileIndex src_tile = TILE_ADD(tile, TileOffsByDiagDir(ReverseDiagDir(enterdir))); + Trackdir trackdir = v->GetVehicleTrackdir(); + assert(IsValidTrackdir(trackdir)); + + /* convert origin trackdir to TrackdirBits */ + TrackdirBits trackdirs = TrackdirToTrackdirBits(trackdir); + /* get available trackdirs on the destination tile */ + TrackdirBits dest_trackdirs = TrackStatusToTrackdirBits(GetTileTrackStatus(v->dest_tile, TRANSPORT_WATER, 0)); + + /* create pathfinder instance */ + Tpf pf; + /* set origin and destination nodes */ + pf.SetOrigin(src_tile, trackdirs); + pf.SetDestination(v->dest_tile, dest_trackdirs); + /* find best path */ + pf.FindPath(v); + + Trackdir next_trackdir = INVALID_TRACKDIR; // this would mean "path not found" + + Node *pNode = pf.GetBestNode(); + if (pNode != NULL) { + /* walk through the path back to the origin */ + Node *pPrevNode = NULL; + while (pNode->m_parent != NULL) { + pPrevNode = pNode; + pNode = pNode->m_parent; + } + /* return trackdir from the best next node (direct child of origin) */ + Node& best_next_node = *pPrevNode; + assert(best_next_node.GetTile() == tile); + next_trackdir = best_next_node.GetTrackdir(); + } + return next_trackdir; + } +}; + +/** Cost Provider module of YAPF for ships */ +template <class Types> +class CYapfCostShipT +{ +public: + typedef typename Types::Tpf Tpf; ///< the pathfinder class (derived from THIS class) + typedef typename Types::TrackFollower TrackFollower; + typedef typename Types::NodeList::Titem Node; ///< this will be our node type + typedef typename Node::Key Key; ///< key to hash tables + +protected: + /** to access inherited path finder */ + Tpf& Yapf() + { + return *static_cast<Tpf*>(this); + } + +public: + /** Called by YAPF to calculate the cost from the origin to the given node. + * Calculates only the cost of given node, adds it to the parent node cost + * and stores the result into Node::m_cost member */ + FORCEINLINE bool PfCalcCost(Node& n, const TrackFollower *tf) + { + /* base tile cost depending on distance */ + int c = IsDiagonalTrackdir(n.GetTrackdir()) ? YAPF_TILE_LENGTH : YAPF_TILE_CORNER_LENGTH; + /* additional penalty for curves */ + if (n.m_parent != NULL && n.GetTrackdir() != NextTrackdir(n.m_parent->GetTrackdir())) { + /* new trackdir does not match the next one when going straight */ + c += YAPF_TILE_LENGTH; + } + + c += YAPF_TILE_LENGTH * tf->m_tiles_skipped; + + /* apply it */ + n.m_cost = n.m_parent->m_cost + c; + return true; + } +}; + +/** Config struct of YAPF for ships. + * Defines all 6 base YAPF modules as classes providing services for CYapfBaseT. + */ +template <class Tpf_, class Ttrack_follower, class Tnode_list> +struct CYapfShip_TypesT +{ + /** Types - shortcut for this struct type */ + typedef CYapfShip_TypesT<Tpf_, Ttrack_follower, Tnode_list> Types; + + /** Tpf - pathfinder type */ + typedef Tpf_ Tpf; + /** track follower helper class */ + typedef Ttrack_follower TrackFollower; + /** node list type */ + typedef Tnode_list NodeList; + /** pathfinder components (modules) */ + typedef CYapfBaseT<Types> PfBase; // base pathfinder class + typedef CYapfFollowShipT<Types> PfFollow; // node follower + typedef CYapfOriginTileT<Types> PfOrigin; // origin provider + typedef CYapfDestinationTileT<Types> PfDestination; // destination/distance provider + typedef CYapfSegmentCostCacheNoneT<Types> PfCache; // segment cost cache provider + typedef CYapfCostShipT<Types> PfCost; // cost provider +}; + +/* YAPF type 1 - uses TileIndex/Trackdir as Node key, allows 90-deg turns */ +struct CYapfShip1 : CYapfT<CYapfShip_TypesT<CYapfShip1, CFollowTrackWater , CShipNodeListTrackDir> > {}; +/* YAPF type 2 - uses TileIndex/DiagDirection as Node key, allows 90-deg turns */ +struct CYapfShip2 : CYapfT<CYapfShip_TypesT<CYapfShip2, CFollowTrackWater , CShipNodeListExitDir > > {}; +/* YAPF type 3 - uses TileIndex/Trackdir as Node key, forbids 90-deg turns */ +struct CYapfShip3 : CYapfT<CYapfShip_TypesT<CYapfShip3, CFollowTrackWaterNo90, CShipNodeListTrackDir> > {}; + +/** Ship controller helper - path finder invoker */ +Trackdir YapfChooseShipTrack(const Vehicle *v, TileIndex tile, DiagDirection enterdir, TrackBits tracks) +{ + /* default is YAPF type 2 */ + typedef Trackdir (*PfnChooseShipTrack)(const Vehicle*, TileIndex, DiagDirection, TrackBits); + PfnChooseShipTrack pfnChooseShipTrack = CYapfShip2::ChooseShipTrack; // default: ExitDir, allow 90-deg + + /* check if non-default YAPF type needed */ + if (_settings_game.pf.forbid_90_deg) { + pfnChooseShipTrack = &CYapfShip3::ChooseShipTrack; // Trackdir, forbid 90-deg + } else if (_settings_game.pf.yapf.disable_node_optimization) { + pfnChooseShipTrack = &CYapfShip1::ChooseShipTrack; // Trackdir, allow 90-deg + } + + Trackdir td_ret = pfnChooseShipTrack(v, tile, enterdir, tracks); + return td_ret; +} + +/** performance measurement helper */ +void *NpfBeginInterval() +{ + CPerformanceTimer& perf = *new CPerformanceTimer; + perf.Start(); + return &perf; +} + +/** performance measurement helper */ +int NpfEndInterval(void *vperf) +{ + CPerformanceTimer& perf = *(CPerformanceTimer*)vperf; + perf.Stop(); + int t = perf.Get(1000000); + delete &perf; + return t; +} |