std::tuple<std::vector<Otc::Direction>, Otc::PathFindResult> Map::findPath(const Position& startPos, const Position& goalPos, int maxComplexity, int flags)
{
// pathfinding using A* search algorithm
// as described in http://en.wikipedia.org/wiki/A*_search_algorithm
struct Node {
Node(const Position& pos) : cost(0), totalCost(0), pos(pos), prev(nullptr), dir(Otc::InvalidDirection) { }
float cost;
float totalCost;
Position pos;
Node *prev;
Otc::Direction dir;
};
struct LessNode : std::binary_function<std::pair<Node*, float>, std::pair<Node*, float>, bool> {
bool operator()(std::pair<Node*, float> a, std::pair<Node*, float> b) const {
return b.second < a.second;
}
};
std::tuple<std::vector<Otc::Direction>, Otc::PathFindResult> ret;
std::vector<Otc::Direction>& dirs = std::get<0>(ret);
Otc::PathFindResult& result = std::get<1>(ret);
result = Otc::PathFindResultNoWay;
if(startPos == goalPos) {
result = Otc::PathFindResultSamePosition;
return ret;
}
if(startPos.z != goalPos.z) {
result = Otc::PathFindResultImpossible;
return ret;
}
// check the goal pos is walkable
if(g_map.isAwareOfPosition(goalPos)) {
const TilePtr goalTile = getTile(goalPos);
if(!goalTile || !goalTile->isWalkable()) {
return ret;
}
}
else {
const MinimapTile& goalTile = g_minimap.getTile(goalPos);
if(goalTile.hasFlag(MinimapTileNotWalkable)) {
return ret;
}
}
std::unordered_map<Position, Node*, PositionHasher> nodes;
std::priority_queue<std::pair<Node*, float>, std::vector<std::pair<Node*, float>>, LessNode> searchList;
Node *currentNode = new Node(startPos);
currentNode->pos = startPos;
nodes[startPos] = currentNode;
Node *foundNode = nullptr;
while(currentNode) {
if((int)nodes.size() > maxComplexity) {
result = Otc::PathFindResultTooFar;
break;
}
// path found
if(currentNode->pos == goalPos && (!foundNode || currentNode->cost < foundNode->cost))
foundNode = currentNode;
// cost too high
if(foundNode && currentNode->totalCost >= foundNode->cost)
break;
for(int i=-1;i<=1;++i) {
for(int j=-1;j<=1;++j) {
if(i == 0 && j == 0)
continue;
bool wasSeen = false;
bool hasCreature = false;
bool isNotWalkable = true;
bool isNotPathable = true;
int speed = 100;
Position neighborPos = currentNode->pos.translated(i, j);
if(g_map.isAwareOfPosition(neighborPos)) {
wasSeen = true;
if(const TilePtr& tile = getTile(neighborPos)) {
hasCreature = tile->hasCreature();
isNotWalkable = !tile->isWalkable();
isNotPathable = !tile->isPathable();
speed = tile->getGroundSpeed();
}
} else {
const MinimapTile& mtile = g_minimap.getTile(neighborPos);
wasSeen = mtile.hasFlag(MinimapTileWasSeen);
isNotWalkable = mtile.hasFlag(MinimapTileNotWalkable);
isNotPathable = mtile.hasFlag(MinimapTileNotPathable);
if(isNotWalkable || isNotPathable)
wasSeen = true;
speed = mtile.getSpeed();
}
float walkFactor = 0;
if(neighborPos != goalPos) {
if(!(flags & Otc::PathFindAllowNotSeenTiles) && !wasSeen)
continue;
if(wasSeen) {
if(!(flags & Otc::PathFindAllowCreatures) && hasCreature)
continue;
if(!(flags & Otc::PathFindAllowNonPathable) && isNotPathable)
continue;
if(!(flags & Otc::PathFindAllowNonWalkable) && isNotWalkable)
continue;
}
} else {
if(!(flags & Otc::PathFindAllowNotSeenTiles) && !wasSeen)
continue;
if(wasSeen) {
if(!(flags & Otc::PathFindAllowNonWalkable) && isNotWalkable)
continue;
}
}
Otc::Direction walkDir = currentNode->pos.getDirectionFromPosition(neighborPos);
if(walkDir >= Otc::NorthEast)
walkFactor += 3.0f;
else
walkFactor += 1.0f;
float cost = currentNode->cost + (speed * walkFactor) / 100.0f;
Node *neighborNode;
if(nodes.find(neighborPos) == nodes.end()) {
neighborNode = new Node(neighborPos);
nodes[neighborPos] = neighborNode;
} else {
neighborNode = nodes[neighborPos];
if(neighborNode->cost <= cost)
continue;
}
neighborNode->prev = currentNode;
neighborNode->cost = cost;
neighborNode->totalCost = neighborNode->cost + neighborPos.distance(goalPos);
neighborNode->dir = walkDir;
searchList.push(std::make_pair(neighborNode, neighborNode->totalCost));
}
}
if(!searchList.empty()) {
currentNode = searchList.top().first;
searchList.pop();
} else
currentNode = nullptr;
}
if(foundNode) {
currentNode = foundNode;
while(currentNode) {
dirs.push_back(currentNode->dir);
currentNode = currentNode->prev;
}
dirs.pop_back();
std::reverse(dirs.begin(), dirs.end());
result = Otc::PathFindResultOk;
}
for(auto it : nodes)
delete it.second;
return ret;
}