FIFE  2008.0
 All Classes Namespaces Functions Variables Enumerations Enumerator Pages
routepathersearch.cpp
1 /***************************************************************************
2  * Copyright (C) 2005-2008 by the FIFE team *
3  * http://www.fifengine.de *
4  * This file is part of FIFE. *
5  * *
6  * FIFE is free software; you can redistribute it and/or *
7  * modify it under the terms of the GNU Lesser General Public *
8  * License as published by the Free Software Foundation; either *
9  * version 2.1 of the License, or (at your option) any later version. *
10  * *
11  * This library is distributed in the hope that it will be useful, *
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
14  * Lesser General Public License for more details. *
15  * *
16  * You should have received a copy of the GNU Lesser General Public *
17  * License along with this library; if not, write to the *
18  * Free Software Foundation, Inc., *
19  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
20  ***************************************************************************/
21 
22 // Standard C++ library includes
23 #include <algorithm>
24 
25 // 3rd party library includes
26 
27 // FIFE includes
28 // These includes are split up in two parts, separated by one empty line
29 // First block: files included from the FIFE root src directory
30 // Second block: files included from the same folder
31 #include "model/metamodel/grids/cellgrid.h"
32 #include "model/structures/layer.h"
33 #include "model/structures/instancetree.h"
34 #include "model/metamodel/object.h"
35 #include "pathfinder/searchspace.h"
36 #include "pathfinder/heuristic.h"
37 #include "util/math/fife_math.h"
38 
39 #include "routepathersearch.h"
40 
41 namespace FIFE {
42  RoutePatherSearch::RoutePatherSearch(const int32_t session_id, const Location& from, const Location& to, SearchSpace* searchSpace)
43  : m_to(to),
44  m_from(from),
45  m_sessionId(session_id),
46  m_searchspace(searchSpace),
47  m_status(search_status_incomplete),
48  m_startCoordInt(searchSpace->convertCoordToInt(from.getLayerCoordinates())),
49  m_destCoordInt(searchSpace->convertCoordToInt(to.getLayerCoordinates())),
50  m_next(0),
51  m_heuristic(Heuristic::getHeuristic(searchSpace->getLayer()->getCellGrid()->getType()))
52  {
53  m_sortedfrontier.pushElement(PriorityQueue<int32_t, double>::value_type(m_startCoordInt, 0.0));
54  int32_t max_index = m_searchspace->getMaxIndex();
55  m_spt.resize(max_index + 1, -1);
56  m_sf.resize(max_index + 1, -1);
57  m_gCosts.resize(max_index + 1, 0.0f);;
58  }
59 
60 
61  void RoutePatherSearch::updateSearch() {
62  if(m_sortedfrontier.empty()) {
63  setSearchStatus(search_status_failed);
64  return;
65  }
66  PriorityQueue<int32_t, double>::value_type topvalue = m_sortedfrontier.getPriorityElement();
67  m_sortedfrontier.popElement();
68  m_next = topvalue.first;
69  m_spt[m_next] = m_sf[m_next];
70  ModelCoordinate destCoord = m_to.getLayerCoordinates();
71  if(m_destCoordInt == m_next) {
72  setSearchStatus(search_status_complete);
73  return;
74  }
75  //use destination layer for getting the cell coordinates for now, this should be moved
76  //into search space.
77  ModelCoordinate nextCoord = m_searchspace->convertIntToCoord(m_next);
78  std::vector<ModelCoordinate> adjacents;
79  m_searchspace->getLayer()->getCellGrid()->getAccessibleCoordinates(nextCoord, adjacents);
80  for(std::vector<ModelCoordinate>::iterator i = adjacents.begin(); i != adjacents.end(); ++i) {
81  //first determine if coordinate is in search space.
82  Location loc;
83  loc.setLayer(m_searchspace->getLayer());
84  loc.setLayerCoordinates((*i));
85  int32_t adjacentInt = m_searchspace->convertCoordToInt((*i));
86  if(m_searchspace->isInSearchSpace(loc)) {
87  if((adjacentInt == m_next || loc.getLayer()->cellContainsBlockingInstance(loc.getLayerCoordinates())) &&
88  adjacentInt != m_destCoordInt) {
89  continue;
90  }
91  double hCost = m_heuristic->calculate((*i), destCoord);
92  //float hCost = Heuristic::getHeuristic(m_searchspace->getLayer()->getCellGrid()->getType())->calculate((*i), destCoord);
93  double gCost = m_gCosts[m_next] + loc.getLayer()->getCellGrid()->getAdjacentCost(nextCoord, (*i));
94  if(m_sf[adjacentInt] == -1) {
95  m_sortedfrontier.pushElement(PriorityQueue<int32_t, double>::value_type(adjacentInt, gCost + hCost));
96  m_gCosts[adjacentInt] = gCost;
97  m_sf[adjacentInt] = m_next;
98  }
99  else if(gCost < m_gCosts[adjacentInt] && m_spt[adjacentInt] == -1) {
100  m_sortedfrontier.changeElementPriority(adjacentInt, gCost + hCost);
101  m_gCosts[adjacentInt] = gCost;
102  m_sf[adjacentInt] = m_next;
103  }
104  }
105  }
106  }
107 
108  RoutePatherSearch::Path RoutePatherSearch::calcPath() {
109  int32_t current = m_destCoordInt;
110  int32_t end = m_startCoordInt;
111  Path path;
112  //This assures that the agent always steps into the center of the cell.
113  Location to(m_to);
114  to.setExactLayerCoordinates(FIFE::intPt2doublePt(to.getLayerCoordinates()));
115  path.push_back(to);
116  while(current != end) {
117  if(m_spt[current] < 0 ) {
118  // This is when the size of m_spt can not handle the distance of the location
119  setSearchStatus(search_status_failed);
120  break;
121  }
122  current = m_spt[current];
123  Location newnode;
124  newnode.setLayer(m_searchspace->getLayer());
125  ModelCoordinate currentCoord = m_searchspace->convertIntToCoord(current);
126  newnode.setLayerCoordinates(currentCoord);
127  path.push_front(newnode);
128  }
129  path.front().setExactLayerCoordinates(m_from.getExactLayerCoordinates());
130  return path;
131  }
132 }