Debian Codegraph @

engine/core/pathfinder/routepather/singlelayersearch.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 "pathfinder/route.h"
36 #include "util/math/fife_math.h"
37
38 #include "singlelayersearch.h"
39
40 namespace FIFE {
41 SingleLayerSearch::SingleLayerSearch(Route* route, const int32_t sessionId):
42 RoutePatherSearch(route, sessionId),
43 m_to(route->getEndNode()),
44 m_from(route->getStartNode()),
45 m_cellCache(m_from.getLayer()->getCellCache()),
46 m_startCoordInt(m_cellCache->convertCoordToInt(m_from.getLayerCoordinates())),
47 m_destCoordInt(m_cellCache->convertCoordToInt(m_to.getLayerCoordinates())),
48 m_next(0) {
49
50 m_sortedfrontier.pushElement(PriorityQueue<int32_t, double>::value_type(m_startCoordInt, 0.0));
51 int32_t max_index = m_cellCache->getMaxIndex();
52 m_spt.resize(max_index, -1);
53 m_sf.resize(max_index, -1);
54 m_gCosts.resize(max_index, 0.0);
55 }
56
57 SingleLayerSearch::~SingleLayerSearch() {
58 }
59
60 void SingleLayerSearch::updateSearch() {
61 if(m_sortedfrontier.empty()) {
62 setSearchStatus(search_status_failed);
63 m_route->setRouteStatus(ROUTE_FAILED);
64 return;
65 }
66
67 PriorityQueue<int32_t, double>::value_type topvalue = m_sortedfrontier.getPriorityElement();
68 m_sortedfrontier.popElement();
69 m_next = topvalue.first;
70 m_spt[m_next] = m_sf[m_next];
71 // found destination
72 if (m_destCoordInt == m_next) {
73 setSearchStatus(search_status_complete);
74 m_route->setRouteStatus(ROUTE_SEARCHED);
75 return;
76 }
77
78 ModelCoordinate destCoord = m_to.getLayerCoordinates();
79 ModelCoordinate nextCoord = m_cellCache->convertIntToCoord(m_next);
80 CellGrid* grid = m_cellCache->getLayer()->getCellGrid();
81 Cell* nextCell = m_cellCache->getCell(nextCoord);
82 if (!nextCell) {
83 return;
84 }
85 int32_t cellZ = nextCell->getLayerCoordinates().z;
86 int32_t maxZ = m_route->getZStepRange();
87 bool zLimited = maxZ != -1;
88 uint8_t blockerThreshold = m_ignoreDynamicBlockers ? 2 : 1;
89 bool limitedArea = m_route->isAreaLimited();
90 const std::vector<Cell*>& adjacents = nextCell->getNeighbors();
91 for (std::vector<Cell*>::const_iterator i = adjacents.begin(); i != adjacents.end(); ++i) {
92 if (*i == NULL) {
93 continue;
94 }
95 if ((*i)->getLayer()->getCellCache() != m_cellCache) {
96 continue;
97 }
98 int32_t adjacentInt = (*i)->getCellId();
99 if (m_sf[adjacentInt] != -1 && m_spt[adjacentInt] != -1) {
100 continue;
101 }
102 if (zLimited && ABS(cellZ-(*i)->getLayerCoordinates().z) > maxZ) {
103 continue;
104 }
105 bool blocker = (*i)->getCellType() > blockerThreshold;
106 ModelCoordinate adjacentCoord = (*i)->getLayerCoordinates();
107 if ((adjacentInt == m_next || blocker) && adjacentInt != m_destCoordInt) {
108 if (!blocker && m_multicell) {
109 continue;
110 } else if (!m_multicell){
111 continue;
112 }
113 }
114 // search if there are blockers which could block multicell object
115 if (m_multicell) {
116 blocker = false;
117 Location currentLoc(nextCell->getLayer());
118 currentLoc.setLayerCoordinates(nextCell->getLayerCoordinates());
119 Location adjacentLoc((*i)->getLayer());
120 adjacentLoc.setLayerCoordinates((*i)->getLayerCoordinates());
121
122 int32_t rotation = getAngleBetween(currentLoc, adjacentLoc);
123 std::vector<ModelCoordinate> coords = grid->toMultiCoordinates(adjacentLoc.getLayerCoordinates(), m_route->getOccupiedCells(rotation));
124 std::vector<ModelCoordinate>::iterator coord_it = coords.begin();
125 for (; coord_it != coords.end(); ++coord_it) {
126 Cell* cell = m_cellCache->getCell(*coord_it);
127 if (cell) {
128 if (cell->getCellType() > blockerThreshold) {
129 std::vector<Cell*>::iterator bc_it = std::find(m_ignoredBlockers.begin(), m_ignoredBlockers.end(), cell);
130 if (bc_it == m_ignoredBlockers.end()) {
131 blocker = true;
132 break;
133 }
134 }
135 if (limitedArea) {
136 // check if cell is on one of the areas
137 bool sameAreas = false;
138 const std::list<std::string> areas = m_route->getLimitedAreas();
139 std::list<std::string>::const_iterator area_it = areas.begin();
140 for (; area_it != areas.end(); ++area_it) {
141 if (m_cellCache->isCellInArea(*area_it, cell)) {
142 sameAreas = true;
143 break;
144 }
145 }
146 if (!sameAreas) {
147 blocker = true;
148 break;
149 }
150 }
151 } else {
152 blocker = true;
153 break;
154 }
155 }
156 if (blocker) {
157 continue;
158 }
159 } else if (limitedArea) {
160 // check if cell is on one of the areas
161 bool sameAreas = false;
162 const std::list<std::string> areas = m_route->getLimitedAreas();
163 std::list<std::string>::const_iterator area_it = areas.begin();
164 for (; area_it != areas.end(); ++area_it) {
165 if (m_cellCache->isCellInArea(*area_it, *i)) {
166 sameAreas = true;
167 break;
168 }
169 }
170 if (!sameAreas) {
171 continue;
172 }
173 }
174
175 double gCost = m_gCosts[m_next];
176 if (m_specialCost) {
177 gCost += m_cellCache->getAdjacentCost(adjacentCoord ,nextCoord, m_route->getCostId());
178 } else {
179 gCost += m_cellCache->getAdjacentCost(adjacentCoord ,nextCoord);
180 }
181 double hCost = grid->getHeuristicCost(adjacentCoord, destCoord);
182 if (m_sf[adjacentInt] == -1) {
183 m_sortedfrontier.pushElement(PriorityQueue<int32_t, double>::value_type(adjacentInt, gCost + hCost));
184 m_gCosts[adjacentInt] = gCost;
185 m_sf[adjacentInt] = m_next;
186 } else if (gCost < m_gCosts[adjacentInt] && m_spt[adjacentInt] == -1) {
187 m_sortedfrontier.changeElementPriority(adjacentInt, gCost + hCost);
188 m_gCosts[adjacentInt] = gCost;
189 m_sf[adjacentInt] = m_next;
190 }
191 }
192 }
193
194 void SingleLayerSearch::calcPath() {
195 int32_t current = m_destCoordInt;
196 int32_t end = m_startCoordInt;
197 Path path;
198 Location newnode(m_cellCache->getLayer());
199 // This assures that the agent always steps into the center of the cell.
200 newnode.setExactLayerCoordinates(FIFE::intPt2doublePt(m_to.getLayerCoordinates()));
201 path.push_back(newnode);
202 while(current != end) {
203 if (m_spt[current] < 0 ) {
204 // This is when the size of m_spt can not handle the distance of the location
205 setSearchStatus(search_status_failed);
206 m_route->setRouteStatus(ROUTE_FAILED);
207 break;
208 }
209 current = m_spt[current];
210 ModelCoordinate currentCoord = m_cellCache->convertIntToCoord(current);
211 newnode.setLayerCoordinates(currentCoord);
212 path.push_front(newnode);
213 }
214 path.front().setExactLayerCoordinates(m_from.getExactLayerCoordinatesRef());
215 m_route->setPath(path);
216 }
217 }