Skip to content

Commit c20e225

Browse files
committed
Add seen check for Dijkstra and A*
1 parent 06540cb commit c20e225

File tree

2 files changed

+12
-0
lines changed

2 files changed

+12
-0
lines changed

src/a_star.cpp

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,9 @@ std::tuple<bool, std::vector<Node>> AStar::Plan(const Node& start,
2929
Node current = open_list.top();
3030
open_list.pop();
3131
current.id_ = current.x_ * n_ + current.y_;
32+
if (closed_list.find(current) != closed_list.end()) {
33+
continue;
34+
}
3235
if (CompareCoordinates(current, goal)) {
3336
closed_list.insert(current);
3437
grid_[current.x_][current.y_] = 2;
@@ -37,6 +40,9 @@ std::tuple<bool, std::vector<Node>> AStar::Plan(const Node& start,
3740
grid_[current.x_][current.y_] = 2; // Point opened
3841
for (const auto& m : motion) {
3942
Node new_point = current + m;
43+
if (closed_list.find(new_point) != closed_list.end()) {
44+
continue;
45+
}
4046
new_point.id_ = n_ * new_point.x_ + new_point.y_;
4147
new_point.pid_ = current.id_;
4248
new_point.h_cost_ =

src/dijkstra.cpp

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,9 @@ std::tuple<bool, std::vector<Node>> Dijkstra::Plan(const Node& start,
2929
Node current = open_list.top();
3030
open_list.pop();
3131
current.id_ = current.x_ * n_ + current.y_;
32+
if (closed_list.find(current) != closed_list.end()) {
33+
continue;
34+
}
3235
if (CompareCoordinates(current, goal)) {
3336
closed_list.insert(current);
3437
grid_[current.x_][current.y_] = 2;
@@ -37,6 +40,9 @@ std::tuple<bool, std::vector<Node>> Dijkstra::Plan(const Node& start,
3740
grid_[current.x_][current.y_] = 2; // Point opened
3841
for (const auto& m : motion) {
3942
Node new_point = current + m;
43+
if (closed_list.find(new_point) != closed_list.end()) {
44+
continue;
45+
}
4046
new_point.id_ = n_ * new_point.x_ + new_point.y_;
4147
new_point.pid_ = current.id_;
4248
if (CompareCoordinates(new_point, goal)) {

0 commit comments

Comments
 (0)