// find shortest path from one marker to another
static BOOL FindPath(CNavigationMarker *pnmSrc, CNavigationMarker *pnmDst)
{
ASSERT(pnmSrc!=pnmDst);
CPathNode *ppnSrc = pnmSrc->GetPathNode();
CPathNode *ppnDst = pnmDst->GetPathNode();
PRINTOUT(CPrintF("--------------------\n"));
PRINTOUT(CPrintF("FindPath(%s, %s)\n", ppnSrc->GetName(), ppnDst->GetName()));
// start with empty open and closed lists
ASSERT(_lhOpen.IsEmpty());
ASSERT(_lhClosed.IsEmpty());
// add the start node to open list
ppnSrc->pn_fG = 0.0f;
ppnSrc->pn_fH = NodeDistance(ppnSrc, ppnDst);
ppnSrc->pn_fF = ppnSrc->pn_fG +ppnSrc->pn_fH;
_lhOpen.AddTail(ppnSrc->pn_lnInOpen);
PRINTOUT(CPrintF("StartState: %s\n", ppnSrc->GetName()));
// while the open list is not empty
while (!_lhOpen.IsEmpty()) {
// get the first node from open list (that is, the one with lowest F)
CPathNode *ppnNode = LIST_HEAD(_lhOpen, CPathNode, pn_lnInOpen);
ppnNode->pn_lnInOpen.Remove();
_lhClosed.AddTail(ppnNode->pn_lnInClosed);
PRINTOUT(CPrintF("Node: %s - moved from OPEN to CLOSED\n", ppnNode->GetName()));
// if this is the goal
if (ppnNode==ppnDst) {
PRINTOUT(CPrintF("PATH FOUND!\n"));
// the path is found
return TRUE;
}
// for each link of current node
CPathNode *ppnLink = NULL;
for(INDEX i=0; (ppnLink=ppnNode->GetLink(i))!=NULL; i++) {
PRINTOUT(CPrintF(" Link %d: %s\n", i, ppnLink->GetName()));
// get cost to get to this node if coming from current node
FLOAT fNewG = ppnLink->pn_fG+NodeDistance(ppnNode, ppnLink);
// if a shorter path already exists
if ((ppnLink->pn_lnInOpen.IsLinked() || ppnLink->pn_lnInClosed.IsLinked()) && fNewG>=ppnLink->pn_fG) {
PRINTOUT(CPrintF(" shorter path exists through: %s\n", ppnLink->pn_ppnParent->GetName()));
// skip this link
continue;
}
// remember this path
ppnLink->pn_ppnParent = ppnNode;
ppnLink->pn_fG = fNewG;
ppnLink->pn_fH = NodeDistance(ppnLink, ppnDst);
ppnLink->pn_fF = ppnLink->pn_fG + ppnLink->pn_fH;
// remove from closed list, if in it
if (ppnLink->pn_lnInClosed.IsLinked()) {
ppnLink->pn_lnInClosed.Remove();
PRINTOUT(CPrintF(" %s removed from CLOSED\n", ppnLink->GetName()));
}
// add to open if not in it
if (!ppnLink->pn_lnInOpen.IsLinked()) {
SortIntoOpenList(ppnLink);
PRINTOUT(CPrintF(" %s added to OPEN\n", ppnLink->GetName()));
}
}
}
// if we get here, there is no path
PRINTOUT(CPrintF("PATH NOT FOUND!\n"));
return FALSE;
}
bool CriticalGraph::ComputeCriticalPath(
const CriticalNode* from,
const CriticalNode* to,
const std::unordered_set<uint32_t>& tids,
CriticalPath* path) const
{
assert(from != nullptr);
assert(to != nullptr);
assert(path != nullptr);
// Topological sort.
std::vector<const CriticalNode*> topological;
if (!TopologicalSort(from, to, &topological)) {
tberror() << "Destination node wasn't found in topological sort." << tbendl();
return false;
}
// Compute maximum distance from destination for each node.
std::unordered_map<const CriticalNode*, NodeDistance> distances;
for (auto it = topological.rbegin(); it != topological.rend(); ++it)
{
// If the node is the destination, the distance is zero.
if (*it == to)
{
distances[*it] = NodeDistance(0, kInvalidCriticalEdgeId);
continue;
}
// Compute the 2 possible distances.
size_t horizontal_distance = kHugeDistance;
CriticalEdgeId horizontal_edge_id = (*it)->edge(kCriticalEdgeOutHorizontal);
if (horizontal_edge_id != kInvalidCriticalEdgeId)
{
const auto& edge = GetEdge(horizontal_edge_id);
auto distance_look = distances.find(edge.to());
if (distance_look != distances.end()) {
horizontal_distance = distance_look->second.distance;
if (horizontal_distance != kHugeDistance)
{
if (tids.find((*it)->tid()) == tids.end())
{
// Reduce the cost of edges that are on other threads.
horizontal_distance = std::min(horizontal_distance, static_cast<size_t>(1));
}
horizontal_distance += edge.Cost();
}
}
}
size_t vertical_distance = kHugeDistance;
CriticalEdgeId vertical_edge_id = (*it)->edge(kCriticalEdgeOutVertical);
if (vertical_edge_id != kInvalidCriticalEdgeId)
{
const auto& edge = GetEdge(vertical_edge_id);
auto distance_look = distances.find(edge.to());
if (distance_look != distances.end()) {
vertical_distance = distance_look->second.distance;
if (vertical_distance != kHugeDistance)
vertical_distance += edge.Cost();
}
}
// Keep the maximum distance which is not invalid.
if (horizontal_distance != kHugeDistance &&
(vertical_distance == kHugeDistance || horizontal_distance >= vertical_distance))
{
distances[*it] = NodeDistance(horizontal_distance, horizontal_edge_id);
}
else if (vertical_distance != kHugeDistance &&
(horizontal_distance == kHugeDistance || vertical_distance >= horizontal_distance))
{
distances[*it] = NodeDistance(vertical_distance, vertical_edge_id);
}
}
// Retrieve the critical path.
const CriticalNode* cur = from;
while (cur != to)
{
const NodeDistance& distance = distances[cur];
const CriticalEdge& edge = GetEdge(distance.edge);
if (edge.type() != CriticalEdgeType::kVertical)
{
path->Push(CriticalPathSegment(
edge.from()->ts(), edge.from()->tid(), edge.type()));
}
cur = GetEdge(distance.edge).to();
}
// Restrict the critical path to the autorized threads.
path->RestrictToThreads(tids);
return true;
}
