31 #include "model/metamodel/grids/cellgrid.h"
32 #include "model/structures/instance.h"
33 #include "model/structures/layer.h"
35 #include "pathfinder/searchspace.h"
37 #include "routepather.h"
38 #include "routepathersearch.h"
41 void RoutePather::setMap(Map* map) {
48 int RoutePather::makeSessionId() {
49 return m_nextFreeSessionId++;
52 void RoutePather::makePlan(
const Instance *instance,
const Location& target,
int session_id,
int priority) {
53 SearchSpace* searchspace = getSearchSpace(target.getLayer());
55 searchspace =
new SearchSpace(target.getLayer());
56 addSearchSpace(searchspace);
58 if(searchspace->isInSearchSpace(target)) {
59 RoutePatherSearch* newSearch =
new RoutePatherSearch(session_id, instance->getLocation(), target, searchspace);
60 m_sessions.pushElement(SessionQueue::value_type(newSearch, priority));
61 addSessionId(session_id);
62 m_path_targets.insert(LocationMap::value_type(session_id,target));
66 bool RoutePather::locationsEqual(
const Location &a,
const Location &b) {
68 const ModelCoordinate a_coord = a.getLayerCoordinates();
69 const ModelCoordinate b_coord = b.getLayerCoordinates();
71 return a_coord == b_coord;
74 bool RoutePather::testStep(
const Instance *instance, Path& path) {
75 Location instanceLoc = instance->getLocation();
77 !locationsEqual(path.front(), instanceLoc) &&
78 instanceLoc.getLayer()->cellContainsBlockingInstance(path.front().getLayerCoordinates())) {
79 const bool last_step = path.front() == path.back();
86 int RoutePather::getNextLocation(
const Instance* instance,
const Location& target,
87 double distance_to_travel, Location& nextLocation,
88 Location& facingLocation,
int session_id,
int priority) {
90 assert(instance->getLocation().getLayer() == target.getLayer());
91 bool plan_needed =
true;
93 if(session_id != -1) {
95 PathMap::iterator path_itor = m_paths.find(session_id);
96 if(path_itor != m_paths.end()) {
97 LocationMap::iterator location_itor = m_path_targets.find(session_id);
98 assert(location_itor != m_path_targets.end());
100 if(path_itor->second.empty()) {
101 m_paths.erase(path_itor);
102 m_path_targets.erase(location_itor);
106 if(!followPath(instance, path_itor->second, distance_to_travel, nextLocation, facingLocation)
107 || !locationsEqual(location_itor->second, target)) {
108 m_paths.erase(path_itor);
109 m_path_targets.erase(location_itor);
112 }
else if(!sessionIdValid(session_id)) {
118 if(session_id == -1) {
119 session_id = makeSessionId();
121 makePlan(instance, target, session_id, priority);
126 void RoutePather::update() {
127 int ticksleft = m_maxticks;
128 while(ticksleft >= 0) {
129 if(m_sessions.empty()) {
132 RoutePatherSearch* priority_session = m_sessions.getPriorityElement().first;
133 if(!sessionIdValid(priority_session->getSessionId())) {
134 delete priority_session;
135 m_sessions.popElement();
138 priority_session->updateSearch();
139 if(priority_session->getSearchStatus() == RoutePatherSearch::search_status_complete) {
140 const int session_id = priority_session->getSessionId();
141 Path newPath = priority_session->calcPath();
142 newPath.erase(newPath.begin());
143 m_paths.insert(PathMap::value_type(session_id, newPath));
144 invalidateSessionId(session_id);
145 delete priority_session;
146 m_sessions.popElement();
147 }
else if(priority_session->getSearchStatus() == RoutePatherSearch::search_status_failed) {
148 const int session_id = priority_session->getSessionId();
149 invalidateSessionId(session_id);
150 delete priority_session;
151 m_sessions.popElement();
157 bool RoutePather::followPath(
const Instance* instance, Path& path,
double speed, Location& nextLocation, Location& facingLocation) {
158 Location instanceLoc = instance->getLocation();
159 if(!testStep(instance, path)) {
167 ExactModelCoordinate instancePos = instanceLoc.getMapCoordinates();
168 ExactModelCoordinate facingPos = path.front().getMapCoordinates();
169 facingPos.x = facingPos.x + (facingPos.x - instancePos.x);
170 facingPos.y = facingPos.y + (facingPos.y - instancePos.y);
171 facingLocation = path.front();
172 facingLocation.setMapCoordinates(facingPos);
173 ExactModelCoordinate targetPos = path.front().getMapCoordinates();
174 CellGrid* grid = instanceLoc.getLayer()->getCellGrid();
175 double dx = (targetPos.x - instancePos.x) * grid->getXScale();
176 double dy = (targetPos.y - instancePos.y) * grid->getYScale();
177 double distance = sqrt(dx * dx + dy * dy);
179 if(speed > distance) {
184 instancePos.x += (dx / distance) * speed;
185 instancePos.y += (dy / distance) * speed;
190 nextLocation.setMapCoordinates(instancePos);
193 if(!testStep(instance, path)) {
200 bool RoutePather::cancelSession(
const int session_id) {
201 if(session_id >= 0) {
202 PathMap::iterator i = m_paths.find(session_id);
203 if(i != m_paths.end()) {
204 LocationMap::iterator j = m_path_targets.find(session_id);
205 assert(j != m_path_targets.end());
207 m_path_targets.erase(j);
210 invalidateSessionId(session_id);
216 void RoutePather::addSessionId(
const int sessionId) {
217 m_registeredSessionIds.push_back(sessionId);
220 bool RoutePather::sessionIdValid(
const int sessionId) {
221 for(SessionList::const_iterator i = m_registeredSessionIds.begin();
222 i != m_registeredSessionIds.end();
224 if((*i) == sessionId) {
231 bool RoutePather::invalidateSessionId(
const int sessionId) {
232 for(SessionList::iterator i = m_registeredSessionIds.begin();
233 i != m_registeredSessionIds.end();
235 if((*i) == sessionId) {
236 m_registeredSessionIds.erase(i);
243 bool RoutePather::addSearchSpace(SearchSpace* search_space) {
244 std::pair<SearchSpaceMap::iterator, bool> res = m_searchspaces.insert(SearchSpaceMap::value_type(search_space->getLayer(), search_space));
249 SearchSpace* RoutePather::getSearchSpace(Layer *
const layer) {
250 SearchSpaceMap::iterator i = m_searchspaces.find(layer);
251 if(i == m_searchspaces.end()) {