std::vector<unsigned> find_path_between_intersections(unsigned intersect_id_start, unsigned intersect_id_end) { if (intersect_id_start >= getNumberOfIntersections() || intersect_id_end >= getNumberOfIntersections()) { std::vector<unsigned> empty; return empty; } std::vector<unsigned> result = GVAR_roadmap->a_star_algorithm_by_time(intersect_id_start, intersect_id_end); return result; }
// POI Intersection std::vector<std::pair<unsigned, unsigned> > closestIntersectiontoPOI(const std::vector<unsigned>& POI_IDs) { std::vector<std::pair<unsigned,unsigned> > POI_intersection_pair; for (std::vector<unsigned>::const_iterator it = POI_IDs.begin(); it != POI_IDs.end(); ++it) { LatLon POI_pos = getPointOfInterestPosition((*it)); double min_dist = std::numeric_limits<double>::infinity(); unsigned min_dist_intersection; // O(n) time complexity -- change to KD tree if lower time desired for (unsigned i = 0; i < getNumberOfIntersections(); i++) { LatLon intersection_pos = GVAR_intersection_positions[i]; double curdist = find_distance_between_two_points(POI_pos, intersection_pos); if (curdist < min_dist) { min_dist = curdist; min_dist_intersection = i; } } std::pair<unsigned,unsigned> poi_and_closest_intersection = { (*it), min_dist_intersection }; POI_intersection_pair.push_back(poi_and_closest_intersection); } return POI_intersection_pair; }
std::vector<unsigned> find_path_to_point_of_interest (unsigned intersect_id_start, std::string point_of_interest_name) { if (intersect_id_start >= getNumberOfIntersections()) { std::vector<unsigned> empty; return empty; } std::vector<unsigned> POI_IDs = getAllPOIwithName(point_of_interest_name); min_priority_queue closestpois_queue = closestPOIstointer(intersect_id_start, POI_IDs); std::vector<unsigned> closepois = extract10fromqueue(closestpois_queue); std::vector<std::pair<unsigned, unsigned> > POIs_and_Neighbor_Int = closestIntersectiontoPOI(closepois); //for (std::vector<std::pair<unsigned,unsigned> >::const_iterator it = POIs_and_Neighbor_Int.begin(); // it != POIs_and_Neighbor_Int.end(); // ++it) { // std::cout << (*it).first << getPointOfInterestName((*it).first) << " is close to " << (*it).second << std::endl; //} std::vector<std::vector<unsigned> > paths; for (std::vector<std::pair<unsigned,unsigned> >::const_iterator it = POIs_and_Neighbor_Int.begin(); it != POIs_and_Neighbor_Int.end(); ++it) { //std::cout << (*it).first << getPointOfInterestName((*it).first) << " is close to " << (*it).second << std::endl; paths.push_back(GVAR_roadmap->a_star_algorithm_by_time(intersect_id_start, (*it).second)); } return find_minimal_time_path(paths); }
void initialize_graph() { if (GVAR_roadmap == NULL) { GVAR_roadmap = new Graph; for (unsigned i = 0; i < getNumberOfIntersections(); i++) { GVAR_intersection_positions.insert(std::make_pair(i, getIntersectionPosition(i))); } } }
void LoadIntersectionNames() { for (unsigned i = 0; i < getNumberOfIntersections(); i++) { stringstream intersection_name; intersection_name << IntersectionNameFilter(getIntersectionName(i)); string street_name; vector<string> key; while (!intersection_name.fail()) { intersection_name >> street_name; //to be completed } } }
unsigned find_closest_intersection(t_point my_position) { unsigned id = 0; double predistance = 1000; double distance; for (unsigned i = 0; i < getNumberOfIntersections(); i++) { t_point position = LatLontoCoordinate(getIntersectionPosition(i)); distance = sqrt(pow(position.x - my_position.x, 2) + pow(position.y - my_position.y, 2)); if (distance < predistance) { id = i; predistance = distance; } } return id; }
std::vector<unsigned> Graph::a_star_algorithm_by_time(unsigned source, unsigned sink) { //std::cout << "Getting directions from: " << getIntersectionPosition(source).lat << "," << getIntersectionPosition(source).lon << " or " << getIntersectionName(source) << std::endl; //std::cout << "To: " << getIntersectionPosition(sink).lat << "," << getIntersectionPosition(sink).lon << " or " << getIntersectionName(sink) << std::endl; if (source == sink || source >= getNumberOfIntersections() || sink >= getNumberOfIntersections()) { std::vector<unsigned> empty; return empty; } boost::heap::fibonacci_heap< std::pair<unsigned, double>, boost::heap::compare<compare>> Q; std::vector<double> dist; std::vector<double> f_score; std::vector<unsigned> prev; std::vector<fib_handle> handles; std::stack<unsigned> streetseg_stack; for (std::vector<Node>::iterator it = node_vector.begin(); it != node_vector.end(); ++it ) { unsigned nodeid = (*it).getNodeID(); if ( nodeid != source) { dist.push_back(std::numeric_limits<double>::infinity()); prev.push_back(0xFACEFACE); // dead means undefined f_score.push_back(std::numeric_limits<double>::infinity()); } else { dist.push_back(0); prev.push_back(0xFACEFACE); // this is the end f_score.push_back(heuristic_cost_straight_dist(nodeid, sink)); } fib_handle handle = Q.push(std::make_pair(nodeid, f_score[nodeid])); handles.push_back(handle); } while ( !Q.empty() ) { Node* curNode = &node_vector[(Q.top()).first]; unsigned u = (*curNode).getNodeID(); if (u == sink) { // extract the previous nodes and daisy chain back to src unsigned currentNode = u; unsigned previousNode = prev[u]; while (previousNode != 0xFACEFACE) { //std::cout << previousNode << std::endl; Node* prevNode = &(node_vector[previousNode]); std::vector<Edge> successors = (*prevNode).getSuccessors(); for (std::vector<Edge>::iterator nodeIT = successors.begin(); nodeIT != successors.end(); ++nodeIT) { if ( (*nodeIT).getnodeID() == currentNode ) { streetseg_stack.push( (*nodeIT).getPath_Streetseg() ); } } currentNode = previousNode; previousNode = prev[previousNode]; } return stack_to_vector(streetseg_stack); } Q.pop(); std::vector<Edge> neighbors = (*curNode).getSuccessors(); unsigned prevSS = 0xAAAAAAAA; unsigned previNode = prev[u]; if (previNode != 0xFACEFACE) { // previous node must be defined //std::cout << "got to " << u << " from " << prev[u] << std::endl; Node* lastNode = &(node_vector[previNode]); std::vector<Edge> howdidIgettoU = (*lastNode).getSuccessors(); for (std::vector<Edge>::iterator it = howdidIgettoU.begin(); it != howdidIgettoU.end(); ++it) { if ((*it).getnodeID() == u) { prevSS = (*it).getPath_Streetseg(); } } } for (std::vector<Edge>::iterator it = neighbors.begin(); it != neighbors.end(); ++it) { unsigned v = (*it).getnodeID(); double alt = dist[u] + find_segment_travel_time((*it).getPath_Streetseg()); unsigned currSS = (*it).getPath_Streetseg(); if (prevSS != 0xAAAAAAAA) { //std::cout << "Turning from " << getStreetName(getStreetSegmentStreetID(prevSS)) << " to " << getStreetName(getStreetSegmentStreetID(currSS)) << std::endl; if (getStreetSegmentStreetID(prevSS) != getStreetSegmentStreetID(currSS)) { //std::cout << "Turning" << std::endl; alt += 0.25; } } if (alt < dist[v]) { dist[v] = alt; prev[v] = u; f_score[v] = dist[v] + heuristic_cost_straight_dist(v, sink); double asdf = f_score[v]; fib_handle PriorityUpdate = handles[v]; Q.update(PriorityUpdate, std::make_pair(v, asdf)); } } } std::vector<unsigned> empty; return empty; }
Graph::Graph() { size = getNumberOfIntersections(); isinitialized = false; initNodes(); }