Debian Codegraph @

engine/core/pathfinder/routepather/multilayersearch.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 <algorithm>
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/layer.h"
33 #include "model/structures/cellcache.h"
34 #include "model/structures/cell.h"
35 #include "model/structures/map.h"
36 #include "pathfinder/route.h"
37 #include "util/math/fife_math.h"
38
39 #include "multilayersearch.h"
40
41 namespace FIFE {
42 MultiLayerSearch::MultiLayerSearch(Route* route, const int32_t sessionId):
43 RoutePatherSearch(route, sessionId),
44 m_to(route->getEndNode()),
45 m_from(route->getStartNode()),
46 m_startCache(m_from.getLayer()->getCellCache()),
47 m_endCache(m_to.getLayer()->getCellCache()),
48 m_currentCache(NULL),
49 m_startZone(m_startCache->getCell(m_from.getLayerCoordinates())->getZone()),
50 m_endZone(m_endCache->getCell(m_to.getLayerCoordinates())->getZone()),
51 m_startCoordInt(m_startCache->convertCoordToInt(m_from.getLayerCoordinates())),
52 m_lastStartCoordInt(m_startCoordInt),
53 m_destCoordInt(m_endCache->convertCoordToInt(m_to.getLayerCoordinates())),
54 m_lastDestCoordInt(-1),
55 m_next(0),
56 m_foundLast(true) {
57
58 // if end zone is invalid (static blocker) then change it
59 if (!m_endZone) {
60 Cell* endcell = m_endCache->getCell(m_to.getLayerCoordinates());
61 const std::vector<Cell*>& neighbors = endcell->getNeighbors();
62 for (std::vector<Cell*>::const_iterator it = neighbors.begin();
63 it != neighbors.end(); ++it) {
64 Zone* tmpzone = (*it)->getZone();
65 if (tmpzone) {
66 m_endZone = tmpzone;
67 if (tmpzone == m_startZone) {
68 break;
69 }
70 }
71 }
72 }
73 Cell* startCell = m_startCache->getCell(m_from.getLayerCoordinates());
74
75 // here we hope to find between targets
76 // first test if target can be reached easy
77 searchBetweenTargetsNeighbor();
78 // if no betweenTargets could be found we check as second the whole map
79 if (m_betweenTargets.empty()) {
80 searchBetweenTargetsMap();
81 }
82 // if it is a protected cell it can have a second startzone
83 if (m_betweenTargets.empty() && startCell->isZoneProtected()) {
84 const std::vector<Cell*>& neighbors = startCell->getNeighbors();
85 for (std::vector<Cell*>::const_iterator it = neighbors.begin();
86 it != neighbors.end(); ++it) {
87 Zone* tmpzone = (*it)->getZone();
88 if (tmpzone) {
89 if (tmpzone != m_startZone) {
90 m_startZone = tmpzone;
91 break;
92 }
93 }
94 }
95 searchBetweenTargetsNeighbor();
96 if (m_betweenTargets.empty()) {
97 searchBetweenTargetsMap();
98 }
99 }
100 // failed to find between targets, no Path can be created
101 if (m_betweenTargets.empty()) {
102 setSearchStatus(search_status_failed);
103 m_route->setRouteStatus(ROUTE_FAILED);
104 }
105 }
106
107 MultiLayerSearch::~MultiLayerSearch() {
108 }
109
110 void MultiLayerSearch::createSearchFrontier(int32_t startInt, CellCache* cache) {
111 // reset all
112 m_sortedFrontier.clear();
113 m_spt.clear();
114 m_sf.clear();
115 m_gCosts.clear();
116 // fill with defaults
117 m_sortedFrontier.pushElement(PriorityQueue<int32_t, double>::value_type(startInt, 0.0));
118 int32_t max_index = cache->getMaxIndex();
119 m_spt.resize(max_index, -1);
120 m_sf.resize(max_index, -1);
121 m_gCosts.resize(max_index, 0.0);
122 m_next = 0;
123 }
124
125 void MultiLayerSearch::updateSearch() {
126 if (m_sortedFrontier.empty()) {
127 if (!m_foundLast || m_lastDestCoordInt == m_destCoordInt || getSearchStatus() == search_status_failed) {
128 setSearchStatus(search_status_failed);
129 m_route->setRouteStatus(ROUTE_FAILED);
130 return;
131 }
132 if (m_betweenTargets.empty()) {
133 TransitionInfo* trans = m_currentCache->getCell(m_currentCache->convertIntToCoord(m_lastDestCoordInt))->getTransition();
134 if (trans) {
135 m_lastStartCoordInt = m_endCache->convertCoordToInt(trans->m_mc);
136 }
137 m_currentCache = m_endCache;
138 m_lastDestCoordInt = m_currentCache->getCell(m_to.getLayerCoordinates())->getCellId();
139 } else {
140 if (m_lastDestCoordInt != -1) {
141 TransitionInfo* trans = m_currentCache->getCell(m_currentCache->convertIntToCoord(m_lastDestCoordInt))->getTransition();
142 if (trans) {
143 m_lastStartCoordInt = trans->m_layer->getCellCache()->convertCoordToInt(trans->m_mc);
144 }
145 }
146 m_currentCache = m_betweenTargets.front()->getLayer()->getCellCache();
147 m_lastDestCoordInt = m_betweenTargets.front()->getCellId();
148 m_betweenTargets.pop_front();
149 m_foundLast = false;
150 }
151 createSearchFrontier(m_lastStartCoordInt, m_currentCache);
152 }
153
154 PriorityQueue<int32_t, double>::value_type topvalue = m_sortedFrontier.getPriorityElement();
155 m_sortedFrontier.popElement();
156 m_next = topvalue.first;
157 m_spt[m_next] = m_sf[m_next];
158 // found destination
159 if (m_destCoordInt == m_next && m_betweenTargets.empty()) {
160 if (m_endCache == m_currentCache) {
161 setSearchStatus(search_status_complete);
162 m_route->setRouteStatus(ROUTE_SEARCHED);
163 return;
164 }
165 }
166
167 // found between target
168 if (m_lastDestCoordInt == m_next) {
169 calcPathStep();
170 m_sortedFrontier.clear();
171 m_foundLast = true;
172 return;
173 }
174
175 ModelCoordinate destCoord = m_currentCache->convertIntToCoord(m_lastDestCoordInt);
176 ModelCoordinate nextCoord = m_currentCache->convertIntToCoord(m_next);
177 CellGrid* grid = m_currentCache->getLayer()->getCellGrid();
178 Cell* nextCell = m_currentCache->getCell(nextCoord);
179 if (!nextCell) {
180 return;
181 }
182 int32_t cellZ = nextCell->getLayerCoordinates().z;
183 int32_t maxZ = m_route->getZStepRange();
184 bool zLimited = maxZ != -1;
185 uint8_t blockerThreshold = m_ignoreDynamicBlockers ? 2 : 1;
186 bool limitedArea = m_route->isAreaLimited();
187 const std::vector<Cell*>& adjacents = nextCell->getNeighbors();
188 if (adjacents.empty()) {
189 return;
190 }
191 for (std::vector<Cell*>::const_iterator i = adjacents.begin(); i != adjacents.end(); ++i) {
192 if (*i == NULL) {
193 continue;
194 }
195 if ((*i)->getLayer()->getCellCache() != m_currentCache) {
196 continue;
197 }
198 int32_t adjacentInt = (*i)->getCellId();
199 if (m_sf[adjacentInt] != -1 && m_spt[adjacentInt] != -1) {
200 continue;
201 }
202 if (zLimited && ABS(cellZ-(*i)->getLayerCoordinates().z) > maxZ) {
203 continue;
204 }
205 bool blocker = (*i)->getCellType() > blockerThreshold;
206 ModelCoordinate adjacentCoord = (*i)->getLayerCoordinates();
207 if ((adjacentInt == m_next || blocker) && adjacentInt != m_destCoordInt) {
208 if (blocker && m_multicell) {
209 std::vector<Cell*>::iterator bc_it = std::find(m_ignoredBlockers.begin(), m_ignoredBlockers.end(), *i);
210 if (bc_it == m_ignoredBlockers.end()) {
211 continue;
212 }
213 } else {
214 continue;
215 }
216 }
217 // search if there are blockers which could block multicell object
218 if (m_multicell) {
219 blocker = false;
220 Location currentLoc(nextCell->getLayer());
221 currentLoc.setLayerCoordinates(nextCell->getLayerCoordinates());
222 Location adjacentLoc((*i)->getLayer());
223 adjacentLoc.setLayerCoordinates((*i)->getLayerCoordinates());
224
225 int32_t rotation = getAngleBetween(currentLoc, adjacentLoc);
226 std::vector<ModelCoordinate> coords = grid->toMultiCoordinates(adjacentLoc.getLayerCoordinates(), m_route->getOccupiedCells(rotation));
227 std::vector<ModelCoordinate>::iterator coord_it = coords.begin();
228 for (; coord_it != coords.end(); ++coord_it) {
229 Cell* cell = m_currentCache->getCell(*coord_it);
230 if (cell) {
231 if (cell->getCellType() > blockerThreshold) {
232 std::vector<Cell*>::iterator bc_it = std::find(m_ignoredBlockers.begin(), m_ignoredBlockers.end(), cell);
233 if (bc_it == m_ignoredBlockers.end()) {
234 blocker = true;
235 break;
236 }
237 }
238 if (limitedArea) {
239 // check if cell is on one of the areas
240 bool sameAreas = false;
241 const std::list<std::string> areas = m_route->getLimitedAreas();
242 std::list<std::string>::const_iterator area_it = areas.begin();
243 for (; area_it != areas.end(); ++area_it) {
244 if (m_currentCache->isCellInArea(*area_it, cell)) {
245 sameAreas = true;
246 break;
247 }
248 }
249 if (!sameAreas) {
250 blocker = true;
251 break;
252 }
253 }
254 } else {
255 blocker = true;
256 break;
257 }
258 }
259 if (blocker) {
260 continue;
261 }
262 } else if (limitedArea) {
263 // check if cell is on one of the areas
264 bool sameAreas = false;
265 const std::list<std::string> areas = m_route->getLimitedAreas();
266 std::list<std::string>::const_iterator area_it = areas.begin();
267 for (; area_it != areas.end(); ++area_it) {
268 if (m_currentCache->isCellInArea(*area_it, *i)) {
269 sameAreas = true;
270 break;
271 }
272 }
273 if (!sameAreas) {
274 continue;
275 }
276 }
277
278 double gCost = m_gCosts[m_next];
279 if (m_specialCost) {
280 gCost += m_currentCache->getAdjacentCost(adjacentCoord ,nextCoord, m_route->getCostId());
281 } else {
282 gCost += m_currentCache->getAdjacentCost(adjacentCoord ,nextCoord);
283 }
284 double hCost = grid->getHeuristicCost(adjacentCoord, destCoord);
285 if (m_sf[adjacentInt] == -1) {
286 m_sortedFrontier.pushElement(PriorityQueue<int32_t, double>::value_type(adjacentInt, gCost + hCost));
287 m_gCosts[adjacentInt] = gCost;
288 m_sf[adjacentInt] = m_next;
289 } else if (gCost < m_gCosts[adjacentInt] && m_spt[adjacentInt] == -1) {
290 m_sortedFrontier.changeElementPriority(adjacentInt, gCost + hCost);
291 m_gCosts[adjacentInt] = gCost;
292 m_sf[adjacentInt] = m_next;
293 }
294 }
295 }
296
297 void MultiLayerSearch::calcPathStep() {
298 int32_t current = m_lastDestCoordInt;
299 int32_t end = m_lastStartCoordInt;
300 Location newnode(m_currentCache->getLayer());
301 Path path;
302 // This assures that the agent always steps into the center of the target cell.
303 newnode.setLayerCoordinates(m_currentCache->convertIntToCoord(current));
304 path.push_back(newnode);
305 while(current != end) {
306 if (m_spt[current] < 0 ) {
307 // This is when the size of m_spt can not handle the distance of the location
308 setSearchStatus(search_status_failed);
309 m_route->setRouteStatus(ROUTE_FAILED);
310 break;
311 }
312 current = m_spt[current];
313 newnode.setLayerCoordinates(m_currentCache->convertIntToCoord(current));
314 path.push_front(newnode);
315 }
316 // set exact start coordinates
317 if (m_path.empty()) {
318 path.front().setExactLayerCoordinates(m_from.getExactLayerCoordinatesRef());
319 }
320 m_path.insert(m_path.end(), path.begin(), path.end());
321 }
322
323 void MultiLayerSearch::calcPath() {
324 int32_t current = m_lastDestCoordInt;
325 int32_t end = m_lastStartCoordInt;
326 Location newnode(m_currentCache->getLayer());
327 Path path;
328 // This assures that the agent always steps into the center of the target cell.
329 newnode.setLayerCoordinates(
330 m_currentCache->getCell(m_currentCache->convertIntToCoord(current))->getLayerCoordinates());
331 path.push_back(newnode);
332 while(current != end) {
333 if (m_spt[current] < 0 ) {
334 // This is when the size of m_spt can not handle the distance of the location
335 setSearchStatus(search_status_failed);
336 m_route->setRouteStatus(ROUTE_FAILED);
337 break;
338 }
339 current = m_spt[current];
340 newnode.setLayerCoordinates(m_currentCache->convertIntToCoord(current));
341 path.push_front(newnode);
342 }
343 m_path.insert(m_path.end(), path.begin(), path.end());
344 m_route->setPath(m_path);
345 }
346
347 void MultiLayerSearch::searchBetweenTargetsNeighbor() {
348 std::vector<Cell*> cells = m_startCache->getTransitionCells(m_endCache->getLayer());
349 if (!cells.empty()) {
350 Location loc;
351 Cell* cell = NULL;
352 // find nearest portal (air-line distance)
353 for (std::vector<Cell*>::iterator it = cells.begin(); it != cells.end(); ++it) {
354 if ((*it)->getZone() != m_startZone) {
355 continue;
356 }
357 TransitionInfo* trans = (*it)->getTransition();
358 Cell* transTarget = trans->m_layer->getCellCache()->getCell(trans->m_mc);
359 if (transTarget->getZone() != m_endZone) {
360 continue;
361 }
362 if (!cell) {
363 loc.setLayer((*it)->getLayer());
364 loc.setLayerCoordinates((*it)->getLayerCoordinates());
365 cell = *it;
366 continue;
367 }
368 Location temp((*it)->getLayer());
369 temp.setLayerCoordinates((*it)->getLayerCoordinates());
370 Location loc1(cell->getTransition()->m_layer);
371 loc1.setLayerCoordinates(cell->getTransition()->m_mc);
372 Location loc2((*it)->getTransition()->m_layer);
373 loc2.setLayerCoordinates((*it)->getTransition()->m_mc);
374
375 double temp_distance = temp.getLayerDistanceTo(m_from) + loc2.getLayerDistanceTo(m_to);
376 double current_distance = loc.getLayerDistanceTo(m_from) + loc1.getLayerDistanceTo(m_to);
377 if (current_distance > temp_distance) {
378 loc = temp;
379 cell = *it;
380 }
381 }
382 if (cell) {
383 m_betweenTargets.push_back(cell);
384 }
385 }
386 }
387
388 void MultiLayerSearch::searchBetweenTargetsMap() {
389 // in case no transition is there then return
390 std::vector<Cell*> endTransCells;
391 std::vector<Cell*> tmpTransCells = m_endCache->getTransitionCells();
392 std::vector<Cell*>::iterator tcell_it = tmpTransCells.begin();
393 for (; tcell_it != tmpTransCells.end(); ++tcell_it) {
394 Zone* zone = (*tcell_it)->getZone();
395 if (zone == m_endZone) {
396 endTransCells.push_back(*tcell_it);
397 }
398 }
399 if (endTransCells.empty()) {
400 return;
401 }
402 // in case no transition is there then return
403 std::vector<Cell*> startTransCells;
404 tmpTransCells = m_startCache->getTransitionCells();
405 tcell_it = tmpTransCells.begin();
406 for (; tcell_it != tmpTransCells.end(); ++tcell_it) {
407 Zone* zone = (*tcell_it)->getZone();
408 if (zone == m_startZone) {
409 startTransCells.push_back(*tcell_it);
410 }
411 }
412 if (startTransCells.empty()) {
413 return;
414 }
415
416 // fetch zones
417 std::vector<Zone*> zones;
418 const std::list<Layer*>& allLayers = m_from.getLayer()->getMap()->getLayers();
419 std::list<Layer*>::const_iterator lay_it = allLayers.begin();
420 for (; lay_it != allLayers.end(); ++lay_it) {
421 CellCache* cache = (*lay_it)->getCellCache();
422 if (cache) {
423 const std::vector<Zone*>& tmp_zones = cache->getZones();
424 zones.insert(zones.end(), tmp_zones.begin(), tmp_zones.end());
425 }
426 }
427
428 // sort zones by iterator distance
429 std::map<Zone*, int32_t> zoneDistanceMap;
430 std::map<int32_t, Zone*> distanceZoneMap;
431 for (std::vector<Zone*>::iterator zit = zones.begin(); zit != zones.end(); ++zit) {
432 // pseudo distance
433 int32_t distance = std::distance(zones.begin(), zit);
434 zoneDistanceMap.insert(std::pair<Zone*, int32_t>(*zit, distance));
435 distanceZoneMap.insert(std::pair<int32_t, Zone*>(distance, *zit));
436 }
437 // start zone
438 int32_t startZone = zoneDistanceMap.find(m_startZone)->second;
439 // target zone
440 int32_t targetZone = zoneDistanceMap.find(m_endZone)->second;
441 // Priority queue to sort zones
442 PriorityQueue<int32_t, double> sortedfrontier;
443 // add start zone
444 sortedfrontier.pushElement(PriorityQueue<int32_t, double>::value_type(startZone, 0.0));
445 // max size zones
446 int32_t max_index = zones.size();
447 // shortest tree
448 std::vector<int32_t> spt(max_index, -1);
449 // search frontier
450 std::vector<int32_t> sf(max_index, -1);
451 // costs
452 std::vector<double> costs(max_index, 0.0);
453 bool found = false;
454 while (!found) {
455 if (sortedfrontier.empty()) {
456 break;
457 }
458 PriorityQueue<int32_t, double>::value_type topvalue = sortedfrontier.getPriorityElement();
459 sortedfrontier.popElement();
460 int32_t next = topvalue.first;
461 spt[next] = sf[next];
462 // found destination zone
463 if (targetZone == next) {
464 found = true;
465 break;
466 }
467 Zone* nextZone = distanceZoneMap.find(next)->second;
468 // look in zone for tranistions
469 std::vector<Cell*> transCells = nextZone->getTransitionCells();
470 if (transCells.empty()) {
471 continue;
472 }
473 std::vector<Cell*>::iterator cell_it = transCells.begin();
474 for (; cell_it != transCells.end(); ++cell_it) {
475 // transition info
476 TransitionInfo* trans = (*cell_it)->getTransition();
477 // target transition cache
478 CellCache* transCache = trans->m_layer->getCellCache();
479 // target transition cell
480 Cell* transCelle = transCache->getCell(trans->m_mc);
481 // next zone as int
482 int32_t nextInt = zoneDistanceMap.find(transCelle->getZone())->second;
483 if (nextInt == next && nextInt != targetZone) {
484 continue;
485 }
486 // iterator distance as cost
487 double cost = costs[next] + static_cast<double>(ABS(nextInt-startZone));
488 if (sf[nextInt] == -1) {
489 sortedfrontier.pushElement(PriorityQueue<int32_t, double>::value_type(nextInt, cost));
490 costs[nextInt] = cost;
491 sf[nextInt] = next;
492 } else if (cost < costs[nextInt] && spt[nextInt] == -1) {
493 sortedfrontier.changeElementPriority(nextInt, cost);
494 costs[nextInt] = cost;
495 sf[nextInt] = next;
496 }
497 }
498 }
499 // startZone to endZone could be solved
500 if (found) {
501 // fetch sorted zones
502 int32_t current = targetZone;
503 int32_t end = startZone;
504
505 std::list<Zone*> betweenZones;
506 Zone* tempZone = distanceZoneMap.find(current)->second;
507 betweenZones.push_back(tempZone);
508 while(current != end) {
509 if (spt[current] < 0 ) {
510 return;
511 }
512 current = spt[current];
513 tempZone = distanceZoneMap.find(current)->second;
514 betweenZones.push_front(tempZone);
515 }
516 // so here we fetch the transistions
517 Location lastLoc = m_from;
518 for (std::list<Zone*>::iterator lit = betweenZones.begin(); lit != betweenZones.end(); ++lit) {
519 ++lit;
520 if (lit == betweenZones.end()) {
521 break;
522 }
523 Zone* nextZone = *lit;
524 --lit;
525 Zone* currentZone = *lit;
526 std::vector<Cell*> tempCells = currentZone->getTransitionCells();
527 if (tempCells.empty()) {
528 continue;
529 }
530
531 Cell* transCell = NULL;
532 double nextCost = 0.0;
533 Location newLoc = lastLoc;
534 for (std::vector<Cell*>::iterator cit = tempCells.begin(); cit != tempCells.end(); ++cit) {
535 Cell* nextCell = *cit;
536 TransitionInfo* nextTrans = nextCell->getTransition();
537 Cell* transTargetCell = nextTrans->m_layer->getCellCache()->getCell(nextTrans->m_mc);
538 // skip wrong transitions
539 if (nextZone != transTargetCell->getZone()) {
540 continue;
541 }
542 // nearest transistion (air-line distance)
543 Location tmpLoc((*cit)->getLayer());
544 tmpLoc.setLayerCoordinates((*cit)->getLayerCoordinates());
545 double locCost = lastLoc.getLayerDistanceTo(tmpLoc);
546 if (!transCell || locCost < nextCost) {
547 nextCost = locCost;
548 transCell = *cit;
549 newLoc.setLayer(transTargetCell->getLayer());
550 newLoc.setLayerCoordinates(transTargetCell->getLayerCoordinates());
551 }
552 }
553 // found valid transition cell
554 if (transCell) {
555 m_betweenTargets.push_back(transCell);
556 lastLoc = newLoc;
557 }
558 }
559 }
560 }
561 }