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