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 }

Generated on Wed Mar 31 22:43:26 2010 for OpenTTD by  doxygen 1.6.1