-
Notifications
You must be signed in to change notification settings - Fork 0
/
PointToPointRouter.cpp
178 lines (153 loc) · 6.04 KB
/
PointToPointRouter.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
#include "provided.h"
#include "ExpandableHashMap.h"
#include <utility>
#include <list>
#include <set>
#include <vector>
using namespace std;
class PointToPointRouterImpl
{
public:
PointToPointRouterImpl(const StreetMap* sm);
~PointToPointRouterImpl();
DeliveryResult generatePointToPointRoute(
const GeoCoord& start,
const GeoCoord& end,
list<StreetSegment>& route,
double& totalDistanceTravelled) const;
private:
const StreetMap* m_stmap;
};
PointToPointRouterImpl::PointToPointRouterImpl(const StreetMap* sm): m_stmap(sm)
{
}
PointToPointRouterImpl::~PointToPointRouterImpl()
{
}
DeliveryResult PointToPointRouterImpl::generatePointToPointRoute(
const GeoCoord& start,
const GeoCoord& end,
list<StreetSegment>& route,
double& totalDistanceTravelled) const
{
route.clear();
totalDistanceTravelled = 0;
if (start == end)
{
return DELIVERY_SUCCESS;
}
// Check if the beginning and ending coordinate are in the map:
vector<StreetSegment> test1,test2;
m_stmap->getSegmentsThatStartWith(start,test1);
m_stmap->getSegmentsThatStartWith(end,test2);
if (test1.empty() || test2.empty())
return BAD_COORD;
ExpandableHashMap<GeoCoord,GeoCoord> closed_list;
set<pair<double,pair<GeoCoord,GeoCoord>>> open_list;
open_list.insert(make_pair(0.0,make_pair(start,start)));
GeoCoord parent = start;
double current_f = 0.0;
while (!open_list.empty())
{
pair<double,pair<GeoCoord,GeoCoord>> top = *open_list.begin();
open_list.erase(open_list.begin());
parent = top.second.second; // set parent to the second geocoord
current_f = top.first; // set the current f-value to be added to the f-value of the parent
closed_list.associate(parent,top.second.first);
/*
if (parent != start)
{
//add the top of the priority queue into the route
route.push_back(top.second);
totalDistanceTravelled += distanceEarthMiles(top.second.start,top.second.end);
}
*/
if (parent == end)
{
GeoCoord current_geo = end;
GeoCoord next_geo = *closed_list.find(end);
while (current_geo != start)
{
totalDistanceTravelled += distanceEarthMiles(next_geo,current_geo);
vector<StreetSegment> find_seg;
m_stmap->getSegmentsThatStartWith(next_geo, find_seg);
for (auto p = find_seg.begin(); p != find_seg.end(); p++)
{
if ((*p).end == current_geo)
{
route.push_front((*p));
}
}
current_geo = next_geo;
next_geo = *closed_list.find(current_geo);
}
// go through every geocoord and its parent
// have to use find in hash map to find geocoord partner & streetsegment appropriate, then push front onto the list
// add up everything
return DELIVERY_SUCCESS;
}
vector<StreetSegment> possible_segs;
m_stmap->getSegmentsThatStartWith(parent,possible_segs);
for (auto p = possible_segs.begin(); p != possible_segs.end(); p++)
{
GeoCoord possible_geo = (*p).end;
if (!closed_list.find(possible_geo)) // check if already in closed list
{
// calculate f-value
double g = distanceEarthMiles(possible_geo,end);
double f = g + current_f;
// check if already in open list (change f-value and parent if f-value is less)
bool no_change = false;
for (auto p = open_list.begin(); p != open_list.end(); p++)
{
if (possible_geo == (*p).second.second)
{
if (f < (*p).first)
{
open_list.erase(p);
break;
}
else
{
no_change = true;
break;
}
}
}
if (!no_change)
{
open_list.insert(make_pair(f,make_pair((*p).start,(*p).end)));
}
}
}
}
/*
First: create an "open list" to store potential values (make a list of pairs of geocoords and f-values),
Second: create a pair for the starting point, and assign f-value of 0; make this equal to "previous geocoord"
third: loop while the open list is empty
fourth: pop off the first item on the list (highest priority), make this one the parent point now (keep its f-value as well), add this to the expandable hash map,check if this is the destination
fifth: find all valid street segments from that point and add to the priority queue with their f-value (probably need a for loop), as well as their parent geocoord; if already in priority queue, then change its f-value and parent point (assuming f-value is less)
sixth: pair of geocoords
reconstruct the route using the map points
*/
return DELIVERY_SUCCESS; // route could not be found
}
//******************** PointToPointRouter functions ***************************
// These functions simply delegate to PointToPointRouterImpl's functions.
// You probably don't want to change any of this code.
PointToPointRouter::PointToPointRouter(const StreetMap* sm)
{
m_impl = new PointToPointRouterImpl(sm);
}
PointToPointRouter::~PointToPointRouter()
{
delete m_impl;
}
DeliveryResult PointToPointRouter::generatePointToPointRoute(
const GeoCoord& start,
const GeoCoord& end,
list<StreetSegment>& route,
double& totalDistanceTravelled) const
{
return m_impl->generatePointToPointRoute(start, end, route, totalDistanceTravelled);
}