routepather.cpp

00001 /***************************************************************************
00002  *   Copyright (C) 2005-2008 by the FIFE team                              *
00003  *   http://www.fifengine.de                                               *
00004  *   This file is part of FIFE.                                            *
00005  *                                                                         *
00006  *   FIFE is free software; you can redistribute it and/or                 *
00007  *   modify it under the terms of the GNU Lesser General Public            *
00008  *   License as published by the Free Software Foundation; either          *
00009  *   version 2.1 of the License, or (at your option) any later version.    *
00010  *                                                                         *
00011  *   This library is distributed in the hope that it will be useful,       *
00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00014  *   Lesser General Public License for more details.                       *
00015  *                                                                         *
00016  *   You should have received a copy of the GNU Lesser General Public      *
00017  *   License along with this library; if not, write to the                 *
00018  *   Free Software Foundation, Inc.,                                       *
00019  *   51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA          *
00020  ***************************************************************************/
00021 
00022 // Standard C++ library includes
00023 #include <cassert>
00024 
00025 // 3rd party library includes
00026 
00027 // FIFE includes
00028 // These includes are split up in two parts, separated by one empty line
00029 // First block: files included from the FIFE root src directory
00030 // Second block: files included from the same folder
00031 #include "model/metamodel/grids/cellgrid.h"
00032 #include "model/structures/instance.h"
00033 #include "model/structures/layer.h"
00034 
00035 #include "pathfinder/searchspace.h"
00036 
00037 #include "routepather.h"
00038 #include "routepathersearch.h"
00039 
00040 namespace FIFE {
00041     void RoutePather::setMap(Map* map) {
00042         if(!map) {
00043             return;
00044         }
00045         m_map = map;
00046     }
00047 
00048     int RoutePather::makeSessionId() {
00049         return m_nextFreeSessionId++;
00050     }
00051     
00052     void RoutePather::makePlan(const Instance *instance, const Location& target, int session_id, int priority) {
00053         SearchSpace* searchspace = getSearchSpace(target.getLayer());
00054         if(!searchspace) {
00055             searchspace = new SearchSpace(target.getLayer());
00056             addSearchSpace(searchspace);
00057         }
00058         if(searchspace->isInSearchSpace(target)) {
00059             RoutePatherSearch* newSearch = new RoutePatherSearch(session_id, instance->getLocation(), target, searchspace);
00060             m_sessions.pushElement(SessionQueue::value_type(newSearch, priority));
00061             addSessionId(session_id);
00062             m_path_targets.insert(LocationMap::value_type(session_id,target));
00063         }
00064     }
00065     
00066     bool RoutePather::locationsEqual(const Location &a, const Location &b) {
00067         
00068         const ModelCoordinate a_coord = a.getLayerCoordinates();
00069         const ModelCoordinate b_coord = b.getLayerCoordinates();
00070         
00071         return a_coord == b_coord;
00072     }
00073     
00074     bool RoutePather::testStep(const Instance *instance, Path& path) {
00075         Location instanceLoc = instance->getLocation();
00076         if(!path.empty() && 
00077            !locationsEqual(path.front(), instanceLoc) &&
00078            instanceLoc.getLayer()->cellContainsBlockingInstance(path.front().getLayerCoordinates())) {
00079             const bool last_step = path.front() == path.back();
00080             path.clear();
00081             return last_step;
00082         }
00083         return true;
00084     }
00085     
00086     int RoutePather::getNextLocation(const Instance* instance, const Location& target, 
00087                                      double distance_to_travel, Location& nextLocation,
00088                                      Location& facingLocation, int session_id, int priority) {
00089         assert(instance);
00090         assert(instance->getLocation().getLayer() == target.getLayer());
00091         bool plan_needed = true;
00092         
00093         if(session_id != -1) {
00094             plan_needed = false;
00095             PathMap::iterator path_itor = m_paths.find(session_id);
00096             if(path_itor != m_paths.end()) {
00097                 LocationMap::iterator location_itor = m_path_targets.find(session_id);
00098                 assert(location_itor != m_path_targets.end());
00099                 
00100                 if(path_itor->second.empty()) {
00101                     m_paths.erase(path_itor);
00102                     m_path_targets.erase(location_itor);
00103                     return -1;
00104                 }
00105                 
00106                 if(!followPath(instance, path_itor->second, distance_to_travel, nextLocation, facingLocation) 
00107                    || !locationsEqual(location_itor->second, target)) {
00108                     m_paths.erase(path_itor);
00109                     m_path_targets.erase(location_itor);
00110                     plan_needed = true;
00111                 }
00112             } else if(!sessionIdValid(session_id)) {
00113                 //Session id is invalid.
00114                 return -1;
00115             }
00116         }
00117         if(plan_needed) {
00118             if(session_id == -1) {
00119                 session_id = makeSessionId();
00120             }
00121             makePlan(instance, target, session_id, priority);
00122         }
00123         return session_id;
00124     }
00125     
00126     void RoutePather::update() {
00127         int ticksleft = m_maxticks;
00128         while(ticksleft >= 0) {
00129             if(m_sessions.empty()) {
00130                 break;
00131             }
00132             RoutePatherSearch* priority_session = m_sessions.getPriorityElement().first;
00133             if(!sessionIdValid(priority_session->getSessionId())) {
00134                 delete priority_session;
00135                 m_sessions.popElement();
00136                 continue;
00137             }
00138             priority_session->updateSearch();
00139             if(priority_session->getSearchStatus() == RoutePatherSearch::search_status_complete) {
00140                 const int session_id = priority_session->getSessionId();
00141                 Path newPath = priority_session->calcPath();
00142                 newPath.erase(newPath.begin());
00143                 m_paths.insert(PathMap::value_type(session_id, newPath));
00144                 invalidateSessionId(session_id);
00145                 delete priority_session;
00146                 m_sessions.popElement();
00147             } else if(priority_session->getSearchStatus() == RoutePatherSearch::search_status_failed) {
00148                 const int session_id = priority_session->getSessionId();
00149                 invalidateSessionId(session_id);
00150                 delete priority_session;
00151                 m_sessions.popElement();
00152             }
00153             --ticksleft;
00154         }
00155     }
00156     
00157     bool RoutePather::followPath(const Instance* instance, Path& path, double speed, Location& nextLocation, Location& facingLocation) {
00158         Location instanceLoc = instance->getLocation();                      
00159         if(!testStep(instance, path)) {
00160             return false;
00161         }
00162         
00163         if(path.empty()) {
00164             return true;
00165         }
00166         
00167         ExactModelCoordinate instancePos = instanceLoc.getMapCoordinates();
00168         ExactModelCoordinate facingPos = path.front().getMapCoordinates();
00169         facingPos.x = facingPos.x + (facingPos.x - instancePos.x);
00170         facingPos.y = facingPos.y + (facingPos.y - instancePos.y);
00171         facingLocation = path.front();
00172         facingLocation.setMapCoordinates(facingPos);
00173         ExactModelCoordinate targetPos = path.front().getMapCoordinates();
00174         CellGrid* grid = instanceLoc.getLayer()->getCellGrid();
00175         double dx = (targetPos.x - instancePos.x) * grid->getXScale();
00176         double dy = (targetPos.y - instancePos.y) * grid->getYScale();
00177         double distance = sqrt(dx * dx + dy * dy);
00178         bool pop = false;
00179         if(speed > distance) {
00180             speed = distance;
00181             pop = true;
00182         }
00183         if(distance != 0) {
00184             instancePos.x += (dx / distance) * speed;
00185             instancePos.y += (dy / distance) * speed;
00186         } else {
00187             pop = true;
00188         }
00189         
00190         nextLocation.setMapCoordinates(instancePos);
00191         if(pop) {
00192             path.pop_front();
00193             if(!testStep(instance, path)) {
00194                 return false;
00195             }
00196         }
00197         return true;
00198     }
00199     
00200     bool RoutePather::cancelSession(const int session_id) {
00201         if(session_id >= 0) {
00202             PathMap::iterator i = m_paths.find(session_id);
00203             if(i != m_paths.end()) {
00204                 LocationMap::iterator j = m_path_targets.find(session_id);
00205                 assert(j != m_path_targets.end());
00206                 m_paths.erase(i);
00207                 m_path_targets.erase(j);
00208                 return true;
00209             } else {
00210                 invalidateSessionId(session_id);
00211             }
00212         }
00213         return false;
00214     }
00215     
00216     void RoutePather::addSessionId(const int sessionId) {
00217         m_registeredSessionIds.push_back(sessionId);
00218     }
00219     
00220     bool RoutePather::sessionIdValid(const int sessionId) {
00221         for(SessionList::const_iterator i = m_registeredSessionIds.begin();
00222             i != m_registeredSessionIds.end();
00223             ++i) {
00224             if((*i) == sessionId) {
00225                 return true;
00226             }
00227         }
00228         return false;
00229     }
00230     
00231     bool RoutePather::invalidateSessionId(const int sessionId) {
00232         for(SessionList::iterator i = m_registeredSessionIds.begin();
00233             i != m_registeredSessionIds.end();
00234             ++i) {
00235             if((*i) == sessionId) {
00236                 m_registeredSessionIds.erase(i);
00237                 return true;
00238             }
00239         }
00240         return false;
00241     }
00242     
00243     bool RoutePather::addSearchSpace(SearchSpace* search_space) {
00244         std::pair<SearchSpaceMap::iterator, bool> res = m_searchspaces.insert(SearchSpaceMap::value_type(search_space->getLayer(), search_space));
00245         
00246         return res.second;
00247     }
00248     
00249     SearchSpace* RoutePather::getSearchSpace(Layer * const layer) {
00250         SearchSpaceMap::iterator i = m_searchspaces.find(layer);
00251         if(i == m_searchspaces.end()) {
00252             return 0;
00253         }
00254         return i->second;
00255     }
00256 }
Generated by  doxygen 1.6.2-20100208