src/pathfinder/npf/aystar.cpp

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