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"
39 #include "routepathersearch.h"
42 RoutePatherSearch::RoutePatherSearch(
const int32_t session_id,
const Location& from,
const Location& to, SearchSpace* searchSpace)
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())),
51 m_heuristic(Heuristic::getHeuristic(searchSpace->getLayer()->getCellGrid()->getType()))
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);;
61 void RoutePatherSearch::updateSearch() {
62 if(m_sortedfrontier.
empty()) {
66 PriorityQueue<int32_t, double>::value_type topvalue = m_sortedfrontier.
getPriorityElement();
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) {
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) {
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) {
91 double hCost = m_heuristic->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;
99 else if(gCost < m_gCosts[adjacentInt] && m_spt[adjacentInt] == -1) {
101 m_gCosts[adjacentInt] = gCost;
102 m_sf[adjacentInt] = m_next;
108 RoutePatherSearch::Path RoutePatherSearch::calcPath() {
109 int32_t current = m_destCoordInt;
110 int32_t end = m_startCoordInt;
116 while(current != end) {
117 if(m_spt[current] < 0 ) {
122 current = m_spt[current];
124 newnode.setLayer(m_searchspace->getLayer());
125 ModelCoordinate currentCoord = m_searchspace->convertIntToCoord(current);
126 newnode.setLayerCoordinates(currentCoord);
127 path.push_front(newnode);
129 path.front().setExactLayerCoordinates(m_from.getExactLayerCoordinates());
void setSearchStatus(const SearchStatus status)
void pushElement(const value_type &element)
const value_type getPriorityElement(void) const
DoublePoint intPt2doublePt(Point pt)
bool changeElementPriority(const index_type &index, const priority_type &newPriority)
credit to phoku for his NodeDisplay example which the visitor code is adapted from ( he coded the qua...