routepather.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <cassert>
00024
00025
00026
00027
00028
00029
00030
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
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 }