void DrawPath(const std::vector<unsigned>& path, t_color color) {
unsigned number_of_curvepoints;
// loop twice to draw the path and path border
for (unsigned dis = 0; dis < 2; dis++) {
if (dis == 0) {
setcolor(BLUE);
setlinewidth(4);
}
if (dis == 1) {
setcolor(color);
setlinewidth(2);
}
for (unsigned i = 0; i < path.size(); i++) {
StreetSegmentEnds end_points = getStreetSegmentEnds(path[i]);
t_point start_point = LatLontoCoordinate(getIntersectionPosition(end_points.from));
t_point end_point = LatLontoCoordinate(getIntersectionPosition(end_points.to));
number_of_curvepoints = getStreetSegmentCurvePointCount(path[i]);
if (number_of_curvepoints == 0) {
drawline(start_point, end_point);
} else {
drawline(start_point, LatLontoCoordinate(getStreetSegmentCurvePoint(path[i], 0)));
drawline(end_point, LatLontoCoordinate(getStreetSegmentCurvePoint(path[i], number_of_curvepoints - 1)));
for (unsigned j = 0; j < number_of_curvepoints - 1; j++)
drawline(LatLontoCoordinate(getStreetSegmentCurvePoint(path[i], j)),
LatLontoCoordinate(getStreetSegmentCurvePoint(path[i], j + 1)));
}
}
}
}
// get the angle of a street segment with specified starting point
// and end point
double getDirection(unsigned startid, unsigned endid) {
t_point start = LatLontoCoordinate(getIntersectionPosition(startid));
t_point end = LatLontoCoordinate(getIntersectionPosition(endid));
double dy = end.y - start.y;
double dx = end.x - start.x;
double direction = atan(dy / dx);
if (direction == 0) if (dx < 0) direction = PI;
if (dx < 0 && dy > 0) direction = PI + direction;
else if (dx < 0 && dy < 0) direction = direction + PI;
else if (dx > 0 && dy < 0) direction = direction + 2 * PI;
return direction;
}
unsigned find_closest_intersection(t_point my_position, string input) {
unsigned id = find_closest_intersection(my_position);
vector<LatLon> POI_pos = find_POI_from_name(input);
vector<unsigned> inter_ids = find_closest_intersection(POI_pos);
double min_dist = 1e10;
unsigned end_id = 0;
for (vector<unsigned>::iterator iter = inter_ids.begin(); iter != inter_ids.end(); iter++){
LatLon id_pos = getIntersectionPosition(id);
LatLon it_pos = getIntersectionPosition(*iter);
double temp_dist = find_distance_between_two_points(id_pos, it_pos);
if (temp_dist < min_dist) {
min_dist = temp_dist;
end_id = *iter;
}
}
return end_id;
}
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)));
}
}
}
t_point Search(string destination) {
int tempID = keywordSearchIntersections(destination);
if (tempID >= 0) {
update_message(getIntersectionName(tempID));
return LatLontoCoordinate(getIntersectionPosition(tempID));
}
else {
update_message("No such street intersection(s)");
return t_point(-1, -1);
}
}
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;
}
// Use A* algorithm to calculate shortest path between intersections
vector<unsigned> DirectedPath(unsigned startid, unsigned endid) {
unordered_map<unsigned, bool> flag; //if node is visited
unordered_map<unsigned, double> dist; //weight of edge
unordered_map<unsigned, pair<unsigned, unsigned>> prev; //the previous node&edge of key
prev[startid] = make_pair(startid, 0);
LatLon end = getIntersectionPosition(endid);
priority_queue<pair<unsigned, double>, vector<pair<unsigned, double>>, comparenorm> Q; // the node to be visited
vector<unsigned> Path;
Q.push(make_pair(startid, 0));
unordered_map<unsigned, unsigned>* outedges; //the out going edges of an intersection
// unordered_map<unsigned, unordered_map<unsigned, pair<unsigned, unsigned>>>::iterator memo;
// bool found_memo = false;
// unsigned memoid = 0;
/**************************DEBUG USE***************************/
bool DEBUG = 0; //enable to draw the process of computing the path
/***************************************************************/
while (!Q.empty()) {
unsigned currentid = Q.top().first; // accessing the weight of the edge, or distance
Q.pop();
if (currentid == endid) break;
if (flag[currentid] != 1) {
flag[currentid] = 1;
// memo = Memoize(currentid, endid); //unfinished implementation of memoization
// if (memo != Memo.end()) {
// found_memo = true;
// memoid = currentid;
// cout << "i broke out" <<endl;
// break;
// }
// vector<unsigned> testdraw; //for debug use
outedges = &getOutEdges(currentid); //obtain the outgoing edges and the other end point
for (unordered_map<unsigned, unsigned>::const_iterator iter = outedges->begin(); iter != outedges->end(); iter++) {
//for all the street segments around the current intersections
//street segment id
unsigned path_segment = iter->first;
//the other end point intersection id
unsigned nextid = iter->second;
// how long it takes to travel through this segment
double travel_time = find_segment_travel_time(path_segment) + dist[currentid];
//if the current street segment is on the same street with the next street segment
if (getStreetSegmentStreetID(path_segment) != getStreetSegmentStreetID(prev[currentid].second))
travel_time += 0.25;
// if (DEBUG) {
// testdraw.push_back(path_segment);
// DrawPath(testdraw, t_color(64, 153, 255));
// }
// if the current path to next intersection is found to be faster than the
// previous path, update the path and time
if ((dist[nextid] == 0) || (travel_time < dist[nextid])) {
dist[nextid] = travel_time;
prev[nextid] = make_pair(currentid, path_segment);
}
// get the position of the next intersection id
LatLon currentposition = getIntersectionPosition(nextid);
// find the distance between the next intersection id and the end point
double current_distance = find_distance_between_two_points(currentposition, end);
double weight = ((dist[nextid]) + current_distance * 0.06 / 100);
// put the intersection into the priority queue
Q.push(make_pair(nextid, weight));
}
}
}
// if(found_memo){
// unsigned iter = endid;
// while (iter != memoid) {
// pair<unsigned, unsigned> idandpath = ((memo->second).find(iter))->second;
// Path.insert(Path.begin(), idandpath.second);
// iter = idandpath.first;
// }
// endid = memoid;
// }
unsigned iter = endid;
while ((iter != startid) && (dist[endid] != 0)) {
pair<unsigned, unsigned> idandpath = prev[iter];
Path.insert(Path.begin(), idandpath.second);
// if (DEBUG) {
// bool OneWay = getStreetSegmentOneWay(idandpath.second);
// StreetSegmentEnds ids = getStreetSegmentEnds(idandpath.second);
// if (OneWay && (ids.from == iter)) {
// cout << "ONEWAY PATH: " << idandpath.second << endl;
// cout << "FROM: "<< ids.from << " TO: " << ids.to << endl;
// cout << "INSTEAD OF: " << idandpath.first << " TO: " << iter <<endl;
// bool connected = are_directly_connected(idandpath.first, iter);
// cout << "ARE THEY DIRECTLY CONNECTED? " << connected << endl;
// break;
// }
// }
// Memo[iter] = prev;
// prev.erase(iter);
iter = idandpath.first;
}
if (DEBUG) {
DrawPath(Path, t_color(255, 0, 0));
double computed_time = compute_path_travel_time(Path);
if (computed_time == 0) cout <<"PATH FROM: "<< startid <<" TO "<< endid << " NOT FOUND"<<endl;
}
return Path;
}
