# 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 }