| 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 |
} |