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)]); }