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