FIFE  be64c707dea6b3250bd4355bf5c825d25920087d
routepather.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2005-2019 by the FIFE team *
3  * http://www.fifengine.net *
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
33 #include "model/structures/layer.h"
35 #include "util/math/angles.h"
36 #include "pathfinder/route.h"
37 
38 #include "routepather.h"
39 #include "routepathersearch.h"
40 #include "singlelayersearch.h"
41 #include "multilayersearch.h"
42 
43 namespace FIFE {
44 
46  return m_nextFreeSessionId++;
47  }
48 
49  bool RoutePather::locationsEqual(const Location& a, const Location& b) {
50  bool sameLayer = a.getLayer() == b.getLayer();
51  const ModelCoordinate a_coord = a.getLayerCoordinates();
52  const ModelCoordinate b_coord = b.getLayerCoordinates();
53 
54  return (a_coord.x == b_coord.x) && (a_coord.y == b_coord.y) && sameLayer;
55  }
56 
58  int32_t ticksleft = m_maxTicks;
59  while (ticksleft > 0) {
60  if(m_sessions.empty()) {
61  break;
62  }
63  RoutePatherSearch* prioritySession = m_sessions.getPriorityElement().first;
64  if(!sessionIdValid(prioritySession->getSessionId())) {
65  delete prioritySession;
67  continue;
68  }
69  prioritySession->updateSearch();
71  const int32_t sessionId = prioritySession->getSessionId();
72  prioritySession->calcPath();
73  Route* route = prioritySession->getRoute();
74  if (route->getRouteStatus() == ROUTE_SOLVED) {
75  invalidateSessionId(sessionId);
76  delete prioritySession;
78  }
79  } else if (prioritySession->getSearchStatus() == RoutePatherSearch::search_status_failed) {
80  const int32_t sessionId = prioritySession->getSessionId();
81  invalidateSessionId(sessionId);
82  delete prioritySession;
84  }
85  --ticksleft;
86  }
87  }
88 
89  bool RoutePather::cancelSession(const int32_t sessionId) {
90  if (sessionId >= 0) {
91  return invalidateSessionId(sessionId);
92  }
93  return false;
94  }
95 
96  void RoutePather::addSessionId(const int32_t sessionId) {
97  m_registeredSessionIds.push_back(sessionId);
98  }
99 
100  bool RoutePather::sessionIdValid(const int32_t sessionId) {
101  for(SessionList::const_iterator i = m_registeredSessionIds.begin();
102  i != m_registeredSessionIds.end(); ++i) {
103  if((*i) == sessionId) {
104  return true;
105  }
106  }
107  return false;
108  }
109 
110  bool RoutePather::invalidateSessionId(const int32_t sessionId) {
111  for(SessionList::iterator i = m_registeredSessionIds.begin();
112  i != m_registeredSessionIds.end(); ++i) {
113  if((*i) == sessionId) {
114  m_registeredSessionIds.erase(i);
115  return true;
116  }
117  }
118  return false;
119  }
120 
121  Route* RoutePather::createRoute(const Location& start, const Location& end, bool immediate, const std::string& costId) {
122  Route* route = new Route(start, end);
123  if (costId != "") {
124  route->setCostId(costId);
125  }
126  if (immediate) {
127  if (!solveRoute(route, MEDIUM_PRIORITY, true)) {
129  }
130  return route;
131  }
132  return route;
133  }
134 
135  bool RoutePather::solveRoute(Route* route, int32_t priority, bool immediate) {
136  if (sessionIdValid(route->getSessionId())) {
137  return false;
138  }
139 
140  const Location& start = route->getStartNode();
141  const Location& end = route->getEndNode();
142 
143  if (locationsEqual(start, end)) {
144  return false;
145  }
146 
147  CellCache* startCache = start.getLayer()->getCellCache();
148  CellCache* endCache = end.getLayer()->getCellCache();
149 
150  if (!startCache || !endCache) {
151  return false;
152  }
153 
154  if (!startCache->isInCellCache(start) || !endCache->isInCellCache(end)) {
155  return false;
156  }
157 
158  Cell* startCell = startCache->getCell(start.getLayerCoordinates());
159  Cell* endCell = endCache->getCell(end.getLayerCoordinates());
160 
161  bool multilayer = startCache != endCache;
162  if (!multilayer) {
163  Zone* startZone = startCell->getZone();
164  Zone* endZone = endCell->getZone();
165  if (startZone != endZone) {
166  // look for special cases (start is zone border or end is static blocker)
167  if (!endZone || startCell->isZoneProtected()) {
168  bool found = false;
169  const std::vector<Cell*>& neighbors = endCell->getNeighbors();
170  for (std::vector<Cell*>::const_iterator it = neighbors.begin();
171  it != neighbors.end(); ++it) {
172  Zone* tmpZone = (*it)->getZone();
173  if (tmpZone) {
174  endZone = tmpZone;
175  if (tmpZone == startZone) {
176  found = true;
177  break;
178  }
179  }
180  }
181  if (!found && startCell->isZoneProtected()) {
182  const std::vector<Cell*>& neighbors = startCell->getNeighbors();
183  for (std::vector<Cell*>::const_iterator it = neighbors.begin();
184  it != neighbors.end(); ++it) {
185  Zone* tmpZone = (*it)->getZone();
186  if (tmpZone) {
187  if (tmpZone == startZone) {
188  endZone = tmpZone;
189  break;
190  }
191  }
192  }
193  }
194  }
195  // target and all neighbors are static blockers
196  if (!endZone) {
197  return false;
198  }
199  // same CellCache but different zones
200  if (startZone != endZone) {
201  multilayer = true;
202  }
203  }
204  }
205 
206  if (route->isAreaLimited()) {
207  // check if target or neighbors are on one of the areas
208  bool sameAreas = false;
209  const std::list<std::string> areas = route->getLimitedAreas();
210  std::list<std::string>::const_iterator area_it = areas.begin();
211  for (; area_it != areas.end(); ++area_it) {
212  if (endCache->isCellInArea(*area_it, endCell)) {
213  sameAreas = true;
214  break;
215  }
216  }
217  if (!sameAreas) {
218  const std::vector<Cell*>& neighbors = endCell->getNeighbors();
219  if (neighbors.empty()) {
220  return false;
221  }
222  area_it = areas.begin();
223  for (; area_it != areas.end(); ++area_it) {
224  std::vector<Cell*>::const_iterator neigh_it = neighbors.begin();
225  for (; neigh_it != neighbors.end(); ++neigh_it) {
226  if (endCache->isCellInArea(*area_it, *neigh_it)) {
227  sameAreas = true;
228  break;
229  }
230  }
231  }
232  }
233  if (!sameAreas) {
234  return false;
235  }
236  }
237 
238  int32_t sessionId = route->getSessionId();
239  if (sessionId == -1) {
240  sessionId = makeSessionId();
241  route->setSessionId(sessionId);
242  }
243 
244  RoutePatherSearch* newSearch;
245  if (multilayer) {
246  newSearch = new MultiLayerSearch(route, sessionId);
247  } else {
248  newSearch = new SingleLayerSearch(route, sessionId);
249  }
250  if (immediate) {
252  newSearch->updateSearch();
255  break;
256  }
257  }
258 
260  newSearch->calcPath();
262  }
263  delete newSearch;
264  return true;
265  }
266  m_sessions.pushElement(SessionQueue::value_type(newSearch, priority));
267  addSessionId(sessionId);
268  return true;
269  }
270 
271  bool RoutePather::followRoute(const Location& current, Route* route, double speed, Location& nextLocation) {
272  Path path = route->getPath();
273  if (path.empty()) {
274  return false;
275  }
276  if (Mathd::Equal(speed, 0.0)) {
277  return true;
278  }
279  bool nextBlocker = false;
280  Location currentNode = route->getCurrentNode();
281  bool multiCell = route->isMultiCell();
282  if (!locationsEqual(current, currentNode)) {
283  // special blocker check for multicell
284  if (multiCell) {
285  int32_t oldRotation = route->getRotation();
286  // old coordinates
287  std::vector<ModelCoordinate> oldCoords = current.getLayer()->getCellGrid()->
288  toMultiCoordinates(current.getLayerCoordinates(), route->getOccupiedCells(route->getRotation()));
289  oldCoords.push_back(current.getLayerCoordinates());
290  route->setRotation(getAngleBetween(current, currentNode));
291  // new coordinates
292  std::vector<ModelCoordinate> newCoords = currentNode.getLayer()->getCellGrid()->
293  toMultiCoordinates(currentNode.getLayerCoordinates(), route->getOccupiedCells(route->getRotation()));
294  newCoords.push_back(currentNode.getLayerCoordinates());
295 
296  std::vector<ModelCoordinate>::const_iterator nco_it = newCoords.begin();
297  for (; nco_it != newCoords.end(); ++nco_it) {
298  if (currentNode.getLayer()->cellContainsBlockingInstance(*nco_it)) {
299  bool found = false;
300  std::vector<ModelCoordinate>::const_iterator oco_it = oldCoords.begin();
301  for (; oco_it != oldCoords.end(); ++oco_it) {
302  if (*oco_it == *nco_it) {
303  found = true;
304  break;
305  }
306  }
307  // if we have a blocker that is not part of the object
308  if (!found) {
309  nextBlocker = true;
310  }
311  }
312  if (nextBlocker) {
313  route->setRotation(oldRotation);
314  break;
315  }
316  }
317  } else {
318  route->setRotation(getAngleBetween(current, currentNode));
319  if (currentNode.getLayer()->cellContainsBlockingInstance(currentNode.getLayerCoordinates())) {
320  nextBlocker = true;
321  }
322  }
323  }
324  // set facinglocation
325  ExactModelCoordinate instancePos = current.getMapCoordinates();
326  // if next node is blocker
327  if (nextBlocker) {
329  return false;
330  }
331  // calculate distance
332  CellCache* nodeCache = currentNode.getLayer()->getCellCache();
333  CellGrid* nodeGrid = currentNode.getLayer()->getCellGrid();
334  ExactModelCoordinate targetPos = currentNode.getMapCoordinates();
335  Cell* tmpCell = nodeCache->getCell(currentNode.getLayerCoordinates());
336  if (tmpCell) {
337  targetPos.z = tmpCell->getLayerCoordinates().z + nodeGrid->getZShift();
338  }
339  double dx = (targetPos.x - instancePos.x) * nodeGrid->getXScale();
340  double dy = (targetPos.y - instancePos.y) * nodeGrid->getYScale();
341  double distance = Mathd::Sqrt(dx * dx + dy * dy);
342  // cell speed multi
343  double multi;
344  if (nodeCache->getCellSpeedMultiplier(current.getLayerCoordinates(), multi)) {
345  speed *= multi;
346  } else {
347  speed *= nodeCache->getDefaultSpeedMultiplier();
348  }
349  bool pop = false;
350  if (speed > distance) {
351  speed = distance;
352  pop = true;
353  }
354  if (!Mathd::Equal(distance, 0.0) && !pop) {
355  Location prevNode = route->getPreviousNode();
356  CellCache* prevCache = prevNode.getLayer()->getCellCache();
357  CellGrid* prevGrid = prevNode.getLayer()->getCellGrid();
359  tmpCell = prevCache->getCell(prevNode.getLayerCoordinates());
360  if (tmpCell) {
361  prevPos.z = tmpCell->getLayerCoordinates().z + prevGrid->getZShift();
362  }
363  double cell_dz = (targetPos.z - prevPos.z);
364  if (!Mathd::Equal(cell_dz, 0.0)) {
365  double cell_dx = (targetPos.x - prevPos.x);
366  double cell_dy = (targetPos.y - prevPos.y);
367  double cell_distance = Mathd::Sqrt(cell_dx * cell_dx + cell_dy * cell_dy);
368  if (cell_dz > 0) {
369  if (locationsEqual(current, currentNode)) {
370  instancePos.z = targetPos.z;
371  } else {
372  instancePos.z = prevPos.z + cell_dz - 4*(0.5-distance/cell_distance)*(0.5-distance/cell_distance) * cell_dz;
373  }
374  } else if (cell_dz < 0) {
375  if (locationsEqual(current, currentNode)) {
376  instancePos.z = prevPos.z + 4*(0.5-distance/cell_distance)*(0.5-distance/cell_distance) * cell_dz;
377  }
378  }
379  }
380  instancePos.x += (dx / distance) * speed;
381  instancePos.y += (dy / distance) * speed;
382  } else {
383  pop = true;
384  }
385  // pop to next node
386  if (pop) {
387  nextLocation.setMapCoordinates(targetPos);
388  // if cw is false we have reached the end
389  bool cw = route->walkToNextNode();
390  // check transistion
391  CellCache* cache = nextLocation.getLayer()->getCellCache();
392  if (cache) {
393  Cell* cell = cache->getCell(nextLocation.getLayerCoordinates());
394  if (cell) {
395  TransitionInfo* ti = cell->getTransition();
396  if (ti) {
397  // "beam" if it is a part of path
398  if (cw &&
399  !cell->getLayer()->getCellGrid()->isAccessible(nextLocation.getLayerCoordinates(),
400  route->getCurrentNode().getLayerCoordinates())) {
401  if (ti->m_difflayer) {
402  nextLocation.setLayer(ti->m_layer);
403  }
404  nextLocation.setLayerCoordinates(ti->m_mc);
405  return cw;
406  // immediate "beam"
407  } else if (ti->m_immediate) {
408  if (ti->m_difflayer) {
409  nextLocation.setLayer(ti->m_layer);
410  }
411  nextLocation.setLayerCoordinates(ti->m_mc);
412  route->setEndNode(nextLocation);
413  return false;
414  }
415  }
416  }
417  }
418  if (cw && !multiCell &&
420  //set facing to end blocker
421  Location facing = route->getCurrentNode();
422  route->setRotation(getAngleBetween(current, facing));
423 
424  return false;
425  }
426  return cw;
427  }
428  nextLocation.setMapCoordinates(instancePos);
429 
430  return true;
431  }
432 
433  void RoutePather::setMaxTicks(int32_t ticks) {
434  m_maxTicks = ticks;
435  }
436 
438  return m_maxTicks;
439  }
440 
441  std::string RoutePather::getName() const {
442  return "RoutePather";
443  }
444 }
ExactModelCoordinate getExactLayerCoordinates() const
Gets exact layer coordinates set to this location.
Definition: location.cpp:109
int32_t getSearchStatus() const
A small function which returns the current status of the search.
int32_t m_nextFreeSessionId
The next free session id.
Definition: routepather.h:170
int32_t getAngleBetween(const Location &loc1, const Location &loc2)
Gets angle of vector defined by given locations.
Definition: angles.cpp:98
void setLayerCoordinates(const ModelCoordinate &coordinates)
Sets "cell precise" layer coordinates to this location.
Definition: location.cpp:94
Layer * m_layer
target layer
Definition: cell.h:72
bool locationsEqual(const Location &a, const Location &b)
Are two locations equivalent from the perspective of pathing (same layer coordinates and layer)...
Definition: routepather.cpp:49
Route * getRoute()
Returns the associated route for this search.
bool m_difflayer
is target on another layer
Definition: cell.h:78
const Location & getPreviousNode()
Returns previous location.
Definition: route.cpp:108
static T Sqrt(T _val)
Definition: fife_math.h:277
bool empty(void) const
Determines whether the queue is currently empty.
Layer * getLayer() const
Gets the layer where this location is pointing to.
Definition: location.cpp:83
const Location & getStartNode()
Returns the start location.
Definition: route.cpp:78
Layer * getLayer()
Returns the current layer.
Definition: cell.cpp:335
bool walkToNextNode(int32_t step=1)
Changes the position on the path.
Definition: route.cpp:137
bool isCellInArea(const std::string &id, Cell *cell)
Returns true if cell is part of the area, otherwise false.
Definition: cellcache.cpp:1491
void setRouteStatus(RouteStatusInfo status)
Sets route status.
Definition: route.cpp:57
void setLayer(Layer *layer)
Sets layer where this location is pointing to.
Definition: location.cpp:79
A CellCache is an abstract depiction of one or a few layers and contains additional information...
Definition: cellcache.h:111
bool solveRoute(Route *route, int32_t priority=MEDIUM_PRIORITY, bool immediate=false)
Solves the route to create a path.
void setMapCoordinates(const ExactModelCoordinate &coordinates)
Sets map coordinates to this location.
Definition: location.cpp:98
CellCache * getCellCache()
Returns the CellCache of this layer.
Definition: layer.cpp:573
std::list< Location > Path
A path is a list with locations. Each location holds the coordinate for one cell. ...
Definition: routepather.h:117
const Location & getEndNode()
Returns the target location.
Definition: route.cpp:94
bool sessionIdValid(const int32_t sessionId)
Determines if the given session Id is valid.
void pushElement(const value_type &element)
Pushes a new element onto the queue.
bool invalidateSessionId(const int32_t sessionId)
Removes a session id from the session map.
MultiLayerSearch using A*.
SingleLayerSearch using A*.
const double getZShift() const
Get the cellgrid z shift.
Definition: cellgrid.h:176
A basic route.
Definition: route.h:64
bool followRoute(const Location &current, Route *route, double speed, Location &nextLocation)
Follows the path of the route.
const std::list< std::string > getLimitedAreas()
Definition: route.cpp:286
int32_t makeSessionId()
Makes a new session id.
Definition: routepather.cpp:45
void setRotation(int32_t rotation)
Sets the current rotation.
Definition: route.cpp:231
Simple class to hold the data for transistions.
Definition: cell.h:69
static bool Equal(T _val1, T _val2)
Definition: fife_math.h:287
double getDefaultSpeedMultiplier()
Gets default speed for this CellCache.
Definition: cellcache.cpp:1165
Cell * getCell(const ModelCoordinate &mc)
Returns cell on this coordinate.
Definition: cellcache.cpp:704
int32_t m_maxTicks
The maximum number of ticks allowed.
Definition: routepather.h:173
bool isZoneProtected()
Returns whether the zone on this cell is protected.
Definition: cell.cpp:282
int32_t getSessionId()
Returns the session identifier.
Definition: route.cpp:227
void setCostId(const std::string &cost)
Sets cost identifier which should be used for pathfinding.
Definition: route.cpp:239
bool isAreaLimited()
Definition: route.cpp:277
Point doublePt2intPt(DoublePoint pt)
Convert from 2D double point to 2D int32_t point.
Definition: point.h:335
const ModelCoordinate getLayerCoordinates() const
Returns the layer coordinates of this cell.
Definition: cell.cpp:310
virtual void calcPath()=0
Calculates final path.
std::string getName() const
Returns name of the pathfinder.
virtual void updateSearch()=0
Updates the search.
RouteStatusInfo getRouteStatus()
Returns route status.
Definition: route.cpp:63
const value_type getPriorityElement(void) const
Retrieves the element with the highest priority.
Definition: priorityqueue.h:99
const std::vector< Cell * > & getNeighbors()
Returns the layer coordinates of this cell.
Definition: cell.cpp:318
A basic cell on a CellCache.
Definition: cell.h:123
int32_t getSessionId() const
Retrieves the session id.
std::pair< RoutePatherSearch *, int32_t > value_type
Definition: priorityqueue.h:46
std::iterator_traits< octet_iterator >::difference_type distance(octet_iterator first, octet_iterator last)
Definition: checked.h:198
bool getCellSpeedMultiplier(const ModelCoordinate &cell, double &multiplier)
Returns speed value from cell.
Definition: cellcache.cpp:1141
Route * createRoute(const Location &start, const Location &end, bool immediate=false, const std::string &costId="")
Creates a route between the start and end location that needs be solved.
bool cancelSession(const int32_t sessionId)
Cancels a session.
Definition: routepather.cpp:89
const double getXScale() const
Get the cellgrid x-scaling.
Definition: cellgrid.h:205
Zone * getZone()
Returns zone.
Definition: cell.cpp:261
Path getPath()
Returns the path.
Definition: route.cpp:177
CellGrid * getCellGrid() const
Get the Cellgrid.
Definition: layer.cpp:93
const double getYScale() const
Get the cellgrid y-scaling.
Definition: cellgrid.h:210
void addSessionId(const int32_t sessionId)
Adds a session id to the session map.
Definition: routepather.cpp:96
void popElement(void)
Pops the element with the highest priority from the queue.
A 3D Point.
Definition: point.h:205
SessionList m_registeredSessionIds
A list of session ids that have been registered.
Definition: routepather.h:167
RoutePatherSearch using A*.
bool isInCellCache(const Location &location) const
Checks whether the location is in CellCache range.
Definition: cellcache.cpp:820
std::vector< ModelCoordinate > getOccupiedCells(int32_t rotation)
Returns relative coordinates for multi cell object based on rotation.
Definition: route.cpp:262
bool cellContainsBlockingInstance(const ModelCoordinate &cellCoordinate)
Determines if a given cell on the layer contains a blocking instance.
Definition: layer.cpp:462
ExactModelCoordinate getMapCoordinates() const
Gets map coordinates set to this location.
Definition: location.cpp:117
ModelCoordinate getLayerCoordinates() const
Gets cell precision layer coordinates set to this location.
Definition: location.cpp:113
const Location & getCurrentNode()
Returns current location.
Definition: route.cpp:98
bool isMultiCell()
Gets if path is for a multi cell object.
Definition: route.cpp:247
int32_t getMaxTicks()
Returns maximal ticks (update steps) to solve routes.
void setEndNode(const Location &node)
Sets the target location.
Definition: route.cpp:82
int32_t getRotation()
Returns the current rotation.
Definition: route.cpp:235
TransitionInfo * getTransition()
Returns the transition.
Definition: cell.cpp:380
SessionQueue m_sessions
A map of currently running sessions (searches).
Definition: routepather.h:164
void update()
Updates the route pather.
Definition: routepather.cpp:57
virtual bool isAccessible(const ModelCoordinate &curpos, const ModelCoordinate &target)=0
Tells if given target point is accessible from curpos only cells adjacent to curpos are considered in...
bool m_immediate
use immediate
Definition: cell.h:80
void setMaxTicks(int32_t ticks)
Sets maximal ticks (update steps) to solve routes.
A Zone is an abstract depiction of a CellCache or of a part of it.
Definition: cellcache.h:50
ModelCoordinate m_mc
target coordinates
Definition: cell.h:76
void setSessionId(int32_t id)
Sets the session identifier.
Definition: route.cpp:223