double compute_path_travel_time(const std::vector<unsigned>& path) {
double total_time_minutes = 0;//time in minutes
if (path.size() == 0) {
return 0;
}
//let i represent the street segment
for (unsigned i = 0; i < path.size(); i++) {
// if this streetsegment name is different from i-1 streetsegment name
// then add 15seconds for the turn
if (i > 0) {
if (getStreetSegmentStreetID(path[i]) != getStreetSegmentStreetID(path[i-1])) {
total_time_minutes += 0.25;
}
}
total_time_minutes += find_segment_travel_time(path[i]);
}
return total_time_minutes;
}
double compute_path_travel_time(const std::vector<unsigned>& path) {
// Returns the time required to travel along the path specified. The path
// is passed in as a vector of street segment ids, and this function can
// assume the vector either forms a legal path or has size == 0.
// The travel time is the sum of the length/speed-limit of each street
// segment, plus 15 seconds per turn implied by the path. A turn occurs
// when two consecutive street segments have different street names.
if (path.empty()) return 0;
else {
double sum = 0;
unsigned temp = getStreetSegmentStreetID(path[0]);
for (unsigned i = 0; i < path.size(); i++) {
sum += find_segment_travel_time(path[i]);
unsigned id = getStreetSegmentStreetID(path[i]);
if (temp != id) {
sum += 0.25;
}
temp = id;
}
// if(sum > 71.0 && sum < 72.0) sum -= 5;
return sum ;
}
}
void GetPathInstructions(vector<unsigned> Path){
// First, draw a box in the graphics window, on which the path instructions
// will be displayed.
float tempXCenter = get_visible_world().get_xcenter();
float tempYCenter = get_visible_world().get_ycenter();
float BoxLeft = tempXCenter + 0.18 * get_visible_world().get_width();
float BoxRight = tempXCenter + 0.48 * get_visible_world().get_width();
float BoxBottom = tempYCenter - 0.48 * get_visible_world().get_height();
float BoxTop = tempYCenter + 0.12 * get_visible_world().get_height();
t_bound_box HelpBox(BoxLeft, BoxBottom, BoxRight, BoxTop);
// string intro = "Welcome to Map-2-Go";
setcolor(246, 246, 246);
fillrect(HelpBox);
// Draw the borders of the box.
setcolor(200, 200, 200);
setlinewidth(6);
t_point border[6];
border[5].x = BoxRight;
border[5].y = BoxTop;
// Add depth to the box, making the user feel as if he or she is looking
// in 3D perspective at the box.
border[2].x = BoxLeft + 0.02 * HelpBox.get_width();
border[2].y = BoxBottom - 0.02 * HelpBox.get_width();
border[3].x = BoxRight + 0.02 * HelpBox.get_width();
border[3].y = BoxBottom - 0.02 * HelpBox.get_width();
border[4].x = BoxRight + 0.02 * HelpBox.get_width();
border[4].y = BoxTop - 0.02 * HelpBox.get_width();
border[0].x = BoxRight;
border[0].y = BoxBottom;
border[1].x = BoxLeft;
border[1].y = BoxBottom;
fillpoly(border, 6);
setcolor(20, 20, 20);
setlinewidth(1);
drawrect(border[1], border[5]);
drawline(border[1], border[2]);
drawline(border[2], border[3]);
drawline(border[3], border[0]);
drawline(border[4], border[5]);
drawline(border[4], border[3]);
// From now on, start displaying the path instructions.
settextrotation(0);
setcolor(20,20,20);
setfontsize(12);
string InstructionLabel = "Path Instructions";
// Write "Path Instructions" onto the box in the graphics window.
drawtext(HelpBox.left() + 0.5 * HelpBox.get_width(), HelpBox.top() - 0.03 * HelpBox.get_height(), InstructionLabel, HelpBox.get_width(), HelpBox.get_height());
// Draw a line separating the header above from the instructions later on.
setlinewidth(2);
drawline(HelpBox.left() + 0.05 * HelpBox.get_width(), HelpBox.top() - 0.06 * HelpBox.get_height(), HelpBox.right() - 0.05 * HelpBox.get_width(), HelpBox.top() - 0.06 * HelpBox.get_height()); //splitline
settextrotation(0);
setcolor(20,20,20);
setfontsize(10);
/////////////////////////////////////////////////////////
bool differentStreet; // Bool flag to determine if two street segments belong to different streets.
bool needExtraLine; // Bool flag to determine if obtained path instruction has too many characters
// to fit in the width of the box.
int numLines = 0; // This counter determines how far down the box each subsequent instruction will be displayed at.
int displayStraightInstruction = 0; // When this integer counter is zero, a path instruction that has "Go Straight"
// will be displayed once, but if the subsequent instruction is still "Go Straight", the integer counter will have
// incremented by one already - thus preventing the redundant instruction rom being displayed.
// Traverse the vector of street segments, and access the current and current+1 street segment with each iteration.
for (unsigned i = 0; i < Path.size()-1; i++){
int LeftOrRight = 3; // This integer determines whether to turn right, left, or go straight. It is initialized
// to 3 for each iteration, so as not to match any of the above cases initially.
string tempString; // This will store the path instruction.
string tempString2; // This will store the remainder of the path instruction, if the instruction is longer than
// 50 characters and will potentially be cut off.
// The bool flags are initially set to false, for each iteration.
differentStreet = false;
needExtraLine = false;
// Obtain street name of the first street segment.
string StreetName1 = getStreetName(getStreetSegmentStreetID(Path[i]));
// Obtain street name of the second street segment.
string StreetName2 = getStreetName(getStreetSegmentStreetID(Path[i+1]));
// If the second segment's street name is "(unknown)" or " ", then do not go and create
// a proper path instruction for it.
if ((StreetName2 != "(unknown)") && (StreetName2 != " ")){
// If the second segment's street name WAS ACTUALLY a valid name...
// If the two street names are different, the user will then go onto a different street, so
// set the flag to true.
if (StreetName2!=StreetName1){
differentStreet = true;
}
// Call TurnLeftOrRight function to determine which way to turn.
// If the returned value is 2, turn right; if the value is 0, turn left;
// else if the value is 1, go straight.
LeftOrRight = TurnLeftOrRight(Path[i],Path[i+1]);
if (LeftOrRight==2){
tempString.append("Turn right");
//displayStraightInstruction = 0; // If turning right, automatically enable the instruction to be
// displayed again, regardless of whether or not the street name remains the same.
}
else if (LeftOrRight==1){
tempString.append("Go straight");
}
else if (LeftOrRight==0){
tempString.append("Turn left");
//displayStraightInstruction = 0; // If turning left, automatically enable the instruction to be
// displayed again, regardless of whether or not the street name remains the same.
}
/////////////////////////////////////////////////////////
// If the user does go onto a different street, add " onto " into the instruction.
if (differentStreet){
tempString.append(" onto ");
tempString.append(StreetName2);
displayStraightInstruction = 0; // If different street names, set the displayStraightInstruction counter
// back to zero, so that subsequent "Go Straight" instructions can be displayed at least once.
}
else{ // If the user does NOT go onto a different street, add " along " into the instruction.
tempString.append(" along ");
tempString.append(StreetName2);
}
/////////////////////////////////////////////////////////
// If the string length is two wide to fit onto the box (max 50 characters),
// store the remainder of the string into a second string, and print it on the next line.
if (tempString.length()>=50){
needExtraLine = true;
size_t pos = tempString.find_last_of(' ',string::npos); // Find the last occurrence of whitespace in the string.
tempString2 = tempString.substr(pos+1,30);
tempString.erase(pos,string::npos);
}
/////////////////////////////////////////////////////////
// The following if case only applies to an instruction if it tells the user to "Go Straight".
// For the first occurrence of "Go Straight", the instruction will be displayed, but if it appears
// consecutively many times afterwards, the instructions will be disabled by incrementing the
// displayStraightInstruction counter.
if (displayStraightInstruction < 1){
// Draw the text onto the box.
drawtext(HelpBox.left() + 0.5 * HelpBox.get_width(), HelpBox.top() - (0.10 +0.04*numLines) * HelpBox.get_height(), tempString, HelpBox.get_width(), HelpBox.get_height());
numLines++; // Increment the numLines counter so set the subsequent line further down the box.
if (needExtraLine){ // If an extra line is needed, display this instruction too.
drawtext(HelpBox.left() + 0.5 * HelpBox.get_width(), HelpBox.top() - (0.10 +0.04*numLines) * HelpBox.get_height(), tempString2, HelpBox.get_width(), HelpBox.get_height());
numLines++;
}
}
// Before iterating the overall for loop again and checking the next two segments, check if
// the instruction this time was
if (!differentStreet)
displayStraightInstruction++;
}
}
// After exiting the for loop and displaying all instructions, display this message for users to
// know how to clear the screen of any displayed path or path instructions.
string endMessage = "Press 'Esc' to clear the path instructions. ";
drawtext(HelpBox.left() + 0.5 * HelpBox.get_width(), HelpBox.top() - (0.10 +0.04*numLines + 0.015) * HelpBox.get_height(), endMessage, HelpBox.get_width(), HelpBox.get_height());
// DisplayPathInstructions(InstructionVector);
}
// The same kind of process as DirectedPath but takes an input of
// a vector of destinations, and the weight of each intersection
// is not added with the hueristic estimation.
// the function will return a path as soon as one of the destination
// is reached , which means that this destination can be reached in
// the least time.
vector<unsigned> Dijkstra(unsigned startid, vector<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);
priority_queue<pair<unsigned, double>, vector<pair<unsigned, double>>, comparenorm> Q;
vector<unsigned> Path;
vector<unsigned> testdraw;//DEBUG
dist[startid] = 0;
Q.push(make_pair(startid, 0));
unsigned destination = endid[0];
bool found = false;
unordered_map<unsigned, unsigned>* outedges;
/**************************DEBUG USE***************************/
// bool DEBUG = 0; //enable to draw the process of computing the path
/***************************************************************/
while (!Q.empty()) {
// vector<unsigned> testdraw;
unsigned currentid = Q.top().first;
Q.pop();
for (vector<unsigned>::iterator iter = endid.begin(); iter != endid.end(); iter++) {
if (currentid == *iter) {
destination = *iter;
found = true;
}
}
if (found) break;
if (flag[currentid] != 1) {
flag[currentid] = 1;
outedges = &getOutEdges(currentid);
for (unordered_map<unsigned, unsigned>::const_iterator iter = outedges->begin(); iter != outedges->end(); iter++) {
unsigned path_segment = iter->first;
unsigned nextid = iter->second;
double travel_time = find_segment_travel_time(path_segment) + dist[currentid];
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 ((dist[nextid] == 0) || (travel_time < dist[nextid])) {
dist[nextid] = travel_time;
prev[nextid] = make_pair(currentid, path_segment);
Q.push(make_pair(nextid, dist[nextid]));
}
}
}
}
unsigned iter = destination;
while ((iter != startid) && (dist[destination] != 0)) {
pair<unsigned, unsigned> idandpath = prev[iter];
Path.insert(Path.begin(), idandpath.second);
iter = idandpath.first;
}
// if (DEBUG) {
// DrawPath(Path, t_color(255, 0, 0));
// }
return Path;
}
// 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;
}
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;
}
dist_prev Graph::dijkstra_time_nostop(unsigned source, std::vector<unsigned> traverse_these_intersections) {
// Returns a vector of prev that if you daisy chain backwards on, you will
// obtain the shortest path.
boost::heap::fibonacci_heap< std::pair<unsigned, double>, boost::heap::compare<compare>> Q;
std::vector<double> dist;
std::vector<unsigned> prev;
std::vector<fib_handle> handles;
auto iterator_check = std::find(traverse_these_intersections.begin(), traverse_these_intersections.end(), source);
if (iterator_check != traverse_these_intersections.end()) {
traverse_these_intersections.erase(iterator_check);
}
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
}
else {
dist.push_back(0);
prev.push_back(0xFACEFACE); // this is the end
}
fib_handle handle = Q.push(std::make_pair(nodeid, dist[nodeid]));
handles.push_back(handle);
}
while ( !Q.empty() && traverse_these_intersections.size()-1 != 0) {
Node* curNode = &node_vector[(Q.top()).first];
unsigned u = (*curNode).getNodeID();
auto iterator_check = std::find(traverse_these_intersections.begin(), traverse_these_intersections.end(), u);
if (iterator_check != traverse_these_intersections.end()) {
traverse_these_intersections.erase(iterator_check);
}
//std::cout << traverse_these_intersections.size() << " ";
Q.pop();
std::vector<Edge> neighbors = (*curNode).getSuccessors();
unsigned prevSS = 0xAAAAAAAA;
unsigned previNode = prev[u];
if (u != source && 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) {
if (getStreetSegmentStreetID(prevSS) != getStreetSegmentStreetID(currSS)) {
// std::cout << "Turning from " << getStreetName(getStreetSegmentStreetID(prevSS)) << " to " << getStreetName(getStreetSegmentStreetID(currSS)) << std::endl;
alt += 0.25;
}
}
if (alt < dist[v]) {
dist[v] = alt;
prev[v] = u;
fib_handle PriorityUpdate = handles[v];
Q.update(PriorityUpdate, std::make_pair(v, alt));
}
}
}
dist_prev result = {prev, dist};
return result;
}
