bool TakeoffQueueRouteFinder::FindRoute( IntersectionNodeInSim * pNodeFrom, IntersectionNodeInSim* pNodeTo, TaxiRouteInSim& route )
{
for(int i=1; i< (int)m_vNodeList.size();i++)
{
IntersectionNodeInSim * pShorestNode = NULL;
DistanceUnit dLessWeight = ARCMath::DISTANCE_INFINITE;
for(IntersectionNodesSet::iterator itr = m_vNodeList.begin();itr!= m_vNodeList.end();itr++)
{
IntersectionNodeInSim *pOtherNode = *itr;
if(pOtherNode == pNodeFrom) continue;
NodePair nodepair(pNodeFrom,pOtherNode);
if( !m_dijRouteMap[nodepair].m_bIsDoneSearch ) // not done search
{
DistanceUnit dWeight = m_dijRouteMap[nodepair].GetWeight();
if( dWeight < dLessWeight )
{
pShorestNode = pOtherNode;
dLessWeight = dWeight;
}
}
}
if(pShorestNode)
{
NodePair nodepair(pNodeFrom,pShorestNode);
m_dijRouteMap[nodepair].m_bIsDoneSearch = true;
for(IntersectionNodesSet::iterator itr = m_vNodeList.begin();itr!= m_vNodeList.end();itr++)
{
IntersectionNodeInSim * pNodeI = *itr;
dijRouteInQueue& routeSrcI = m_dijRouteMap[NodePair(pNodeFrom,pNodeI)];
dijRouteInQueue& routeShortI = m_dijRouteMap[NodePair(pShorestNode,pNodeI)];
dijRouteInQueue& routeSrcShort = m_dijRouteMap[NodePair(pNodeFrom,pShorestNode)];
if(!routeSrcI.m_bIsDoneSearch)
{
if(dLessWeight + routeShortI.GetWeight() < routeSrcI.GetWeight())
{
FlightGroundRouteDirectSegList segList;
segList.insert(segList.end(),routeSrcShort.m_vRoutes.begin(),routeSrcShort.m_vRoutes.end() );
segList.insert(segList.end(),routeShortI.m_vRoutes.begin(), routeShortI.m_vRoutes.end() );
routeSrcI.m_vRoutes = segList;
}
}
}
}
if(pShorestNode == pNodeTo)
break;
}
route.AddTaxiwaySegList(m_dijRouteMap[NodePair(pNodeFrom,pNodeTo)].m_vRoutes);
return route.GetItemCount() > 0;
}
NodePath* Dijkstra::get_path(Node* start, Node* dest) {
/**
* After running the find_paths function, you can here extract the exact way from start to destination.
*/
if (saved_paths.find(NodePair(start, dest)) != saved_paths.end()) {
return create_path_copy(saved_paths[NodePair(start, dest)]);
}
// otherwise calculate a new path
return calculate_path(start, dest);
}
void Caller::update_call_graph() {
// if we're leaving uncalled nodes, add'em:
if (_leave_uncalled) {
function<void(Node*)> add_node = [&](Node* node) {
if (_visited_nodes.find(node->id()) == _visited_nodes.end()) {
_call_graph.create_node(node->sequence(), node->id());
_start_node_map[node->id()] = NodePair(node->id(), -1);
_end_node_map[node->id()] = NodePair(node->id(), -1);
}
};
_graph->for_each_node(add_node);
}
// map every edge in the original graph to equivalent sides
// in the call graph. if both sides exist, make an edge in the call graph
function<void(Edge*)> map_edge = [&](Edge* edge) {
const NodeMap& from_map = edge->from_start() ? _start_node_map : _end_node_map;
auto mappedNode1 = from_map.find(edge->from());
if (mappedNode1 != from_map.end()) {
const NodeMap& to_map = edge->to_end() ? _end_node_map : _start_node_map;
auto mappedNode2 = to_map.find(edge->to());
if (mappedNode2 != to_map.end()) {
// up to 2 mappings for each node, so 4 possible edges:
if (mappedNode1->second.first != -1 && mappedNode2->second.first != -1) {
_call_graph.create_edge(mappedNode1->second.first, mappedNode2->second.first,
edge->from_start(), edge->to_end());
}
if (mappedNode1->second.first != -1 && mappedNode2->second.second != -1) {
_call_graph.create_edge(mappedNode1->second.first, mappedNode2->second.second,
edge->from_start(), edge->to_end());
}
if (mappedNode1->second.second != -1 && mappedNode2->second.first != -1) {
_call_graph.create_edge(mappedNode1->second.second, mappedNode2->second.first,
edge->from_start(), edge->to_end());
}
if (mappedNode1->second.second != -1 && mappedNode2->second.second != -1) {
_call_graph.create_edge(mappedNode1->second.second, mappedNode2->second.second,
edge->from_start(), edge->to_end());
}
}
}
};
_graph->for_each_edge(map_edge);
// make sure paths are saved
_call_graph.paths.rebuild_node_mapping();
_call_graph.paths.rebuild_mapping_aux();
_call_graph.paths.to_graph(_call_graph.graph);
// fix ids
//_call_graph.sort();
//_call_graph.compact_ids();
}
void Dijkstra::save_path(Node* start, Node* dest) {
NodePath* path = new NodePath;
Node* current_node = dest;
if (current_node->get_cost_from_start() == max_cost) {
saved_paths[NodePair(start, dest)] = path;
return;
}
while (start != current_node) {
path->push_back(current_node);
current_node = current_node->get_parent();
}
saved_paths[NodePair(start, dest)] = path;
}
NodePath* Dijkstra::calculate_path(Node* start, Node* dest) {
/**
* This is the dijkstra algorithm that searches through the node graph for
* the most efficiant way from start node to destination node.
*/
//Initialize; set cost to startnode to zero and rest to inf.
Node* current_node = start;
NodeVector graph = grid->get_nodes();
for (NodeVector::iterator it = graph.begin(); it != graph.end(); it++) {
(*it)->set_cost_from_start(max_cost);
}
start->set_cost_from_start(0);
prio_queue.push(current_node);
//Core loop
while (!prio_queue.empty()) {
current_node = prio_queue.top();
prio_queue.pop();
if (current_node == dest) {
save_path(start, dest);
while (!prio_queue.empty())
{
prio_queue.pop();
}
return create_path_copy(saved_paths[NodePair(start, dest)]);
}
NodeVector neighbors = current_node->get_neighbors();
for (NodeVector::iterator neighbor = neighbors.begin(); neighbor != neighbors.end(); neighbor++) {
if (*neighbor == current_node)
continue;
if (!(*neighbor)->is_allowed())
continue;
int new_cost = current_node->get_cost_from_start();
new_cost++;
int neighbor_cost = (*neighbor)->get_cost_from_start();
if (new_cost < neighbor_cost) {
(*neighbor)->set_cost_from_start(new_cost);
(*neighbor)->set_parent(current_node);
prio_queue.push((*neighbor));
}
}
}
save_path(start, dest);
return create_path_copy(saved_paths[NodePair(start, dest)]);
}
