FIFE  2008.0
 All Classes Namespaces Functions Variables Enumerations Enumerator Pages
routepather.cpp
1 /***************************************************************************
2  * Copyright (C) 2005-2008 by the FIFE team *
3  * http://www.fifengine.de *
4  * This file is part of FIFE. *
5  * *
6  * FIFE is free software; you can redistribute it and/or *
7  * modify it under the terms of the GNU Lesser General Public *
8  * License as published by the Free Software Foundation; either *
9  * version 2.1 of the License, or (at your option) any later version. *
10  * *
11  * This library is distributed in the hope that it will be useful, *
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
14  * Lesser General Public License for more details. *
15  * *
16  * You should have received a copy of the GNU Lesser General Public *
17  * License along with this library; if not, write to the *
18  * Free Software Foundation, Inc., *
19  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
20  ***************************************************************************/
21 
22 // Standard C++ library includes
23 #include <cassert>
24 
25 // 3rd party library includes
26 
27 // FIFE includes
28 // These includes are split up in two parts, separated by one empty line
29 // First block: files included from the FIFE root src directory
30 // Second block: files included from the same folder
31 #include "model/metamodel/grids/cellgrid.h"
32 #include "model/structures/instance.h"
33 #include "model/structures/layer.h"
34 
35 #include "pathfinder/searchspace.h"
36 
37 #include "routepather.h"
38 #include "routepathersearch.h"
39 
40 namespace FIFE {
41  void RoutePather::setMap(Map* map) {
42  if(!map) {
43  return;
44  }
45  m_map = map;
46  }
47 
48  int RoutePather::makeSessionId() {
49  return m_nextFreeSessionId++;
50  }
51 
52  void RoutePather::makePlan(const Instance *instance, const Location& target, int session_id, int priority) {
53  SearchSpace* searchspace = getSearchSpace(target.getLayer());
54  if(!searchspace) {
55  searchspace = new SearchSpace(target.getLayer());
56  addSearchSpace(searchspace);
57  }
58  if(searchspace->isInSearchSpace(target)) {
59  RoutePatherSearch* newSearch = new RoutePatherSearch(session_id, instance->getLocation(), target, searchspace);
60  m_sessions.pushElement(SessionQueue::value_type(newSearch, priority));
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);
122  }
123  return session_id;
124  }
125 
126  void RoutePather::update() {
127  int ticksleft = m_maxticks;
128  while(ticksleft >= 0) {
129  if(m_sessions.empty()) {
130  break;
131  }
132  RoutePatherSearch* priority_session = m_sessions.getPriorityElement().first;
133  if(!sessionIdValid(priority_session->getSessionId())) {
134  delete priority_session;
135  m_sessions.popElement();
136  continue;
137  }
138  priority_session->updateSearch();
139  if(priority_session->getSearchStatus() == RoutePatherSearch::search_status_complete) {
140  const int session_id = priority_session->getSessionId();
141  Path newPath = priority_session->calcPath();
142  newPath.erase(newPath.begin());
143  m_paths.insert(PathMap::value_type(session_id, newPath));
144  invalidateSessionId(session_id);
145  delete priority_session;
146  m_sessions.popElement();
147  } else if(priority_session->getSearchStatus() == RoutePatherSearch::search_status_failed) {
148  const int session_id = priority_session->getSessionId();
149  invalidateSessionId(session_id);
150  delete priority_session;
151  m_sessions.popElement();
152  }
153  --ticksleft;
154  }
155  }
156 
157  bool RoutePather::followPath(const Instance* instance, Path& path, double speed, Location& nextLocation, Location& facingLocation) {
158  Location instanceLoc = instance->getLocation();
159  if(!testStep(instance, path)) {
160  return false;
161  }
162 
163  if(path.empty()) {
164  return true;
165  }
166 
167  ExactModelCoordinate instancePos = instanceLoc.getMapCoordinates();
168  ExactModelCoordinate facingPos = path.front().getMapCoordinates();
169  facingPos.x = facingPos.x + (facingPos.x - instancePos.x);
170  facingPos.y = facingPos.y + (facingPos.y - instancePos.y);
171  facingLocation = path.front();
172  facingLocation.setMapCoordinates(facingPos);
173  ExactModelCoordinate targetPos = path.front().getMapCoordinates();
174  CellGrid* grid = instanceLoc.getLayer()->getCellGrid();
175  double dx = (targetPos.x - instancePos.x) * grid->getXScale();
176  double dy = (targetPos.y - instancePos.y) * grid->getYScale();
177  double distance = sqrt(dx * dx + dy * dy);
178  bool pop = false;
179  if(speed > distance) {
180  speed = distance;
181  pop = true;
182  }
183  if(distance != 0) {
184  instancePos.x += (dx / distance) * speed;
185  instancePos.y += (dy / distance) * speed;
186  } else {
187  pop = true;
188  }
189 
190  nextLocation.setMapCoordinates(instancePos);
191  if(pop) {
192  path.pop_front();
193  if(!testStep(instance, path)) {
194  return false;
195  }
196  }
197  return true;
198  }
199 
200  bool RoutePather::cancelSession(const int session_id) {
201  if(session_id >= 0) {
202  PathMap::iterator i = m_paths.find(session_id);
203  if(i != m_paths.end()) {
204  LocationMap::iterator j = m_path_targets.find(session_id);
205  assert(j != m_path_targets.end());
206  m_paths.erase(i);
207  m_path_targets.erase(j);
208  return true;
209  } else {
210  invalidateSessionId(session_id);
211  }
212  }
213  return false;
214  }
215 
216  void RoutePather::addSessionId(const int sessionId) {
217  m_registeredSessionIds.push_back(sessionId);
218  }
219 
220  bool RoutePather::sessionIdValid(const int sessionId) {
221  for(SessionList::const_iterator i = m_registeredSessionIds.begin();
222  i != m_registeredSessionIds.end();
223  ++i) {
224  if((*i) == sessionId) {
225  return true;
226  }
227  }
228  return false;
229  }
230 
231  bool RoutePather::invalidateSessionId(const int sessionId) {
232  for(SessionList::iterator i = m_registeredSessionIds.begin();
233  i != m_registeredSessionIds.end();
234  ++i) {
235  if((*i) == sessionId) {
236  m_registeredSessionIds.erase(i);
237  return true;
238  }
239  }
240  return false;
241  }
242 
243  bool RoutePather::addSearchSpace(SearchSpace* search_space) {
244  std::pair<SearchSpaceMap::iterator, bool> res = m_searchspaces.insert(SearchSpaceMap::value_type(search_space->getLayer(), search_space));
245 
246  return res.second;
247  }
248 
249  SearchSpace* RoutePather::getSearchSpace(Layer * const layer) {
250  SearchSpaceMap::iterator i = m_searchspaces.find(layer);
251  if(i == m_searchspaces.end()) {
252  return 0;
253  }
254  return i->second;
255  }
256 }