# aystar.cpp

Go to the documentation of this file.

    00001 /* $Id: aystar.cpp 18683 2010-01-01 18:45:40Z rubidium $ */
    00002
    00003 /*
    00004  * This file is part of OpenTTD.
    00005  * 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.
    00006  * 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.
    00007  * 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/>.
    00008  */
    00009
    00012 /*
    00013  * This file has the core function for AyStar
    00014  *  AyStar is a fast pathfinding routine and is used for things like
    00015  *  AI_pathfinding and Train_pathfinding.
    00016  *  For more information about AyStar (A* Algorithm), you can look at
    00017  *    http://en.wikipedia.org/wiki/A-star_search_algorithm
    00018  */
    00019
    00020 /*
    00021  * Friendly reminder:
    00022  *  Call (AyStar).free() when you are done with Aystar. It reserves a lot of memory
    00023  *  And when not free'd, it can cause system-crashes.
    00024  * Also remember that when you stop an algorithm before it is finished, your
    00025  * should call clear() yourself!
    00026  */
    00027
    00028 #include "../../stdafx.h"
    00029 #include "../../core/alloc_func.hpp"
    00030 #include "aystar.h"
    00031
    00032 static int _aystar_stats_open_size;
    00033 static int _aystar_stats_closed_size;
    00034
    00035 /* This looks in the Hash if a node exists in ClosedList
    00036  *  If so, it returns the PathNode, else NULL */
    00037 static PathNode *AyStarMain_ClosedList_IsInList(AyStar *aystar, const AyStarNode *node)
    00038 {
    00039   return (PathNode*)Hash_Get(&aystar->ClosedListHash, node->tile, node->direction);
    00040 }
    00041
    00042 /* This adds a node to the ClosedList
    00043  *  It makes a copy of the data */
    00044 static void AyStarMain_ClosedList_Add(AyStar *aystar, const PathNode *node)
    00045 {
    00046   /* Add a node to the ClosedList */
    00047   PathNode *new_node = MallocT<PathNode>(1);
    00048   *new_node = *node;
    00049   Hash_Set(&aystar->ClosedListHash, node->node.tile, node->node.direction, new_node);
    00050 }
    00051
    00052 /* Checks if a node is in the OpenList
    00053  *   If so, it returns the OpenListNode, else NULL */
    00054 static OpenListNode *AyStarMain_OpenList_IsInList(AyStar *aystar, const AyStarNode *node)
    00055 {
    00056   return (OpenListNode*)Hash_Get(&aystar->OpenListHash, node->tile, node->direction);
    00057 }
    00058
    00059 /* Gets the best node from OpenList
    00060  *  returns the best node, or NULL of none is found
    00061  * Also it deletes the node from the OpenList */
    00062 static OpenListNode *AyStarMain_OpenList_Pop(AyStar *aystar)
    00063 {
    00064   /* Return the item the Queue returns.. the best next OpenList item. */
    00065   OpenListNode *res = (OpenListNode*)aystar->OpenListQueue.pop(&aystar->OpenListQueue);
    00066   if (res != NULL) {
    00067     Hash_Delete(&aystar->OpenListHash, res->path.node.tile, res->path.node.direction);
    00068   }
    00069
    00070   return res;
    00071 }
    00072
    00073 /* Adds a node to the OpenList
    00074  *  It makes a copy of node, and puts the pointer of parent in the struct */
    00075 static void AyStarMain_OpenList_Add(AyStar *aystar, PathNode *parent, const AyStarNode *node, int f, int g)
    00076 {
    00077   /* Add a new Node to the OpenList */
    00078   OpenListNode *new_node = MallocT<OpenListNode>(1);
    00079   new_node->g = g;
    00080   new_node->path.parent = parent;
    00081   new_node->path.node = *node;
    00082   Hash_Set(&aystar->OpenListHash, node->tile, node->direction, new_node);
    00083
    00084   /* Add it to the queue */
    00085   aystar->OpenListQueue.push(&aystar->OpenListQueue, new_node, f);
    00086 }
    00087
    00088 /*
    00089  * Checks one tile and calculate his f-value
    00090  *  return values:
    00091  * AYSTAR_DONE : indicates we are done
    00092  */
    00093 static int AyStarMain_CheckTile(AyStar *aystar, AyStarNode *current, OpenListNode *parent)
    00094 {
    00095   int new_f, new_g, new_h;
    00096   PathNode *closedlist_parent;
    00097   OpenListNode *check;
    00098
    00099   /* Check the new node against the ClosedList */
    00100   if (AyStarMain_ClosedList_IsInList(aystar, current) != NULL) return AYSTAR_DONE;
    00101
    00102   /* Calculate the G-value for this node */
    00103   new_g = aystar->CalculateG(aystar, current, parent);
    00104   /* If the value was INVALID_NODE, we don't do anything with this node */
    00105   if (new_g == AYSTAR_INVALID_NODE) return AYSTAR_DONE;
    00106
    00107   /* There should not be given any other error-code.. */
    00108   assert(new_g >= 0);
    00109   /* Add the parent g-value to the new g-value */
    00110   new_g += parent->g;
    00111   if (aystar->max_path_cost != 0 && (uint)new_g > aystar->max_path_cost) return AYSTAR_DONE;
    00112
    00113   /* Calculate the h-value */
    00114   new_h = aystar->CalculateH(aystar, current, parent);
    00115   /* There should not be given any error-code.. */
    00116   assert(new_h >= 0);
    00117
    00118   /* The f-value if g + h */
    00119   new_f = new_g + new_h;
    00120
    00121   /* Get the pointer to the parent in the ClosedList (the currentone is to a copy of the one in the OpenList) */
    00122   closedlist_parent = AyStarMain_ClosedList_IsInList(aystar, &parent->path.node);
    00123
    00124   /* Check if this item is already in the OpenList */
    00125   check = AyStarMain_OpenList_IsInList(aystar, current);
    00126   if (check != NULL) {
    00127     uint i;
    00128     /* Yes, check if this g value is lower.. */
    00129     if (new_g > check->g) return AYSTAR_DONE;
    00130     aystar->OpenListQueue.del(&aystar->OpenListQueue, check, 0);
    00131     /* It is lower, so change it to this item */
    00132     check->g = new_g;
    00133     check->path.parent = closedlist_parent;
    00134     /* Copy user data, will probably have changed */
    00135     for (i = 0; i < lengthof(current->user_data); i++) {
    00136       check->path.node.user_data[i] = current->user_data[i];
    00137     }
    00138     /* Readd him in the OpenListQueue */
    00139     aystar->OpenListQueue.push(&aystar->OpenListQueue, check, new_f);
    00140   } else {
    00141     /* A new node, add him to the OpenList */
    00142     AyStarMain_OpenList_Add(aystar, closedlist_parent, current, new_f, new_g);
    00143   }
    00144
    00145   return AYSTAR_DONE;
    00146 }
    00147
    00148 /*
    00149  * This function is the core of AyStar. It handles one item and checks
    00150  *  his neighbour items. If they are valid, they are added to be checked too.
    00151  *  return values:
    00152  *   AYSTAR_EMPTY_OPENLIST : indicates all items are tested, and no path
    00153  *    has been found.
    00154  *   AYSTAR_LIMIT_REACHED : Indicates that the max_nodes limit has been
    00155  *    reached.
    00156  *   AYSTAR_FOUND_END_NODE : indicates we found the end. Path_found now is true, and in path is the path found.
    00157  *   AYSTAR_STILL_BUSY : indicates we have done this tile, did not found the path yet, and have items left to try.
    00158  */
    00159 static int AyStarMain_Loop(AyStar *aystar)
    00160 {
    00161   int i, r;
    00162
    00163   /* Get the best node from OpenList */
    00164   OpenListNode *current = AyStarMain_OpenList_Pop(aystar);
    00165   /* If empty, drop an error */
    00166   if (current == NULL) return AYSTAR_EMPTY_OPENLIST;
    00167
    00168   /* Check for end node and if found, return that code */
    00169   if (aystar->EndNodeCheck(aystar, current) == AYSTAR_FOUND_END_NODE) {
    00170     if (aystar->FoundEndNode != NULL)
    00171       aystar->FoundEndNode(aystar, current);
    00172     free(current);
    00173     return AYSTAR_FOUND_END_NODE;
    00174   }
    00175
    00176   /* Add the node to the ClosedList */
    00177   AyStarMain_ClosedList_Add(aystar, &current->path);
    00178
    00179   /* Load the neighbours */
    00180   aystar->GetNeighbours(aystar, current);
    00181
    00182   /* Go through all neighbours */
    00183   for (i = 0; i < aystar->num_neighbours; i++) {
    00184     /* Check and add them to the OpenList if needed */
    00185     r = aystar->checktile(aystar, &aystar->neighbours[i], current);
    00186   }
    00187
    00188   /* Free the node */
    00189   free(current);
    00190
    00191   if (aystar->max_search_nodes != 0 && Hash_Size(&aystar->ClosedListHash) >= aystar->max_search_nodes) {
    00192     /* We've expanded enough nodes */
    00193     return AYSTAR_LIMIT_REACHED;
    00194   } else {
    00195     /* Return that we are still busy */
    00196     return AYSTAR_STILL_BUSY;
    00197   }
    00198 }
    00199
    00200 /*
    00201  * This function frees the memory it allocated
    00202  */
    00203 static void AyStarMain_Free(AyStar *aystar)
    00204 {
    00205   aystar->OpenListQueue.free(&aystar->OpenListQueue, false);
    00206   /* 2nd argument above is false, below is true, to free the values only
    00207    * once */
    00208   delete_Hash(&aystar->OpenListHash, true);
    00209   delete_Hash(&aystar->ClosedListHash, true);
    00210 #ifdef AYSTAR_DEBUG
    00211   printf("[AyStar] Memory free'd\n");
    00212 #endif
    00213 }
    00214
    00215 /*
    00216  * This function make the memory go back to zero
    00217  *  This function should be called when you are using the same instance again.
    00218  */
    00219 void AyStarMain_Clear(AyStar *aystar)
    00220 {
    00221   /* Clean the Queue, but not the elements within. That will be done by
    00222    * the hash. */
    00223   aystar->OpenListQueue.clear(&aystar->OpenListQueue, false);
    00224   /* Clean the hashes */
    00225   clear_Hash(&aystar->OpenListHash, true);
    00226   clear_Hash(&aystar->ClosedListHash, true);
    00227
    00228 #ifdef AYSTAR_DEBUG
    00229   printf("[AyStar] Cleared AyStar\n");
    00230 #endif
    00231 }
    00232
    00233 /*
    00234  * This is the function you call to run AyStar.
    00235  *  return values:
    00236  *   AYSTAR_FOUND_END_NODE : indicates we found an end node.
    00237  *   AYSTAR_NO_PATH : indicates that there was no path found.
    00238  *   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.
    00239  * When the algorithm is done (when the return value is not AYSTAR_STILL_BUSY)
    00240  * aystar->clear() is called. Note that when you stop the algorithm halfway,
    00241  * you should still call clear() yourself!
    00242  */
    00243 int AyStarMain_Main(AyStar *aystar)
    00244 {
    00245   int r, i = 0;
    00246   /* Loop through the OpenList
    00247    *  Quit if result is no AYSTAR_STILL_BUSY or is more than loops_per_tick */
    00248   while ((r = aystar->loop(aystar)) == AYSTAR_STILL_BUSY && (aystar->loops_per_tick == 0 || ++i < aystar->loops_per_tick)) { }
    00249 #ifdef AYSTAR_DEBUG
    00250   switch (r) {
    00251     case AYSTAR_FOUND_END_NODE: printf("[AyStar] Found path!\n"); break;
    00252     case AYSTAR_EMPTY_OPENLIST: printf("[AyStar] OpenList run dry, no path found\n"); break;
    00253     case AYSTAR_LIMIT_REACHED:  printf("[AyStar] Exceeded search_nodes, no path found\n"); break;
    00254     default: break;
    00255   }
    00256 #endif
    00257   if (r != AYSTAR_STILL_BUSY) {
    00258     /* We're done, clean up */
    00259     _aystar_stats_open_size = aystar->OpenListHash.size;
    00260     _aystar_stats_closed_size = aystar->ClosedListHash.size;
    00261     aystar->clear(aystar);
    00262   }
    00263
    00264   switch (r) {
    00265     case AYSTAR_FOUND_END_NODE: return AYSTAR_FOUND_END_NODE;
    00266     case AYSTAR_EMPTY_OPENLIST:
    00267     case AYSTAR_LIMIT_REACHED:  return AYSTAR_NO_PATH;
    00268     default:                    return AYSTAR_STILL_BUSY;
    00269   }
    00270 }
    00271
    00272 /*
    00273  * Adds a node from where to start an algorithm. Multiple nodes can be added
    00274  * if wanted. You should make sure that clear() is called before adding nodes
    00275  * if the AyStar has been used before (though the normal main loop calls
    00276  * clear() automatically when the algorithm finishes
    00277  * g is the cost for starting with this node.
    00278  */
    00279 static void AyStarMain_AddStartNode(AyStar *aystar, AyStarNode *start_node, uint g)
    00280 {
    00281 #ifdef AYSTAR_DEBUG
    00282   printf("[AyStar] Starting A* Algorithm from node (%d, %d, %d)\n",
    00283     TileX(start_node->tile), TileY(start_node->tile), start_node->direction);
    00284 #endif
    00285   AyStarMain_OpenList_Add(aystar, NULL, start_node, 0, g);
    00286 }
    00287
    00288 void init_AyStar(AyStar *aystar, Hash_HashProc hash, uint num_buckets)
    00289 {
    00290   /* Allocated the Hash for the OpenList and ClosedList */
    00291   init_Hash(&aystar->OpenListHash, hash, num_buckets);
    00292   init_Hash(&aystar->ClosedListHash, hash, num_buckets);
    00293
    00294   /* Set up our sorting queue
    00295    *  BinaryHeap allocates a block of 1024 nodes
    00296    *  When that one gets full it reserves another one, till this number
    00297    *  That is why it can stay this high */
    00298   init_BinaryHeap(&aystar->OpenListQueue, 102400);
    00299
    00300   aystar->addstart  = AyStarMain_AddStartNode;
    00301   aystar->main      = AyStarMain_Main;
    00302   aystar->loop      = AyStarMain_Loop;
    00303   aystar->free      = AyStarMain_Free;
    00304   aystar->clear     = AyStarMain_Clear;
    00305   aystar->checktile = AyStarMain_CheckTile;
    00306 }