Когда вы добавляете узел в список узлов для рассмотрения, также добавьте флаг, если путь через этот узел уже прошел через стену.
possibleNode.heuristic = currentNode.distance + heuristicEstimate(possibleNode)
possibleNode.goneThroughWall = currentNode.goneThroughWall || possibleNode.isAWall
allPossiblePaths.insert(possibleNode) // Priority queue sorted on Node.heuristic
Тогда при рассмотрении возможных путей от узла, если он не прошел через стену, считайте соседние стены правильными путями.
foreach neighborNode in neighbors(someNode)
if !(someNode.goneThroughWall && neighborNode.isAWall)
addToPossiblePaths(neighborNode)
Вы уже должны поддерживать расстояние от начального узла до текущего обрабатываемого узла, и он использует то, что у вас уже есть.
Полное доказательство концепции:
Мы видим, что operator== определен так, чтобы также учитывать, достиг ли путь еще стены. Это позволяет нам рассматривать узел дважды, если это необходимо, когда мы смотрим в закрытом множестве, чтобы увидеть, не встретили ли мы уже этот узел. Это случай в центральном коридоре в примере лабиринта из источника ниже.
Части кода, помеченные #ifdef I_AM_ALLOWED_TO_GO_THROUGH_A_WALL, показывают участки, необходимые для расширения обычного алгоритма A *, чтобы можно было пройти через одну стену.
#include <cassert>
#include <algorithm>
#include <cstdlib>
#include <iostream>
#include <set>
#include <vector>
#define I_AM_ALLOWED_TO_GO_THROUGH_A_WALL
const int width = 5;
const int height = 5;
// Define maze
const int maze[height][width] =
{ { 0, 1, 1, 0, 0 },
{ 0, 1, 0, 1, 0 },
{ 0, 1, 0, 1, 0 },
{ 0, 1, 0, 1, 0 },
{ 0, 0, 0, 0, 0 } };
// Square represents the actual structure of the maze
// Here, we only care about where it is, and whether it is a wall
struct Square {
Square(int _x, int _y)
: x(_x), y(_y) {}
bool operator==(const Square& rhs) const {
return x == rhs.x && y == rhs.y;
}
bool isAWall() const {
return maze[y][x];
}
int distance(const Square& goal) const {
return std::abs(x - goal.x) + std::abs(y - goal.y);
}
int x;
int y;
};
// Define start and end of maze
const Square goalSquare(3, 0);
const Square startSquare(0, 0);
// A PathNode describes the A* algorithm's path through the maze
// It keeps track of its associated Square
// its previous PathNode in the path
// the length of the path up to the current PathNode
// whether the path so far has goneThroughWall
struct PathNode {
explicit PathNode(const Square& s, int length = 0, const PathNode* _prev = NULL)
: square(s),
prev(_prev),
pathLength(length) {
heuristic = pathLength + square.distance(goalSquare);
#ifdef I_AM_ALLOWED_TO_GO_THROUGH_A_WALL
if(prev)
goneThroughWall = prev->goneThroughWall || square.isAWall();
else
goneThroughWall = false;
// Sanity check, these should be the same
assert(goneThroughWall == hasGoneThroughAWall());
#endif
}
bool operator<(const PathNode& rhs) const {
return heuristic < rhs.heuristic;
}
// This is very important. When examining the closed set to see
// if we've already evaulated this node, we want to differentiate
// from whether we got to that node using a path through a wall.
//
// This is especially important in the given example maze.
// Without this differentiation, paths going up column 3 will hit
// old, closed paths going through the walls in column 2, and not
// find the best path.
bool operator==(const PathNode& rhs) const {
return square == rhs.square
#ifdef I_AM_ALLOWED_TO_GO_THROUGH_A_WALL
&& goneThroughWall == rhs.goneThroughWall
#endif
;
}
bool weakEquals(const PathNode& rhs) const {
return square == rhs.square;
}
bool weakEquals(const Square& rhs) const {
return square == rhs;
}
// Sanity checker
bool hasGoneThroughAWall() const {
if(square.isAWall()) return true;
const PathNode* p = prev;
while(p) {
if(p->square.isAWall())
return true;
p = p->prev;
}
return false;
}
Square square;
const PathNode* prev;
int heuristic, pathLength;
#ifdef I_AM_ALLOWED_TO_GO_THROUGH_A_WALL
bool goneThroughWall;
#endif
};
std::ostream& operator<<(std::ostream& ostr, const PathNode& p) {
ostr << "(" << p.square.x << ", " << p.square.y << ")";
if(p.square.isAWall())
ostr << " <- Wall!";
return ostr;
}
std::vector<Square> getNeighbors(const Square& s) {
std::vector<Square> neighbors;
if(s.x > 0)
neighbors.push_back(Square(s.x - 1, s.y));
if(s.x < width - 1)
neighbors.push_back(Square(s.x + 1, s.y));
if(s.y > 0)
neighbors.push_back(Square(s.x, s.y - 1));
if(s.y < height - 1)
neighbors.push_back(Square(s.x, s.y + 1));
return neighbors;
}
void printList(const PathNode* p, int i = 0) {
if(p) {
printList(p->prev, i + 1);
std::cout << *p << std::endl;
} else {
std::cout << "Length: " << i << std::endl;
}
}
typedef std::multiset<PathNode> Set;
int main(int argc, char** argv) {
// Print out maze
for(int j = 0; j < height; ++j) {
for(int i = 0; i < width; ++i) {
std::cout << " " << maze[j][i];
}
std::cout << std::endl;
}
std::cout << std::endl;
Set closedSet;
Set openSet;
openSet.insert(PathNode(startSquare)); // Start node (defined at line ~42)
int processedCount = 0;
while(!openSet.empty()) {
Set::iterator currentNode = openSet.begin();
++processedCount;
// We've found the goal, so print solution.
if(currentNode->weakEquals(goalSquare)) {
std::cout << "Processed nodes: " << processedCount << std::endl;
printList(&*currentNode);
return 0;
}
{
// Move from open set to closed set
Set::iterator del = currentNode;
currentNode = closedSet.insert(*currentNode);
openSet.erase(del);
}
std::vector<Square> neighbors = getNeighbors(currentNode->square);
for(unsigned int i = 0; i < neighbors.size(); ++i) {
PathNode currentNeighbor(neighbors[i], currentNode->pathLength + 1, &*currentNode);
// Skip if it is 2nd wall
if(
#ifdef I_AM_ALLOWED_TO_GO_THROUGH_A_WALL
currentNode->goneThroughWall &&
#endif
currentNeighbor.square.isAWall()
)
continue;
// Skip if it is already in the closed set
// Note: Using std::find here instead of std::multiset::find because
// std::multiset::find uses the definition !(a < b) && !(b < a) for
// searching. I want it to use my overloaded a == b.
if(find(closedSet.begin(), closedSet.end(), currentNeighbor) != closedSet.end())
continue;
// Skip if we were just there
const PathNode* p = currentNode->prev;
if(p && p->weakEquals(currentNeighbor))
continue;
// See if there is a better alternative in the open set
// Note: Using std::find here instead of std::multiset::find. See above.
Set::iterator contender = find(openSet.begin(), openSet.end(), currentNeighbor);
if(contender == openSet.end())
openSet.insert(currentNeighbor);
else if(currentNeighbor.pathLength < contender->pathLength) {
// We've found a better alternative, so replace
openSet.erase(contender);
openSet.insert(currentNeighbor);
}
}
}
std::cout << "Failure." << std::endl;
return 1;
}