I am trying to implement Join State A* for multiple agents.
It returns the shortest path possible but it do not removes the conflicts any help to correct this code will be highly appreciated.
To avoid conflict I am using the following algorithm:
AStar::CoordinateList AStar::Generator::findPath(const std::vector<Vec2i>& source_, const std::vector<Vec2i>& target_)
{
struct JointStateNode
{
uint G, H;
std::vector<Vec2i> coordinates;
JointStateNode* parent;
JointStateNode(const std::vector<Vec2i>& coord_, JointStateNode* parent_ = nullptr)
: coordinates(coord_), parent(parent_), G(0), H(0) {}
uint getScore() const {
return G + H;
}
};
using JointStateNodeSet = std::vector<JointStateNode*>;
auto findNodeOnList = [](JointStateNodeSet& nodes_, const std::vector<Vec2i>& coordinates_) -> JointStateNode* {
for (auto node : nodes_) {
if (node->coordinates == coordinates_) {
return node;
}
}
return nullptr;
};
auto releaseNodes = [](JointStateNodeSet& nodes_) {
for (auto it = nodes_.begin(); it != nodes_.end();) {
delete *it;
it = nodes_.erase(it);
}
};
JointStateNode* current = nullptr;
JointStateNodeSet openSet, closedSet;
openSet.reserve(100);
closedSet.reserve(100);
openSet.push_back(new JointStateNode(source_));
while (!openSet.empty()) {
auto current_it = openSet.begin();
current = *current_it;
for (auto it = openSet.begin(); it != openSet.end(); it++) {
auto node = *it;
if (node->getScore() <= current->getScore()) {
current = node;
current_it = it;
}
}
if (current->coordinates == target_) {
break;
}
closedSet.push_back(current);
openSet.erase(current_it);
for (uint i = 0; i < directions; ++i) {
std::vector<Vec2i> newCoordinates;
for (const auto& coord : current->coordinates) {
newCoordinates.push_back(coord + direction[i]);
}
if (detectVertexCollision(newCoordinates) || detectEdgeCollision(current->coordinates, newCoordinates) ||
std::any_of(newCoordinates.begin(), newCoordinates.end(),
[&](const Vec2i& coord) { return detectCollision(coord); }) ||
findNodeOnList(closedSet, newCoordinates)) {
std::cout<<"Collision detected can't set this vertex {"<<newCoordinates[0].x<<", "<<newCoordinates[0].y<<"}n";
continue;
}
uint totalCost = current->G + 1;
JointStateNode* successor = findNodeOnList(openSet, newCoordinates);
if (successor == nullptr) {
successor = new JointStateNode(newCoordinates, current);
successor->G = totalCost;
successor->H = heuristic(successor->coordinates, target_);
openSet.push_back(successor);
} else if (totalCost < successor->G) {
successor->parent = current;
successor->G = totalCost;
}
}
}
CoordinateList path;
while (current != nullptr) {
path.push_back(current->coordinates);
current = current->parent;
}
releaseNodes(openSet);
releaseNodes(closedSet);
std::reverse(path.begin(), path.end());
return path;
}
After testing on 5×5 grid for three agents with starting points and target points:
std::vector<AStar::Vec2i> start1 = {{4, 0}};
std::vector<AStar::Vec2i> target1 = {{2, 2}};
std::vector<AStar::Vec2i> start2 = {{4, 1}};
std::vector<AStar::Vec2i> target2 = {{0, 0}};
std::vector<AStar::Vec2i> start3 = {{2, 3}};
std::vector<AStar::Vec2i> target3 = {{1, 1}};
I get the following output:
Path 1: (4, 0) (3, 0) (2, 0) (2, 1) (2, 2)
Path 2: (4, 1) (3, 1) (2, 1) (1, 1) (0, 1) (0, 0)
Path 3: (2, 3) (1, 3) (1, 2) (1, 1)
As it can be seen that (1,1) in path 2 and (1,1) in path are at the same position which is indeed a conflict. I want it to remove the conflict according functions logic and choose some different path.