/*************************************************************************** * Copyright (C) 2005-2019 by the FIFE team * * http://www.fifengine.net * * This file is part of FIFE. * * * * FIFE is free software; you can redistribute it and/or * * modify it under the terms of the GNU Lesser General Public * * License as published by the Free Software Foundation; either * * version 2.1 of the License, or (at your option) any later version. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * * Lesser General Public License for more details. * * * * You should have received a copy of the GNU Lesser General Public * * License along with this library; if not, write to the * * Free Software Foundation, Inc., * * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA * ***************************************************************************/ // Standard C++ library includes #include // 3rd party library includes // FIFE includes // These includes are split up in two parts, separated by one empty line // First block: files included from the FIFE root src directory // Second block: files included from the same folder #include "model/metamodel/grids/cellgrid.h" #include "model/structures/layer.h" #include "model/structures/cellcache.h" #include "model/structures/cell.h" #include "model/structures/map.h" #include "pathfinder/route.h" #include "util/math/fife_math.h" #include "multilayersearch.h" namespace FIFE { MultiLayerSearch::MultiLayerSearch(Route* route, const int32_t sessionId): RoutePatherSearch(route, sessionId), m_to(route->getEndNode()), m_from(route->getStartNode()), m_startCache(m_from.getLayer()->getCellCache()), m_endCache(m_to.getLayer()->getCellCache()), m_currentCache(NULL), m_startZone(m_startCache->getCell(m_from.getLayerCoordinates())->getZone()), m_endZone(m_endCache->getCell(m_to.getLayerCoordinates())->getZone()), m_startCoordInt(m_startCache->convertCoordToInt(m_from.getLayerCoordinates())), m_lastStartCoordInt(m_startCoordInt), m_destCoordInt(m_endCache->convertCoordToInt(m_to.getLayerCoordinates())), m_lastDestCoordInt(-1), m_next(0), m_foundLast(true) { // if end zone is invalid (static blocker) then change it if (!m_endZone) { Cell* endcell = m_endCache->getCell(m_to.getLayerCoordinates()); const std::vector& neighbors = endcell->getNeighbors(); for (std::vector::const_iterator it = neighbors.begin(); it != neighbors.end(); ++it) { Zone* tmpzone = (*it)->getZone(); if (tmpzone) { m_endZone = tmpzone; if (tmpzone == m_startZone) { break; } } } } Cell* startCell = m_startCache->getCell(m_from.getLayerCoordinates()); // here we hope to find between targets // first test if target can be reached easy searchBetweenTargetsNeighbor(); // if no betweenTargets could be found we check as second the whole map if (m_betweenTargets.empty()) { searchBetweenTargetsMap(); } // if it is a protected cell it can have a second startzone if (m_betweenTargets.empty() && startCell->isZoneProtected()) { const std::vector& neighbors = startCell->getNeighbors(); for (std::vector::const_iterator it = neighbors.begin(); it != neighbors.end(); ++it) { Zone* tmpzone = (*it)->getZone(); if (tmpzone) { if (tmpzone != m_startZone) { m_startZone = tmpzone; break; } } } searchBetweenTargetsNeighbor(); if (m_betweenTargets.empty()) { searchBetweenTargetsMap(); } } // failed to find between targets, no Path can be created if (m_betweenTargets.empty()) { setSearchStatus(search_status_failed); m_route->setRouteStatus(ROUTE_FAILED); } } MultiLayerSearch::~MultiLayerSearch() { } void MultiLayerSearch::createSearchFrontier(int32_t startInt, CellCache* cache) { // reset all m_sortedFrontier.clear(); m_spt.clear(); m_sf.clear(); m_gCosts.clear(); // fill with defaults m_sortedFrontier.pushElement(PriorityQueue::value_type(startInt, 0.0)); int32_t max_index = cache->getMaxIndex(); m_spt.resize(max_index, -1); m_sf.resize(max_index, -1); m_gCosts.resize(max_index, 0.0); m_next = 0; } void MultiLayerSearch::updateSearch() { if (m_sortedFrontier.empty()) { if (!m_foundLast || m_lastDestCoordInt == m_destCoordInt || getSearchStatus() == search_status_failed) { setSearchStatus(search_status_failed); m_route->setRouteStatus(ROUTE_FAILED); return; } if (m_betweenTargets.empty()) { TransitionInfo* trans = m_currentCache->getCell(m_currentCache->convertIntToCoord(m_lastDestCoordInt))->getTransition(); if (trans) { m_lastStartCoordInt = m_endCache->convertCoordToInt(trans->m_mc); } m_currentCache = m_endCache; m_lastDestCoordInt = m_currentCache->getCell(m_to.getLayerCoordinates())->getCellId(); } else { if (m_lastDestCoordInt != -1) { TransitionInfo* trans = m_currentCache->getCell(m_currentCache->convertIntToCoord(m_lastDestCoordInt))->getTransition(); if (trans) { m_lastStartCoordInt = trans->m_layer->getCellCache()->convertCoordToInt(trans->m_mc); } } m_currentCache = m_betweenTargets.front()->getLayer()->getCellCache(); m_lastDestCoordInt = m_betweenTargets.front()->getCellId(); m_betweenTargets.pop_front(); m_foundLast = false; } createSearchFrontier(m_lastStartCoordInt, m_currentCache); } PriorityQueue::value_type topvalue = m_sortedFrontier.getPriorityElement(); m_sortedFrontier.popElement(); m_next = topvalue.first; m_spt[m_next] = m_sf[m_next]; // found destination if (m_destCoordInt == m_next && m_betweenTargets.empty()) { if (m_endCache == m_currentCache) { setSearchStatus(search_status_complete); m_route->setRouteStatus(ROUTE_SEARCHED); return; } } // found between target if (m_lastDestCoordInt == m_next) { calcPathStep(); m_sortedFrontier.clear(); m_foundLast = true; return; } ModelCoordinate destCoord = m_currentCache->convertIntToCoord(m_lastDestCoordInt); ModelCoordinate nextCoord = m_currentCache->convertIntToCoord(m_next); CellGrid* grid = m_currentCache->getLayer()->getCellGrid(); Cell* nextCell = m_currentCache->getCell(nextCoord); if (!nextCell) { return; } int32_t cellZ = nextCell->getLayerCoordinates().z; int32_t maxZ = m_route->getZStepRange(); bool zLimited = maxZ != -1; uint8_t blockerThreshold = m_ignoreDynamicBlockers ? 2 : 1; bool limitedArea = m_route->isAreaLimited(); const std::vector& adjacents = nextCell->getNeighbors(); if (adjacents.empty()) { return; } for (std::vector::const_iterator i = adjacents.begin(); i != adjacents.end(); ++i) { if (*i == NULL) { continue; } if ((*i)->getLayer()->getCellCache() != m_currentCache) { continue; } int32_t adjacentInt = (*i)->getCellId(); if (m_sf[adjacentInt] != -1 && m_spt[adjacentInt] != -1) { continue; } if (zLimited && ABS(cellZ-(*i)->getLayerCoordinates().z) > maxZ) { continue; } bool blocker = (*i)->getCellType() > blockerThreshold; ModelCoordinate adjacentCoord = (*i)->getLayerCoordinates(); if ((adjacentInt == m_next || blocker) && adjacentInt != m_destCoordInt) { if (blocker && m_multicell) { std::vector::iterator bc_it = std::find(m_ignoredBlockers.begin(), m_ignoredBlockers.end(), *i); if (bc_it == m_ignoredBlockers.end()) { continue; } } else { continue; } } // search if there are blockers which could block multicell object if (m_multicell) { blocker = false; Location currentLoc(nextCell->getLayer()); currentLoc.setLayerCoordinates(nextCell->getLayerCoordinates()); Location adjacentLoc((*i)->getLayer()); adjacentLoc.setLayerCoordinates((*i)->getLayerCoordinates()); int32_t rotation = getAngleBetween(currentLoc, adjacentLoc); std::vector coords = grid->toMultiCoordinates(adjacentLoc.getLayerCoordinates(), m_route->getOccupiedCells(rotation)); std::vector::iterator coord_it = coords.begin(); for (; coord_it != coords.end(); ++coord_it) { Cell* cell = m_currentCache->getCell(*coord_it); if (cell) { if (cell->getCellType() > blockerThreshold) { std::vector::iterator bc_it = std::find(m_ignoredBlockers.begin(), m_ignoredBlockers.end(), cell); if (bc_it == m_ignoredBlockers.end()) { blocker = true; break; } } if (limitedArea) { // check if cell is on one of the areas bool sameAreas = false; const std::list areas = m_route->getLimitedAreas(); std::list::const_iterator area_it = areas.begin(); for (; area_it != areas.end(); ++area_it) { if (m_currentCache->isCellInArea(*area_it, cell)) { sameAreas = true; break; } } if (!sameAreas) { blocker = true; break; } } } else { blocker = true; break; } } if (blocker) { continue; } } else if (limitedArea) { // check if cell is on one of the areas bool sameAreas = false; const std::list areas = m_route->getLimitedAreas(); std::list::const_iterator area_it = areas.begin(); for (; area_it != areas.end(); ++area_it) { if (m_currentCache->isCellInArea(*area_it, *i)) { sameAreas = true; break; } } if (!sameAreas) { continue; } } double gCost = m_gCosts[m_next]; if (m_specialCost) { gCost += m_currentCache->getAdjacentCost(adjacentCoord ,nextCoord, m_route->getCostId()); } else { gCost += m_currentCache->getAdjacentCost(adjacentCoord ,nextCoord); } double hCost = grid->getHeuristicCost(adjacentCoord, destCoord); if (m_sf[adjacentInt] == -1) { m_sortedFrontier.pushElement(PriorityQueue::value_type(adjacentInt, gCost + hCost)); m_gCosts[adjacentInt] = gCost; m_sf[adjacentInt] = m_next; } else if (gCost < m_gCosts[adjacentInt] && m_spt[adjacentInt] == -1) { m_sortedFrontier.changeElementPriority(adjacentInt, gCost + hCost); m_gCosts[adjacentInt] = gCost; m_sf[adjacentInt] = m_next; } } } void MultiLayerSearch::calcPathStep() { int32_t current = m_lastDestCoordInt; int32_t end = m_lastStartCoordInt; Location newnode(m_currentCache->getLayer()); Path path; // This assures that the agent always steps into the center of the target cell. newnode.setLayerCoordinates(m_currentCache->convertIntToCoord(current)); path.push_back(newnode); while(current != end) { if (m_spt[current] < 0 ) { // This is when the size of m_spt can not handle the distance of the location setSearchStatus(search_status_failed); m_route->setRouteStatus(ROUTE_FAILED); break; } current = m_spt[current]; newnode.setLayerCoordinates(m_currentCache->convertIntToCoord(current)); path.push_front(newnode); } // set exact start coordinates if (m_path.empty()) { path.front().setExactLayerCoordinates(m_from.getExactLayerCoordinatesRef()); } m_path.insert(m_path.end(), path.begin(), path.end()); } void MultiLayerSearch::calcPath() { int32_t current = m_lastDestCoordInt; int32_t end = m_lastStartCoordInt; Location newnode(m_currentCache->getLayer()); Path path; // This assures that the agent always steps into the center of the target cell. newnode.setLayerCoordinates( m_currentCache->getCell(m_currentCache->convertIntToCoord(current))->getLayerCoordinates()); path.push_back(newnode); while(current != end) { if (m_spt[current] < 0 ) { // This is when the size of m_spt can not handle the distance of the location setSearchStatus(search_status_failed); m_route->setRouteStatus(ROUTE_FAILED); break; } current = m_spt[current]; newnode.setLayerCoordinates(m_currentCache->convertIntToCoord(current)); path.push_front(newnode); } m_path.insert(m_path.end(), path.begin(), path.end()); m_route->setPath(m_path); } void MultiLayerSearch::searchBetweenTargetsNeighbor() { std::vector cells = m_startCache->getTransitionCells(m_endCache->getLayer()); if (!cells.empty()) { Location loc; Cell* cell = NULL; // find nearest portal (air-line distance) for (std::vector::iterator it = cells.begin(); it != cells.end(); ++it) { if ((*it)->getZone() != m_startZone) { continue; } TransitionInfo* trans = (*it)->getTransition(); Cell* transTarget = trans->m_layer->getCellCache()->getCell(trans->m_mc); if (transTarget->getZone() != m_endZone) { continue; } if (!cell) { loc.setLayer((*it)->getLayer()); loc.setLayerCoordinates((*it)->getLayerCoordinates()); cell = *it; continue; } Location temp((*it)->getLayer()); temp.setLayerCoordinates((*it)->getLayerCoordinates()); Location loc1(cell->getTransition()->m_layer); loc1.setLayerCoordinates(cell->getTransition()->m_mc); Location loc2((*it)->getTransition()->m_layer); loc2.setLayerCoordinates((*it)->getTransition()->m_mc); double temp_distance = temp.getLayerDistanceTo(m_from) + loc2.getLayerDistanceTo(m_to); double current_distance = loc.getLayerDistanceTo(m_from) + loc1.getLayerDistanceTo(m_to); if (current_distance > temp_distance) { loc = temp; cell = *it; } } if (cell) { m_betweenTargets.push_back(cell); } } } void MultiLayerSearch::searchBetweenTargetsMap() { // in case no transition is there then return std::vector endTransCells; std::vector tmpTransCells = m_endCache->getTransitionCells(); std::vector::iterator tcell_it = tmpTransCells.begin(); for (; tcell_it != tmpTransCells.end(); ++tcell_it) { Zone* zone = (*tcell_it)->getZone(); if (zone == m_endZone) { endTransCells.push_back(*tcell_it); } } if (endTransCells.empty()) { return; } // in case no transition is there then return std::vector startTransCells; tmpTransCells = m_startCache->getTransitionCells(); tcell_it = tmpTransCells.begin(); for (; tcell_it != tmpTransCells.end(); ++tcell_it) { Zone* zone = (*tcell_it)->getZone(); if (zone == m_startZone) { startTransCells.push_back(*tcell_it); } } if (startTransCells.empty()) { return; } // fetch zones std::vector zones; const std::list& allLayers = m_from.getLayer()->getMap()->getLayers(); std::list::const_iterator lay_it = allLayers.begin(); for (; lay_it != allLayers.end(); ++lay_it) { CellCache* cache = (*lay_it)->getCellCache(); if (cache) { const std::vector& tmp_zones = cache->getZones(); zones.insert(zones.end(), tmp_zones.begin(), tmp_zones.end()); } } // sort zones by iterator distance std::map zoneDistanceMap; std::map distanceZoneMap; for (std::vector::iterator zit = zones.begin(); zit != zones.end(); ++zit) { // pseudo distance int32_t distance = std::distance(zones.begin(), zit); zoneDistanceMap.insert(std::pair(*zit, distance)); distanceZoneMap.insert(std::pair(distance, *zit)); } // start zone int32_t startZone = zoneDistanceMap.find(m_startZone)->second; // target zone int32_t targetZone = zoneDistanceMap.find(m_endZone)->second; // Priority queue to sort zones PriorityQueue sortedfrontier; // add start zone sortedfrontier.pushElement(PriorityQueue::value_type(startZone, 0.0)); // max size zones int32_t max_index = zones.size(); // shortest tree std::vector spt(max_index, -1); // search frontier std::vector sf(max_index, -1); // costs std::vector costs(max_index, 0.0); bool found = false; while (!found) { if (sortedfrontier.empty()) { break; } PriorityQueue::value_type topvalue = sortedfrontier.getPriorityElement(); sortedfrontier.popElement(); int32_t next = topvalue.first; spt[next] = sf[next]; // found destination zone if (targetZone == next) { found = true; break; } Zone* nextZone = distanceZoneMap.find(next)->second; // look in zone for tranistions std::vector transCells = nextZone->getTransitionCells(); if (transCells.empty()) { continue; } std::vector::iterator cell_it = transCells.begin(); for (; cell_it != transCells.end(); ++cell_it) { // transition info TransitionInfo* trans = (*cell_it)->getTransition(); // target transition cache CellCache* transCache = trans->m_layer->getCellCache(); // target transition cell Cell* transCelle = transCache->getCell(trans->m_mc); // next zone as int int32_t nextInt = zoneDistanceMap.find(transCelle->getZone())->second; if (nextInt == next && nextInt != targetZone) { continue; } // iterator distance as cost double cost = costs[next] + static_cast(ABS(nextInt-startZone)); if (sf[nextInt] == -1) { sortedfrontier.pushElement(PriorityQueue::value_type(nextInt, cost)); costs[nextInt] = cost; sf[nextInt] = next; } else if (cost < costs[nextInt] && spt[nextInt] == -1) { sortedfrontier.changeElementPriority(nextInt, cost); costs[nextInt] = cost; sf[nextInt] = next; } } } // startZone to endZone could be solved if (found) { // fetch sorted zones int32_t current = targetZone; int32_t end = startZone; std::list betweenZones; Zone* tempZone = distanceZoneMap.find(current)->second; betweenZones.push_back(tempZone); while(current != end) { if (spt[current] < 0 ) { return; } current = spt[current]; tempZone = distanceZoneMap.find(current)->second; betweenZones.push_front(tempZone); } // so here we fetch the transistions Location lastLoc = m_from; for (std::list::iterator lit = betweenZones.begin(); lit != betweenZones.end(); ++lit) { ++lit; if (lit == betweenZones.end()) { break; } Zone* nextZone = *lit; --lit; Zone* currentZone = *lit; std::vector tempCells = currentZone->getTransitionCells(); if (tempCells.empty()) { continue; } Cell* transCell = NULL; double nextCost = 0.0; Location newLoc = lastLoc; for (std::vector::iterator cit = tempCells.begin(); cit != tempCells.end(); ++cit) { Cell* nextCell = *cit; TransitionInfo* nextTrans = nextCell->getTransition(); Cell* transTargetCell = nextTrans->m_layer->getCellCache()->getCell(nextTrans->m_mc); // skip wrong transitions if (nextZone != transTargetCell->getZone()) { continue; } // nearest transistion (air-line distance) Location tmpLoc((*cit)->getLayer()); tmpLoc.setLayerCoordinates((*cit)->getLayerCoordinates()); double locCost = lastLoc.getLayerDistanceTo(tmpLoc); if (!transCell || locCost < nextCost) { nextCost = locCost; transCell = *cit; newLoc.setLayer(transTargetCell->getLayer()); newLoc.setLayerCoordinates(transTargetCell->getLayerCoordinates()); } } // found valid transition cell if (transCell) { m_betweenTargets.push_back(transCell); lastLoc = newLoc; } } } } }