00001
00002
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00029
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
00036
00037 static void AyStarMain_ClosedList_Add(AyStar *aystar, const PathNode *node)
00038 {
00039
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
00046
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
00053
00054
00055 static OpenListNode *AyStarMain_OpenList_Pop(AyStar *aystar)
00056 {
00057
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
00067
00068 static void AyStarMain_OpenList_Add(AyStar *aystar, PathNode *parent, const AyStarNode *node, int f, int g)
00069 {
00070
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
00078 aystar->OpenListQueue.push(&aystar->OpenListQueue, new_node, f);
00079 }
00080
00081
00082
00083
00084
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
00093 if (AyStarMain_ClosedList_IsInList(aystar, current) != NULL) return AYSTAR_DONE;
00094
00095
00096 new_g = aystar->CalculateG(aystar, current, parent);
00097
00098 if (new_g == AYSTAR_INVALID_NODE) return AYSTAR_DONE;
00099
00100
00101 assert(new_g >= 0);
00102
00103 new_g += parent->g;
00104 if (aystar->max_path_cost != 0 && (uint)new_g > aystar->max_path_cost) return AYSTAR_DONE;
00105
00106
00107 new_h = aystar->CalculateH(aystar, current, parent);
00108
00109 assert(new_h >= 0);
00110
00111
00112 new_f = new_g + new_h;
00113
00114
00115 closedlist_parent = AyStarMain_ClosedList_IsInList(aystar, &parent->path.node);
00116
00117
00118 check = AyStarMain_OpenList_IsInList(aystar, current);
00119 if (check != NULL) {
00120 uint i;
00121
00122 if (new_g > check->g) return AYSTAR_DONE;
00123 aystar->OpenListQueue.del(&aystar->OpenListQueue, check, 0);
00124
00125 check->g = new_g;
00126 check->path.parent = closedlist_parent;
00127
00128 for (i = 0; i < lengthof(current->user_data); i++) {
00129 check->path.node.user_data[i] = current->user_data[i];
00130 }
00131
00132 aystar->OpenListQueue.push(&aystar->OpenListQueue, check, new_f);
00133 } else {
00134
00135 AyStarMain_OpenList_Add(aystar, closedlist_parent, current, new_f, new_g);
00136 }
00137
00138 return AYSTAR_DONE;
00139 }
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152 int AyStarMain_Loop(AyStar *aystar)
00153 {
00154 int i, r;
00155
00156
00157 OpenListNode *current = AyStarMain_OpenList_Pop(aystar);
00158
00159 if (current == NULL) return AYSTAR_EMPTY_OPENLIST;
00160
00161
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
00170 AyStarMain_ClosedList_Add(aystar, ¤t->path);
00171
00172
00173 aystar->GetNeighbours(aystar, current);
00174
00175
00176 for (i = 0; i < aystar->num_neighbours; i++) {
00177
00178 r = aystar->checktile(aystar, &aystar->neighbours[i], current);
00179 }
00180
00181
00182 free(current);
00183
00184 if (aystar->max_search_nodes != 0 && Hash_Size(&aystar->ClosedListHash) >= aystar->max_search_nodes) {
00185
00186 return AYSTAR_LIMIT_REACHED;
00187 } else {
00188
00189 return AYSTAR_STILL_BUSY;
00190 }
00191 }
00192
00193
00194
00195
00196 void AyStarMain_Free(AyStar *aystar)
00197 {
00198 aystar->OpenListQueue.free(&aystar->OpenListQueue, false);
00199
00200
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
00210
00211
00212 void AyStarMain_Clear(AyStar *aystar)
00213 {
00214
00215
00216 aystar->OpenListQueue.clear(&aystar->OpenListQueue, false);
00217
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
00228
00229
00230
00231
00232
00233
00234
00235
00236 int AyStarMain_Main(AyStar *aystar) {
00237 int r, i = 0;
00238
00239
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
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
00266
00267
00268
00269
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
00283 init_Hash(&aystar->OpenListHash, hash, num_buckets);
00284 init_Hash(&aystar->ClosedListHash, hash, num_buckets);
00285
00286
00287
00288
00289
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 }