Mercurial > fife-parpg

comparison engine/core/pathfinder/routepather/routepather.cpp @ 190:f970f7dab2dd

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Allow routepather to re-plan when the plan becomes invalid: - one of the steps in the plan is blocked - the target of the plan is changed
author icelus@33b003aa-7bff-0310-803a-e67f0ece8222
date Sat, 28 Feb 2009 12:15:04 +0000
parents 90005975cdbb
children 6362c6812580
comparison
equal deleted inserted replaced
189:3d0cc4545938 190:f970f7dab2dd
43 return; 43 return;
44 } 44 }
45 m_map = map; 45 m_map = map;
46 } 46 }
47 47
48 int RoutePather::getNextLocation(const Instance* instance, const Location& target, 48 int RoutePather::makeSessionId() {
49 double distance_to_travel, Location& nextLocation, 49 return m_nextFreeSessionId++;
50 Location& facingLocation, int session_id, int priority) { 50 }
51 assert(instance); 51
52 assert(instance->getLocation().getLayer() == target.getLayer()); 52 void RoutePather::makePlan(const Instance *instance, const Location& target, int session_id, int priority) {
53 if(session_id != -1) {
54 PathMap::iterator j = m_paths.find(session_id);
55 if(j != m_paths.end()) {
56 if(j->second.empty()) {
57 m_paths.erase(j);
58 return -1;
59 } else {
60 followPath(instance, j->second, distance_to_travel, nextLocation, facingLocation);
61 return session_id;
62 }
63 } else {
64 if(!sessionIdValid(session_id)) {
65 //Session id is invalid.
66 return -1;
67 }
68 }
69 return session_id;
70 }
71 SearchSpace* searchspace = getSearchSpace(target.getLayer()); 53 SearchSpace* searchspace = getSearchSpace(target.getLayer());
72 if(!searchspace) { 54 if(!searchspace) {
73 searchspace = new SearchSpace(target.getLayer()); 55 searchspace = new SearchSpace(target.getLayer());
74 addSearchSpace(searchspace); 56 addSearchSpace(searchspace);
75 } 57 }
76 if(searchspace->isInSearchSpace(target)) { 58 if(searchspace->isInSearchSpace(target)) {
77 session_id = m_nextFreeSessionId++;
78 RoutePatherSearch* newSearch = new RoutePatherSearch(session_id, instance->getLocation(), target, searchspace); 59 RoutePatherSearch* newSearch = new RoutePatherSearch(session_id, instance->getLocation(), target, searchspace);
79 m_sessions.pushElement(SessionQueue::value_type(newSearch, priority)); 60 m_sessions.pushElement(SessionQueue::value_type(newSearch, priority));
80 addSessionId(session_id); 61 addSessionId(session_id);
62 m_path_targets.insert(LocationMap::value_type(session_id,target));
63 }
64 }
65
66 bool RoutePather::locationsEqual(const Location &a, const Location &b) {
67
68 const ModelCoordinate a_coord = a.getLayerCoordinates();
69 const ModelCoordinate b_coord = b.getLayerCoordinates();
70
71 return a_coord == b_coord;
72 }
73
74 bool RoutePather::testStep(const Instance *instance, Path& path) {
75 Location instanceLoc = instance->getLocation();
76 if(!path.empty() &&
77 !locationsEqual(path.front(), instanceLoc) &&
78 instanceLoc.getLayer()->cellContainsBlockingInstance(path.front().getLayerCoordinates())) {
79 const bool last_step = path.front() == path.back();
80 path.clear();
81 return last_step;
82 }
83 return true;
84 }
85
86 int RoutePather::getNextLocation(const Instance* instance, const Location& target,
87 double distance_to_travel, Location& nextLocation,
88 Location& facingLocation, int session_id, int priority) {
89 assert(instance);
90 assert(instance->getLocation().getLayer() == target.getLayer());
91 bool plan_needed = true;
92
93 if(session_id != -1) {
94 plan_needed = false;
95 PathMap::iterator path_itor = m_paths.find(session_id);
96 if(path_itor != m_paths.end()) {
97 LocationMap::iterator location_itor = m_path_targets.find(session_id);
98 assert(location_itor != m_path_targets.end());
99
100 if(path_itor->second.empty()) {
101 m_paths.erase(path_itor);
102 m_path_targets.erase(location_itor);
103 return -1;
104 }
105
106 if(!followPath(instance, path_itor->second, distance_to_travel, nextLocation, facingLocation)
107 || !locationsEqual(location_itor->second, target)) {
108 m_paths.erase(path_itor);
109 m_path_targets.erase(location_itor);
110 plan_needed = true;
111 }
112 } else if(!sessionIdValid(session_id)) {
113 //Session id is invalid.
114 return -1;
115 }
116 }
117 if(plan_needed) {
118 if(session_id == -1) {
119 session_id = makeSessionId();
120 }
121 makePlan(instance, target, session_id, priority);
81 } 122 }
82 return session_id; 123 return session_id;
83 } 124 }
84 125
85 void RoutePather::update() { 126 void RoutePather::update() {
86 int ticksleft = m_maxticks; 127 int ticksleft = m_maxticks;
87 while(ticksleft >= 0) { 128 while(ticksleft >= 0) {
88 if(m_sessions.empty()) { 129 if(m_sessions.empty()) {
89 break; 130 break;
94 m_sessions.popElement(); 135 m_sessions.popElement();
95 continue; 136 continue;
96 } 137 }
97 priority_session->updateSearch(); 138 priority_session->updateSearch();
98 if(priority_session->getSearchStatus() == Search::search_status_complete) { 139 if(priority_session->getSearchStatus() == Search::search_status_complete) {
140 const int session_id = priority_session->getSessionId();
99 Path newPath = priority_session->calcPath(); 141 Path newPath = priority_session->calcPath();
100 m_paths.insert(PathMap::value_type(priority_session->getSessionId(), newPath)); 142 newPath.erase(newPath.begin());
101 invalidateSessionId(priority_session->getSessionId()); 143 m_paths.insert(PathMap::value_type(session_id, newPath));
144 invalidateSessionId(session_id);
102 delete priority_session; 145 delete priority_session;
103 m_sessions.popElement(); 146 m_sessions.popElement();
104 } else if(priority_session->getSearchStatus() == Search::search_status_failed) { 147 } else if(priority_session->getSearchStatus() == Search::search_status_failed) {
105 invalidateSessionId(priority_session->getSessionId()); 148 const int session_id = priority_session->getSessionId();
149 invalidateSessionId(session_id);
106 delete priority_session; 150 delete priority_session;
107 m_sessions.popElement(); 151 m_sessions.popElement();
108 } 152 }
109 --ticksleft; 153 --ticksleft;
110 } 154 }
111 } 155 }
112 156
113 void RoutePather::followPath(const Instance* instance, Path& path, double speed, Location& nextLocation, Location& facingLocation) { 157 bool RoutePather::followPath(const Instance* instance, Path& path, double speed, Location& nextLocation, Location& facingLocation) {
114 Location instanceLoc = instance->getLocation(); 158 Location instanceLoc = instance->getLocation();
159 if(!testStep(instance, path)) {
160 return false;
161 }
162
163 if(path.empty()) {
164 return true;
165 }
166
115 ExactModelCoordinate instancePos = instanceLoc.getMapCoordinates(); 167 ExactModelCoordinate instancePos = instanceLoc.getMapCoordinates();
116 ExactModelCoordinate facingPos = path.front().getMapCoordinates(); 168 ExactModelCoordinate facingPos = path.front().getMapCoordinates();
117 facingPos.x = facingPos.x + (facingPos.x - instancePos.x); 169 facingPos.x = facingPos.x + (facingPos.x - instancePos.x);
118 facingPos.y = facingPos.y + (facingPos.y - instancePos.y); 170 facingPos.y = facingPos.y + (facingPos.y - instancePos.y);
119 facingLocation = path.front(); 171 facingLocation = path.front();
126 bool pop = false; 178 bool pop = false;
127 if(speed > distance) { 179 if(speed > distance) {
128 speed = distance; 180 speed = distance;
129 pop = true; 181 pop = true;
130 } 182 }
131 if(distance != 0) 183 if(distance != 0) {
132 {
133 instancePos.x += (dx / distance) * speed; 184 instancePos.x += (dx / distance) * speed;
134 instancePos.y += (dy / distance) * speed; 185 instancePos.y += (dy / distance) * speed;
135
136 } else { 186 } else {
137 pop = true; 187 pop = true;
138 } 188 }
189
139 nextLocation.setMapCoordinates(instancePos); 190 nextLocation.setMapCoordinates(instancePos);
140 if(pop) 191 if(pop) {
141 {
142 path.pop_front(); 192 path.pop_front();
143 if(!path.empty() && instanceLoc.getLayer()->cellContainsBlockingInstance(path.front().getLayerCoordinates())) 193 if(!testStep(instance, path)) {
144 { 194 return false;
145 //TODO: Determine if we're close enough to stop, otherwise find alternate route. 195 }
146 path.clear(); 196 }
147 return; 197 return true;
148 }
149 }
150 } 198 }
151 199
152 bool RoutePather::cancelSession(const int session_id) { 200 bool RoutePather::cancelSession(const int session_id) {
153 if(session_id >= 0) { 201 if(session_id >= 0) {
154 PathMap::iterator i = m_paths.find(session_id); 202 PathMap::iterator i = m_paths.find(session_id);
155 if(i != m_paths.end()) { 203 if(i != m_paths.end()) {
204 LocationMap::iterator j = m_path_targets.find(session_id);
205 assert(j != m_path_targets.end());
156 m_paths.erase(i); 206 m_paths.erase(i);
207 m_path_targets.erase(j);
157 return true; 208 return true;
158 } else { 209 } else {
159 invalidateSessionId(session_id); 210 invalidateSessionId(session_id);
160 } 211 }
161 } 212 }
162 return false; 213 return false;
163 } 214 }
164 215
165 void RoutePather::addSessionId(const int sessionId) { 216 void RoutePather::addSessionId(const int sessionId) {
166 m_registeredSessionIds.push_back(sessionId); 217 m_registeredSessionIds.push_back(sessionId);
167 } 218 }
168 219
169 bool RoutePather::sessionIdValid(const int sessionId) { 220 bool RoutePather::sessionIdValid(const int sessionId) {
170 for(SessionList::const_iterator i = m_registeredSessionIds.begin(); 221 for(SessionList::const_iterator i = m_registeredSessionIds.begin();
171 i != m_registeredSessionIds.end(); 222 i != m_registeredSessionIds.end();
172 ++i) { 223 ++i) {
173 if((*i) == sessionId) { 224 if((*i) == sessionId) {
174 return true; 225 return true;
175 } 226 }
176 } 227 }
177 return false; 228 return false;
178 } 229 }
179 230
180 bool RoutePather::invalidateSessionId(const int sessionId) { 231 bool RoutePather::invalidateSessionId(const int sessionId) {
181 for(SessionList::iterator i = m_registeredSessionIds.begin(); 232 for(SessionList::iterator i = m_registeredSessionIds.begin();
182 i != m_registeredSessionIds.end(); 233 i != m_registeredSessionIds.end();
183 ++i) { 234 ++i) {
184 if((*i) == sessionId) { 235 if((*i) == sessionId) {
185 m_registeredSessionIds.erase(i); 236 m_registeredSessionIds.erase(i);
186 return true; 237 return true;
187 } 238 }
188 } 239 }
189 return false; 240 return false;
190 } 241 }
191 242
192 bool RoutePather::addSearchSpace(SearchSpace* search_space) { 243 bool RoutePather::addSearchSpace(SearchSpace* search_space) {
193 std::pair<SearchSpaceMap::iterator, bool> res = m_searchspaces.insert(SearchSpaceMap::value_type(search_space->getLayer(), search_space)); 244 std::pair<SearchSpaceMap::iterator, bool> res = m_searchspaces.insert(SearchSpaceMap::value_type(search_space->getLayer(), search_space));
194 245
195 return res.second; 246 return res.second;
196 } 247 }
197 248
198 SearchSpace* RoutePather::getSearchSpace(Layer * const layer) { 249 SearchSpace* RoutePather::getSearchSpace(Layer * const layer) {
199 SearchSpaceMap::iterator i = m_searchspaces.find(layer); 250 SearchSpaceMap::iterator i = m_searchspaces.find(layer);
200 if(i == m_searchspaces.end()) { 251 if(i == m_searchspaces.end()) {
201 return 0; 252 return 0;
202 } 253 }