00001
00002
00003
00004
00005
00006
00007
00008
00009
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00036
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
00043
00044 static void AyStarMain_ClosedList_Add(AyStar *aystar, const PathNode *node)
00045 {
00046
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
00053
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
00060
00061
00062 static OpenListNode *AyStarMain_OpenList_Pop(AyStar *aystar)
00063 {
00064
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
00074
00075 static void AyStarMain_OpenList_Add(AyStar *aystar, PathNode *parent, const AyStarNode *node, int f, int g)
00076 {
00077
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
00085 aystar->OpenListQueue.push(&aystar->OpenListQueue, new_node, f);
00086 }
00087
00088
00089
00090
00091
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
00100 if (AyStarMain_ClosedList_IsInList(aystar, current) != NULL) return AYSTAR_DONE;
00101
00102
00103 new_g = aystar->CalculateG(aystar, current, parent);
00104
00105 if (new_g == AYSTAR_INVALID_NODE) return AYSTAR_DONE;
00106
00107
00108 assert(new_g >= 0);
00109
00110 new_g += parent->g;
00111 if (aystar->max_path_cost != 0 && (uint)new_g > aystar->max_path_cost) return AYSTAR_DONE;
00112
00113
00114 new_h = aystar->CalculateH(aystar, current, parent);
00115
00116 assert(new_h >= 0);
00117
00118
00119 new_f = new_g + new_h;
00120
00121
00122 closedlist_parent = AyStarMain_ClosedList_IsInList(aystar, &parent->path.node);
00123
00124
00125 check = AyStarMain_OpenList_IsInList(aystar, current);
00126 if (check != NULL) {
00127 uint i;
00128
00129 if (new_g > check->g) return AYSTAR_DONE;
00130 aystar->OpenListQueue.del(&aystar->OpenListQueue, check, 0);
00131
00132 check->g = new_g;
00133 check->path.parent = closedlist_parent;
00134
00135 for (i = 0; i < lengthof(current->user_data); i++) {
00136 check->path.node.user_data[i] = current->user_data[i];
00137 }
00138
00139 aystar->OpenListQueue.push(&aystar->OpenListQueue, check, new_f);
00140 } else {
00141
00142 AyStarMain_OpenList_Add(aystar, closedlist_parent, current, new_f, new_g);
00143 }
00144
00145 return AYSTAR_DONE;
00146 }
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 static int AyStarMain_Loop(AyStar *aystar)
00160 {
00161 int i, r;
00162
00163
00164 OpenListNode *current = AyStarMain_OpenList_Pop(aystar);
00165
00166 if (current == NULL) return AYSTAR_EMPTY_OPENLIST;
00167
00168
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
00177 AyStarMain_ClosedList_Add(aystar, ¤t->path);
00178
00179
00180 aystar->GetNeighbours(aystar, current);
00181
00182
00183 for (i = 0; i < aystar->num_neighbours; i++) {
00184
00185 r = aystar->checktile(aystar, &aystar->neighbours[i], current);
00186 }
00187
00188
00189 free(current);
00190
00191 if (aystar->max_search_nodes != 0 && Hash_Size(&aystar->ClosedListHash) >= aystar->max_search_nodes) {
00192
00193 return AYSTAR_LIMIT_REACHED;
00194 } else {
00195
00196 return AYSTAR_STILL_BUSY;
00197 }
00198 }
00199
00200
00201
00202
00203 static void AyStarMain_Free(AyStar *aystar)
00204 {
00205 aystar->OpenListQueue.free(&aystar->OpenListQueue, false);
00206
00207
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
00217
00218
00219 void AyStarMain_Clear(AyStar *aystar)
00220 {
00221
00222
00223 aystar->OpenListQueue.clear(&aystar->OpenListQueue, false);
00224
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
00235
00236
00237
00238
00239
00240
00241
00242
00243 int AyStarMain_Main(AyStar *aystar)
00244 {
00245 int r, i = 0;
00246
00247
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
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
00274
00275
00276
00277
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
00291 init_Hash(&aystar->OpenListHash, hash, num_buckets);
00292 init_Hash(&aystar->ClosedListHash, hash, num_buckets);
00293
00294
00295
00296
00297
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 }