void TDijkstraAlgorithm::Calculate()
{
OpenNodes->AddNode(StartNodeId, 0, 0);
while (!OpenNodes->IsEmpty()) {
OpenNodes->FindNextBestNode();
uint64_t nodeId = OpenNodes->GetBestNodeId();
double pathLengthFromStart = OpenNodes->GetBestNodePathLengthFromStart();
++SeenNodesCount;
if (nodeId == FinishNodeId) {
Path->PathWasFound(pathLengthFromStart);
if (NeedFullOutput) {
Path->ReconstructPath();
}
return;
}
ClosedNodes->AddNode(nodeId);
for (TNeighbor neighbor : Graph->GetNode(nodeId).Neighbors) {
if (ClosedNodes->Contains(neighbor.Id)) {
continue;
}
double pathLengthFromStartThroughNode =
pathLengthFromStart +
neighbor.Distance;
if (!OpenNodes->Contains(neighbor.Id)) {
OpenNodes->AddNode(neighbor.Id, pathLengthFromStartThroughNode, 0);
Path->SetNodeParent(neighbor.Id, nodeId);
} else {
if (pathLengthFromStartThroughNode < OpenNodes->GetPathLengthFromStart(neighbor.Id)) {
OpenNodes->AddNode(neighbor.Id, pathLengthFromStartThroughNode,
HeuristicCostEstimate(
Graph->GetNode(neighbor.Id),
Graph->GetNode(FinishNodeId)));
Path->SetNodeParent(neighbor.Id, nodeId);
}
}
}
}
}
int FindPath( const int nStartX, const int nStartY,
const int nTargetX, const int nTargetY,
const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
int* pOutBuffer, const int nOutBufferSize ) {
static_assert(sizeof(int)*2 == sizeof(long long), "");
std::set<Position> open_set, closed_set;
std::map<Position, Position> came_from;
std::map<Position, int> g_score;
std::map<Position, int> f_score;
//std::unordered_set<Position, position_hash> open_set, closed_set;
//std::unordered_map<Position, Position, position_hash> came_from;
//std::unordered_map<Position, int, position_hash> g_score;
//std::unordered_map<Position, int, position_hash> f_score;
Position start(nStartX, nStartY);
Position target(nTargetX, nTargetY);
open_set.insert(start);
g_score[start] = 0;
f_score[start] = HeuristicCostEstimate(start, target);
while (!open_set.empty()) {
Position current = *open_set.begin();
for (Position alt : open_set) {
if (f_score[alt] < f_score[current]) {
current = alt;
}
}
if (current == target) {
int i = g_score[current]+0;
int ret = i;
if (i >= nOutBufferSize) {
return -2;
}
while (i > 0) {
i -= 1;
pOutBuffer[i] = current.getIndex(nMapWidth);
current = came_from[current];
}
return ret;
}
open_set.erase(current);
closed_set.insert(current);
Position neighbors[4] = {
current.leftOf(),
current.rightOf(),
current.above(),
current.below()
};
for (int i = 0; i < 4; i += 1) {
Position neighbor = neighbors[i];
if (!WithinBounds(neighbor, nMapWidth, nMapHeight) || pMap[neighbor.getIndex(nMapWidth)] == 0 ) {
continue;
}
int new_g_score = g_score[current] + 1;
int new_f_score = new_g_score + HeuristicCostEstimate(neighbor, target);
if (closed_set.find(neighbor) != closed_set.end() && new_f_score >= f_score[neighbor]) {
continue;
}
if (open_set.find(neighbor) == open_set.end() || new_f_score < f_score[neighbor]) {
came_from[neighbor] = current;
g_score[neighbor] = new_g_score;
f_score[neighbor] = new_f_score;
if (open_set.find(neighbor) == open_set.end()) {
open_set.insert(neighbor);
}
}
}
}
return -1;
}