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