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