// CLASS FUNCTIONS
std::vector<MapNode*> JPS::get_path
(
Map* map,
MapNode* start,
MapNode* end
)
{
start->set_data<AStarNodeData>(new AStarNodeData(0, heuristic(start,end)));
m_open_set.insert(start);
m_open_queue.push(start);
AStarNodeData* s_data = start->get_data<AStarNodeData>();
std::unordered_map<MapNode*,MapNode*> came_from;
// The return path
std::vector<MapNode*> path;
while (m_open_queue.size() > 0)
{
// Get the first item in the min-heap
MapNode* current = m_open_queue.top();
m_open_set.erase(current);
m_open_queue.pop();
AStarNodeData* c_data = current->get_data<AStarNodeData>();
// Short-circuit for the end
if (!current->is_seen() && c_data->f_score() < s_data->f_score())
{
int dist = path_length(current, end);
if (AStarNodeData* data = end->get_data<AStarNodeData>())
{
data->g_score = data->g_score + dist;
data->h_score = 0;
}
else
{
end->set_data<AStarNodeData>(
new AStarNodeData(c_data->g_score + dist, 0)
);
}
path = reconstruct_path(came_from, current);
break;
}
if (current == end)
{
path = reconstruct_path(came_from, end);
break;
}
m_closed_set.insert(current);
for (MapNode* neighbor : get_successors(map, current, end))
{
bool is_in_open_set = m_open_set.find(neighbor) != m_open_set.end();
bool is_in_closed_set = m_closed_set.find(neighbor) != m_closed_set.end();
// If it's in the closed set skip
if (is_in_closed_set)
{
continue; // Ignore the neighbor which is already evaluated.
}
AStarNodeData* n_data = neighbor->get_data<AStarNodeData>();
// The distance from start to goal passing through current and the neighbor.
int tentative_g_score = c_data->g_score + path_length(current,neighbor);
if (is_in_open_set && tentative_g_score >= n_data->g_score)
{
continue; // This is not a better path.
}
// This path is the best until now. Record it!
came_from[neighbor] = current;
if (n_data)
{
n_data->g_score = tentative_g_score;
n_data->h_score = heuristic(neighbor, end);
}
else
{
neighbor->set_data<AStarNodeData>(
new AStarNodeData(
tentative_g_score,
heuristic(neighbor, end)
)
);
}
if (!is_in_open_set)
{
m_open_set.insert(neighbor);
m_open_queue.push(neighbor);
}
}
}
return path;
}
