aystar.cpp

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

Generated on Sun Nov 15 15:40:10 2009 for OpenTTD by  doxygen 1.5.6