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)));
}
}
}
}
// Determines if the next street segment is on the left or right
// turn of the current street segment
int TurnLeftOrRight(unsigned current_seg, unsigned next_seg){
StreetSegmentEnds curr_endpts = getStreetSegmentEnds(current_seg);
StreetSegmentEnds next_endpts = getStreetSegmentEnds(next_seg);
double curr_angle = 0;
double next_angle = 0;
if (curr_endpts.from == next_endpts.from){
curr_angle = getDirection(curr_endpts.to, curr_endpts.from);
next_angle = getDirection(next_endpts.from, next_endpts.to);
}
else if (curr_endpts.from == next_endpts.to){
curr_angle = getDirection(curr_endpts.to, curr_endpts.from);
next_angle = getDirection(next_endpts.to, next_endpts.from);
}
else {
curr_angle = getDirection(curr_endpts.from, curr_endpts.to);
next_angle = getDirection(next_endpts.to, next_endpts.from);
}
// cout << "Current angle: " << curr_angle << " Next angle: " << next_angle << endl;
if (((curr_angle - next_angle)*180/PI) > 60) return 2;
else if (((next_angle - curr_angle)*180/PI) > 60 ) return 0;
else return 1;
}
void Graph::initNodes() {
if (isinitialized == false) {
// All intersections are nodes. Traverse through all of them
for (unsigned i = 0; i < size; i++) {
Node newIntersection(i);
//std::string intname = getIntersectionName(i);
// Get adjacent intersections
std::vector<unsigned> adjacentInts = find_adjacent_intersections(i);
// Get all the street segments of this intersection
std::vector<unsigned> attached_SS = find_intersection_street_segments(i);
// Add new edges to the current node
for (std::vector<unsigned>::iterator int_it = adjacentInts.begin();
int_it != adjacentInts.end();
++int_it ) {
// Insert in adjacency list of a new edge
Edge newEdge(*int_it);
double minlength = std::numeric_limits<double>::infinity();
// Need to insert the streetsegments that take you along the edge
for (std::vector<unsigned>::const_iterator SS_it = attached_SS.begin();
SS_it != attached_SS.end();
++SS_it) {
StreetSegmentEnds currentEnds = getStreetSegmentEnds(*SS_it);
bool OneWay = getStreetSegmentOneWay(*SS_it);
// If the node and the adjacent intersection is connected in any way
// by this street segment
if ( (currentEnds.from == i && currentEnds.to == (*int_it)) ||
(currentEnds.from == (*int_it) && currentEnds.to == i)) {
// If it is one way, then only connect in the right direction
// To must be the adjacent one
if (OneWay) {
if ( currentEnds.to == (*int_it) ) {
if ( find_street_segment_length(*SS_it) < minlength ) {
newEdge.setpath_streetseg(*SS_it);
newEdge.setLength(find_street_segment_length(*SS_it));
minlength = find_street_segment_length(*SS_it);
}
}
}
else {
if ( find_street_segment_length(*SS_it) < minlength ) {
newEdge.setpath_streetseg(*SS_it);
newEdge.setLength(find_street_segment_length(*SS_it));
minlength = find_street_segment_length(*SS_it);
}
}
}
}
newIntersection.addEdge( newEdge );
}
node_vector.push_back(newIntersection);
}
isinitialized = true;
}
}