src/pathfinder/npf/aystar.cpp

   1 /*
   2  * This file is part of OpenTTD.
   3  * 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.
   4  * 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.
   5  * 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/>.
   6  */
   7
   8 /** @file aystar.cpp Implementation of A*.
   9  *
  10  * This file has the core function for %AyStar.
  11  * %AyStar is a fast path finding routine and is used for things like AI path finding and Train path finding.
  12  * For more information about %AyStar (A* Algorithm), you can look at
  13  * <A HREF='http://en.wikipedia.org/wiki/A-star_search_algorithm'>http://en.wikipedia.org/wiki/A-star_search_algorithm</A>.
  14  */
  15
  16 /*
  17  * Friendly reminder:
  18  *  Call (AyStar).free() when you are done with Aystar. It reserves a lot of memory
  19  *  And when not free'd, it can cause system-crashes.
  20  * Also remember that when you stop an algorithm before it is finished, your
  21  * should call clear() yourself!
  22  */
  23
  24 #include "../../stdafx.h"
  25 #include "../../core/alloc_func.hpp"
  26 #include "aystar.h"
  27
  28 #include "../../safeguards.h"
  29
  30 /**
  31  * This looks in the hash whether a node exists in the closed list.
  32  * @param node Node to search.
  33  * @return The #PathNode if it is available, else \c nullptr
  34  */
  35 PathNode *AyStar::ClosedListIsInList(const AyStarNode *node)
  36 {
  37         return (PathNode*)this->closedlist_hash.Get(node->tile, node->direction);
  38 }
  39
  40 /**
  41  * This adds a node to the closed list.
  42  * It makes a copy of the data.
  43  * @param node Node to add to the closed list.
  44  */
  45 void AyStar::ClosedListAdd(const PathNode *node)
  46 {
  47         /* Add a node to the ClosedList */
  48         PathNode *new_node = MallocT<PathNode>(1);
  49         *new_node = *node;
  50         this->closedlist_hash.Set(node->node.tile, node->node.direction, new_node);
  51 }
  52
  53 /**
  54  * Check whether a node is in the open list.
  55  * @param node Node to search.
  56  * @return If the node is available, it is returned, else \c nullptr is returned.
  57  */
  58 OpenListNode *AyStar::OpenListIsInList(const AyStarNode *node)
  59 {
  60         return (OpenListNode*)this->openlist_hash.Get(node->tile, node->direction);
  61 }
  62
  63 /**
  64  * Gets the best node from the open list.
  65  * It deletes the returned node from the open list.
  66  * @returns the best node available, or \c nullptr of none is found.
  67  */
  68 OpenListNode *AyStar::OpenListPop()
  69 {
  70         /* Return the item the Queue returns.. the best next OpenList item. */
  71         OpenListNode *res = (OpenListNode*)this->openlist_queue.Pop();
  72         if (res != nullptr) {
  73                 this->openlist_hash.DeleteValue(res->path.node.tile, res->path.node.direction);
  74         }
  75
  76         return res;
  77 }
  78
  79 /**
  80  * Adds a node to the open list.
  81  * It makes a copy of node, and puts the pointer of parent in the struct.
  82  */
  83 void AyStar::OpenListAdd(PathNode *parent, const AyStarNode *node, int f, int g)
  84 {
  85         /* Add a new Node to the OpenList */
  86         OpenListNode *new_node = MallocT<OpenListNode>(1);
  87         new_node->g = g;
  88         new_node->path.parent = parent;
  89         new_node->path.node = *node;
  90         this->openlist_hash.Set(node->tile, node->direction, new_node);
  91
  92         /* Add it to the queue */
  93         this->openlist_queue.Push(new_node, f);
  94 }
  95
  96 /**
  97  * Checks one tile and calculate its f-value
  98  */
  99 void AyStar::CheckTile(AyStarNode *current, OpenListNode *parent)
 100 {
 101         int new_f, new_g, new_h;
 102         PathNode *closedlist_parent;
 103         OpenListNode *check;
 104
 105         /* Check the new node against the ClosedList */
 106         if (this->ClosedListIsInList(current) != nullptr) return;
 107
 108         /* Calculate the G-value for this node */
 109         new_g = this->CalculateG(this, current, parent);
 110         /* If the value was INVALID_NODE, we don't do anything with this node */
 111         if (new_g == AYSTAR_INVALID_NODE) return;
 112
 113         /* There should not be given any other error-code.. */
 114         assert(new_g >= 0);
 115         /* Add the parent g-value to the new g-value */
 116         new_g += parent->g;
 117         if (this->max_path_cost != 0 && (uint)new_g > this->max_path_cost) return;
 118
 119         /* Calculate the h-value */
 120         new_h = this->CalculateH(this, current, parent);
 121         /* There should not be given any error-code.. */
 122         assert(new_h >= 0);
 123
 124         /* The f-value if g + h */
 125         new_f = new_g + new_h;
 126
 127         /* Get the pointer to the parent in the ClosedList (the current one is to a copy of the one in the OpenList) */
 128         closedlist_parent = this->ClosedListIsInList(&parent->path.node);
 129
 130         /* Check if this item is already in the OpenList */
 131         check = this->OpenListIsInList(current);
 132         if (check != nullptr) {
 133                 uint i;
 134                 /* Yes, check if this g value is lower.. */
 135                 if (new_g > check->g) return;
 136                 this->openlist_queue.Delete(check, 0);
 137                 /* It is lower, so change it to this item */
 138                 check->g = new_g;
 139                 check->path.parent = closedlist_parent;
 140                 /* Copy user data, will probably have changed */
 141                 for (i = 0; i < lengthof(current->user_data); i++) {
 142                         check->path.node.user_data[i] = current->user_data[i];
 143                 }
 144                 /* Re-add it in the openlist_queue. */
 145                 this->openlist_queue.Push(check, new_f);
 146         } else {
 147                 /* A new node, add him to the OpenList */
 148                 this->OpenListAdd(closedlist_parent, current, new_f, new_g);
 149         }
 150 }
 151
 152 /**
 153  * This function is the core of %AyStar. It handles one item and checks
 154  * his neighbour items. If they are valid, they are added to be checked too.
 155  * @return Possible values:
 156  *  - #AYSTAR_EMPTY_OPENLIST : indicates all items are tested, and no path has been found.
 157  *  - #AYSTAR_LIMIT_REACHED : Indicates that the max_search_nodes limit has been reached.
 158  *  - #AYSTAR_FOUND_END_NODE : indicates we found the end. Path_found now is true, and in path is the path found.
 159  *  - #AYSTAR_STILL_BUSY : indicates we have done this tile, did not found the path yet, and have items left to try.
 160  */
 161 int AyStar::Loop()
 162 {
 163         int i;
 164
 165         /* Get the best node from OpenList */
 166         OpenListNode *current = this->OpenListPop();
 167         /* If empty, drop an error */
 168         if (current == nullptr) return AYSTAR_EMPTY_OPENLIST;
 169
 170         /* Check for end node and if found, return that code */
 171         if (this->EndNodeCheck(this, current) == AYSTAR_FOUND_END_NODE && !CheckIgnoreFirstTile(&current->path)) {
 172                 if (this->FoundEndNode != nullptr) {
 173                         this->FoundEndNode(this, current);
 174                 }
 175                 free(current);
 176                 return AYSTAR_FOUND_END_NODE;
 177         }
 178
 179         /* Add the node to the ClosedList */
 180         this->ClosedListAdd(&current->path);
 181
 182         /* Load the neighbours */
 183         this->GetNeighbours(this, current);
 184
 185         /* Go through all neighbours */
 186         for (i = 0; i < this->num_neighbours; i++) {
 187                 /* Check and add them to the OpenList if needed */
 188                 this->CheckTile(&this->neighbours[i], current);
 189         }
 190
 191         /* Free the node */
 192         free(current);
 193
 194         if (this->max_search_nodes != 0 && this->closedlist_hash.GetSize() >= this->max_search_nodes) {
 195                 /* We've expanded enough nodes */
 196                 return AYSTAR_LIMIT_REACHED;
 197         } else {
 198                 /* Return that we are still busy */
 199                 return AYSTAR_STILL_BUSY;
 200         }
 201 }
 202
 203 /**
 204  * This function frees the memory it allocated
 205  */
 206 void AyStar::Free()
 207 {
 208         this->openlist_queue.Free(false);
 209         /* 2nd argument above is false, below is true, to free the values only
 210          * once */
 211         this->openlist_hash.Delete(true);
 212         this->closedlist_hash.Delete(true);
 213 #ifdef AYSTAR_DEBUG
 214         printf("[AyStar] Memory free'd\n");
 215 #endif
 216 }
 217
 218 /**
 219  * This function make the memory go back to zero.
 220  * This function should be called when you are using the same instance again.
 221  */
 222 void AyStar::Clear()
 223 {
 224         /* Clean the Queue, but not the elements within. That will be done by
 225          * the hash. */
 226         this->openlist_queue.Clear(false);
 227         /* Clean the hashes */
 228         this->openlist_hash.Clear(true);
 229         this->closedlist_hash.Clear(true);
 230
 231 #ifdef AYSTAR_DEBUG
 232         printf("[AyStar] Cleared AyStar\n");
 233 #endif
 234 }
 235
 236 /**
 237  * This is the function you call to run AyStar.
 238  * @return Possible values:
 239  *  - #AYSTAR_FOUND_END_NODE : indicates we found an end node.
 240  *  - #AYSTAR_NO_PATH : indicates that there was no path found.
 241  *  - #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.
 242  * @note When the algorithm is done (when the return value is not #AYSTAR_STILL_BUSY) #Clear() is called automatically.
 243  *       When you stop the algorithm halfway, you should call #Clear() yourself!
 244  */
 245 int AyStar::Main()
 246 {
 247         int r, i = 0;
 248         /* Loop through the OpenList
 249          *  Quit if result is no AYSTAR_STILL_BUSY or is more than loops_per_tick */
 250         while ((r = this->Loop()) == AYSTAR_STILL_BUSY && (this->loops_per_tick == 0 || ++i < this->loops_per_tick)) { }
 251 #ifdef AYSTAR_DEBUG
 252         switch (r) {
 253                 case AYSTAR_FOUND_END_NODE: printf("[AyStar] Found path!\n"); break;
 254                 case AYSTAR_EMPTY_OPENLIST: printf("[AyStar] OpenList run dry, no path found\n"); break;
 255                 case AYSTAR_LIMIT_REACHED:  printf("[AyStar] Exceeded search_nodes, no path found\n"); break;
 256                 default: break;
 257         }
 258 #endif
 259         if (r != AYSTAR_STILL_BUSY) {
 260                 /* We're done, clean up */
 261                 this->Clear();
 262         }
 263
 264         switch (r) {
 265                 case AYSTAR_FOUND_END_NODE: return AYSTAR_FOUND_END_NODE;
 266                 case AYSTAR_EMPTY_OPENLIST:
 267                 case AYSTAR_LIMIT_REACHED:  return AYSTAR_NO_PATH;
 268                 default:                    return AYSTAR_STILL_BUSY;
 269         }
 270 }
 271
 272 /**
 273  * Adds a node from where to start an algorithm. Multiple nodes can be added
 274  * if wanted. You should make sure that #Clear() is called before adding nodes
 275  * if the #AyStar has been used before (though the normal main loop calls
 276  * #Clear() automatically when the algorithm finishes.
 277  * @param start_node Node to start with.
 278  * @param g the cost for starting with this node.
 279  */
 280 void AyStar::AddStartNode(AyStarNode *start_node, uint g)
 281 {
 282 #ifdef AYSTAR_DEBUG
 283         printf("[AyStar] Starting A* Algorithm from node (%d, %d, %d)\n",
 284                 TileX(start_node->tile), TileY(start_node->tile), start_node->direction);
 285 #endif
 286         this->OpenListAdd(nullptr, start_node, 0, g);
 287 }
 288
 289 /**
 290  * Initialize an #AyStar. You should fill all appropriate fields before
 291  * calling #Init (see the declaration of #AyStar for which fields are internal).
 292  */
 293 void AyStar::Init(Hash_HashProc hash, uint num_buckets)
 294 {
 295         /* Allocated the Hash for the OpenList and ClosedList */
 296         this->openlist_hash.Init(hash, num_buckets);
 297         this->closedlist_hash.Init(hash, num_buckets);
 298
 299         /* Set up our sorting queue
 300          *  BinaryHeap allocates a block of 1024 nodes
 301          *  When that one gets full it reserves another one, till this number
 302          *  That is why it can stay this high */
 303         this->openlist_queue.Init(102400);
 304 }