Debian Codegraph @

engine/core/pathfinder/routepather/routepather.cpp

raw
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
31 #include "model/metamodel/grids/cellgrid.h"
32 #include "model/structures/instance.h"
33 #include "model/structures/layer.h"
34 #include "model/structures/cellcache.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
45 int32_t RoutePather::makeSessionId() {
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
57 void RoutePather::update() {
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;
66 m_sessions.popElement();
67 continue;
68 }
69 prioritySession->updateSearch();
70 if (prioritySession->getSearchStatus() == RoutePatherSearch::search_status_complete) {
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;
77 m_sessions.popElement();
78 }
79 } else if (prioritySession->getSearchStatus() == RoutePatherSearch::search_status_failed) {
80 const int32_t sessionId = prioritySession->getSessionId();
81 invalidateSessionId(sessionId);
82 delete prioritySession;
83 m_sessions.popElement();
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)) {
128 route->setRouteStatus(ROUTE_FAILED);
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) {
251 while (newSearch->getSearchStatus() != RoutePatherSearch::search_status_complete) {
252 newSearch->updateSearch();
253 if (newSearch->getSearchStatus() == RoutePatherSearch::search_status_failed) {
254 route->setRouteStatus(ROUTE_FAILED);
255 break;
256 }
257 }
258
259 if (newSearch->getSearchStatus() == RoutePatherSearch::search_status_complete) {
260 newSearch->calcPath();
261 route->setRouteStatus(ROUTE_SOLVED);
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) {
328 nextLocation.setLayerCoordinates(FIFE::doublePt2intPt(current.getExactLayerCoordinates()));
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();
358 ExactModelCoordinate prevPos = route->getPreviousNode().getMapCoordinates();
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 &&
419 currentNode.getLayer()->cellContainsBlockingInstance(route->getCurrentNode().getLayerCoordinates())) {
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
437 int32_t RoutePather::getMaxTicks() {
438 return m_maxTicks;
439 }
440
441 std::string RoutePather::getName() const {
442 return "RoutePather";
443 }
444 }