3 * AyStar is a fast pathfinding routine and is used for things like |
3 * AyStar is a fast pathfinding routine and is used for things like |
4 * AI_pathfinding and Train_pathfinding. |
4 * AI_pathfinding and Train_pathfinding. |
5 * For more information about AyStar (A* Algorithm), you can look at |
5 * For more information about AyStar (A* Algorithm), you can look at |
6 * http://en.wikipedia.org/wiki/A-star_search_algorithm |
6 * http://en.wikipedia.org/wiki/A-star_search_algorithm |
7 */ |
7 */ |
8 |
8 |
9 /* |
9 /* |
10 * Friendly reminder: |
10 * Friendly reminder: |
11 * Call (AyStar).free() when you are done with Aystar. It reserves a lot of memory |
11 * Call (AyStar).free() when you are done with Aystar. It reserves a lot of memory |
12 * And when not free'd, it can cause system-crashes. |
12 * And when not free'd, it can cause system-crashes. |
13 * Also remember that when you stop an algorithm before it is finished, your |
13 * Also remember that when you stop an algorithm before it is finished, your |
44 OpenListNode *AyStarMain_OpenList_Pop(AyStar *aystar) { |
44 OpenListNode *AyStarMain_OpenList_Pop(AyStar *aystar) { |
45 // Return the item the Queue returns.. the best next OpenList item. |
45 // Return the item the Queue returns.. the best next OpenList item. |
46 OpenListNode* res = (OpenListNode*)aystar->OpenListQueue.pop(&aystar->OpenListQueue); |
46 OpenListNode* res = (OpenListNode*)aystar->OpenListQueue.pop(&aystar->OpenListQueue); |
47 if (res != NULL) |
47 if (res != NULL) |
48 Hash_Delete(&aystar->OpenListHash, res->path.node.tile, res->path.node.direction); |
48 Hash_Delete(&aystar->OpenListHash, res->path.node.tile, res->path.node.direction); |
49 |
49 |
50 return res; |
50 return res; |
51 } |
51 } |
52 |
52 |
53 // Adds a node to the OpenList |
53 // Adds a node to the OpenList |
54 // It makes a copy of node, and puts the pointer of parent in the struct |
54 // It makes a copy of node, and puts the pointer of parent in the struct |
74 PathNode *closedlist_parent; |
74 PathNode *closedlist_parent; |
75 OpenListNode *check; |
75 OpenListNode *check; |
76 |
76 |
77 // Check the new node against the ClosedList |
77 // Check the new node against the ClosedList |
78 if (AyStarMain_ClosedList_IsInList(aystar, current) != NULL) return AYSTAR_DONE; |
78 if (AyStarMain_ClosedList_IsInList(aystar, current) != NULL) return AYSTAR_DONE; |
79 |
79 |
80 // Calculate the G-value for this node |
80 // Calculate the G-value for this node |
81 new_g = aystar->CalculateG(aystar, current, parent); |
81 new_g = aystar->CalculateG(aystar, current, parent); |
82 // If the value was INVALID_NODE, we don't do anything with this node |
82 // If the value was INVALID_NODE, we don't do anything with this node |
83 if (new_g == AYSTAR_INVALID_NODE) return AYSTAR_DONE; |
83 if (new_g == AYSTAR_INVALID_NODE) return AYSTAR_DONE; |
84 |
84 |
85 // There should not be given any other error-code.. |
85 // There should not be given any other error-code.. |
86 assert(new_g >= 0); |
86 assert(new_g >= 0); |
87 // Add the parent g-value to the new g-value |
87 // Add the parent g-value to the new g-value |
88 new_g += parent->g; |
88 new_g += parent->g; |
89 if (aystar->max_path_cost != 0 && (uint)new_g > aystar->max_path_cost) return AYSTAR_DONE; |
89 if (aystar->max_path_cost != 0 && (uint)new_g > aystar->max_path_cost) return AYSTAR_DONE; |
90 |
90 |
91 // Calculate the h-value |
91 // Calculate the h-value |
92 new_h = aystar->CalculateH(aystar, current, parent); |
92 new_h = aystar->CalculateH(aystar, current, parent); |
93 // There should not be given any error-code.. |
93 // There should not be given any error-code.. |
94 assert(new_h >= 0); |
94 assert(new_h >= 0); |
95 |
95 |
96 // The f-value if g + h |
96 // The f-value if g + h |
97 new_f = new_g + new_h; |
97 new_f = new_g + new_h; |
98 |
98 |
99 // Get the pointer to the parent in the ClosedList (the currentone is to a copy of the one in the OpenList) |
99 // Get the pointer to the parent in the ClosedList (the currentone is to a copy of the one in the OpenList) |
100 closedlist_parent = AyStarMain_ClosedList_IsInList(aystar, &parent->path.node); |
100 closedlist_parent = AyStarMain_ClosedList_IsInList(aystar, &parent->path.node); |
101 |
101 |
102 // Check if this item is already in the OpenList |
102 // Check if this item is already in the OpenList |
103 if ((check = AyStarMain_OpenList_IsInList(aystar, current)) != NULL) { |
103 if ((check = AyStarMain_OpenList_IsInList(aystar, current)) != NULL) { |
104 int i; |
104 int i; |
105 // Yes, check if this g value is lower.. |
105 // Yes, check if this g value is lower.. |
106 if (new_g > check->g) return AYSTAR_DONE; |
106 if (new_g > check->g) return AYSTAR_DONE; |
115 aystar->OpenListQueue.push(&aystar->OpenListQueue, check, new_f); |
115 aystar->OpenListQueue.push(&aystar->OpenListQueue, check, new_f); |
116 } else { |
116 } else { |
117 // A new node, add him to the OpenList |
117 // A new node, add him to the OpenList |
118 AyStarMain_OpenList_Add(aystar, closedlist_parent, current, new_f, new_g, 0); |
118 AyStarMain_OpenList_Add(aystar, closedlist_parent, current, new_f, new_g, 0); |
119 } |
119 } |
120 |
120 |
121 return AYSTAR_DONE; |
121 return AYSTAR_DONE; |
122 } |
122 } |
123 |
123 |
124 /* |
124 /* |
125 * This function is the core of AyStar. It handles one item and checks |
125 * This function is the core of AyStar. It handles one item and checks |
132 * AYSTAR_FOUND_END_NODE : indicates we found the end. Path_found now is true, and in path is the path found. |
132 * AYSTAR_FOUND_END_NODE : indicates we found the end. Path_found now is true, and in path is the path found. |
133 * AYSTAR_STILL_BUSY : indicates we have done this tile, did not found the path yet, and have items left to try. |
133 * AYSTAR_STILL_BUSY : indicates we have done this tile, did not found the path yet, and have items left to try. |
134 */ |
134 */ |
135 int AyStarMain_Loop(AyStar *aystar) { |
135 int AyStarMain_Loop(AyStar *aystar) { |
136 int i, r; |
136 int i, r; |
137 |
137 |
138 // Get the best node from OpenList |
138 // Get the best node from OpenList |
139 OpenListNode *current = AyStarMain_OpenList_Pop(aystar); |
139 OpenListNode *current = AyStarMain_OpenList_Pop(aystar); |
140 // If empty, drop an error |
140 // If empty, drop an error |
141 if (current == NULL) return AYSTAR_EMPTY_OPENLIST; |
141 if (current == NULL) return AYSTAR_EMPTY_OPENLIST; |
142 |
142 |
143 // Check for end node and if found, return that code |
143 // Check for end node and if found, return that code |
144 if (aystar->EndNodeCheck(aystar, current) == AYSTAR_FOUND_END_NODE) { |
144 if (aystar->EndNodeCheck(aystar, current) == AYSTAR_FOUND_END_NODE) { |
145 if (aystar->FoundEndNode != NULL) |
145 if (aystar->FoundEndNode != NULL) |
146 aystar->FoundEndNode(aystar, current); |
146 aystar->FoundEndNode(aystar, current); |
147 free(current); |
147 free(current); |
148 return AYSTAR_FOUND_END_NODE; |
148 return AYSTAR_FOUND_END_NODE; |
149 } |
149 } |
150 |
150 |
151 // Add the node to the ClosedList |
151 // Add the node to the ClosedList |
152 AyStarMain_ClosedList_Add(aystar, ¤t->path); |
152 AyStarMain_ClosedList_Add(aystar, ¤t->path); |
153 |
153 |
154 // Load the neighbours |
154 // Load the neighbours |
155 aystar->GetNeighbours(aystar, current); |
155 aystar->GetNeighbours(aystar, current); |
156 |
156 |
157 // Go through all neighbours |
157 // Go through all neighbours |
158 for (i=0;i<aystar->num_neighbours;i++) { |
158 for (i=0;i<aystar->num_neighbours;i++) { |
159 // Check and add them to the OpenList if needed |
159 // Check and add them to the OpenList if needed |
160 r = aystar->checktile(aystar, &aystar->neighbours[i], current); |
160 r = aystar->checktile(aystar, &aystar->neighbours[i], current); |
161 } |
161 } |
162 |
162 |
163 // Free the node |
163 // Free the node |
164 free(current); |
164 free(current); |
165 |
165 |
166 if (aystar->max_search_nodes != 0 && Hash_Size(&aystar->ClosedListHash) >= aystar->max_search_nodes) |
166 if (aystar->max_search_nodes != 0 && Hash_Size(&aystar->ClosedListHash) >= aystar->max_search_nodes) |
167 /* We've expanded enough nodes */ |
167 /* We've expanded enough nodes */ |
168 return AYSTAR_LIMIT_REACHED; |
168 return AYSTAR_LIMIT_REACHED; |
169 else |
169 else |
170 // Return that we are still busy |
170 // Return that we are still busy |
226 printf("[AyStar] Exceeded search_nodes, no path found\n"); |
226 printf("[AyStar] Exceeded search_nodes, no path found\n"); |
227 #endif |
227 #endif |
228 if (r != AYSTAR_STILL_BUSY) |
228 if (r != AYSTAR_STILL_BUSY) |
229 /* We're done, clean up */ |
229 /* We're done, clean up */ |
230 aystar->clear(aystar); |
230 aystar->clear(aystar); |
231 |
231 |
232 // Check result-value |
232 // Check result-value |
233 if (r == AYSTAR_FOUND_END_NODE) return AYSTAR_FOUND_END_NODE; |
233 if (r == AYSTAR_FOUND_END_NODE) return AYSTAR_FOUND_END_NODE; |
234 // Check if we have some left in the OpenList |
234 // Check if we have some left in the OpenList |
235 if (r == AYSTAR_EMPTY_OPENLIST || r == AYSTAR_LIMIT_REACHED) return AYSTAR_NO_PATH; |
235 if (r == AYSTAR_EMPTY_OPENLIST || r == AYSTAR_LIMIT_REACHED) return AYSTAR_NO_PATH; |
236 |
236 |
240 |
240 |
241 /* |
241 /* |
242 * Adds a node from where to start an algorithm. Multiple nodes can be added |
242 * Adds a node from where to start an algorithm. Multiple nodes can be added |
243 * if wanted. You should make sure that clear() is called before adding nodes |
243 * if wanted. You should make sure that clear() is called before adding nodes |
244 * if the AyStar has been used before (though the normal main loop calls |
244 * if the AyStar has been used before (though the normal main loop calls |
245 * clear() automatically when the algorithm finishes |
245 * clear() automatically when the algorithm finishes |
246 */ |
246 */ |
247 void AyStarMain_AddStartNode(AyStar *aystar, AyStarNode *start_node) { |
247 void AyStarMain_AddStartNode(AyStar *aystar, AyStarNode *start_node) { |
248 #ifdef AYSTAR_DEBUG |
248 #ifdef AYSTAR_DEBUG |
249 printf("[AyStar] Starting A* Algorithm from node (%d, %d, %d)\n", GET_TILE_X(start_node->tile), GET_TILE_Y(start_node->tile), start_node->direction); |
249 printf("[AyStar] Starting A* Algorithm from node (%d, %d, %d)\n", GET_TILE_X(start_node->tile), GET_TILE_Y(start_node->tile), start_node->direction); |
250 #endif |
250 #endif |