140 {
142 std::shared_ptr<Node> current = nullptr;
144 int searchStep = 0;
145
146 openSet.reserve(20);
147 closedSet.reserve(20);
148 openSet.push_back(std::make_shared<Node>(source_));
149
151 return path;
152
154 auto current_it = openSet.begin();
155
156 current = *current_it;
157
158 for (auto it = openSet.begin(); it != openSet.end(); it++) {
159 auto node = *it;
160 if (node->getFScore() <= current->getFScore()) {
161 current = node;
162 current_it = it;
163 }
164 }
165
166 if (current->coordinates == target_)
167 break;
168
169 closedSet.push_back(current);
170 openSet.erase(current_it);
171
174
177 continue;
178 }
179
180 newCoordinates.set_move_cost((i < 4) ? 10 : 14);
181 uint32_t totalCost = current->G + newCoordinates.move_cost();
182
183 std::shared_ptr<Node> successor =
findNodeOnList(openSet, newCoordinates);
184 if (successor == nullptr) {
185 successor = std::make_shared<Node>(newCoordinates, current);
186 successor->G = totalCost;
187 successor->H =
heuristic(successor->coordinates, target_);
188 openSet.push_back(successor);
189 } else if (totalCost < successor->G) {
190 successor->parent = current;
191 successor->G = totalCost;
192 }
193 }
194
195 searchStep++;
196 }
197
198 while (current != nullptr) {
199 path.push_back(current->coordinates);
200 current = current->parent;
201 }
202
205
206 return path;
207 }
#define MAX_VIEW_RANGE
Definition: Horizon.hpp:59
int16_t y() const
Definition: Coordinates.hpp:120
int16_t x() const
Definition: Coordinates.hpp:119
CoordinateList direction
Definition: AStar.hpp:212
static void releaseNodes(NodeSet nodes_)
Definition: AStar.hpp:109
CollisionDetectionFunction check_collision
Definition: AStar.hpp:211
uint32_t directions
Definition: AStar.hpp:217
static std::shared_ptr< Node > findNodeOnList(NodeSet &nodes_, MapCoords coordinates_)
Definition: AStar.hpp:100
HeuristicFunction heuristic
Definition: AStar.hpp:210
std::vector< std::shared_ptr< Node > > NodeSet
Definition: AStar.hpp:96
std::vector< MapCoords > CoordinateList
We use a vector because the AStar algorithm is only searching on small datasets. Other data structure...
Definition: AStar.hpp:77