aystar.cpp

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

Generated on Wed Oct 1 17:03:20 2008 for openttd by  doxygen 1.5.6