routepathersearch.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 <algorithm>
00024
00025
00026
00027
00028
00029
00030
00031 #include "model/metamodel/grids/cellgrid.h"
00032 #include "model/structures/layer.h"
00033 #include "model/structures/instancetree.h"
00034 #include "model/metamodel/object.h"
00035 #include "pathfinder/searchspace.h"
00036 #include "pathfinder/heuristic.h"
00037 #include "util/math/fife_math.h"
00038
00039 #include "routepathersearch.h"
00040
00041 namespace FIFE {
00042 RoutePatherSearch::RoutePatherSearch(const int session_id, const Location& from, const Location& to, SearchSpace* searchSpace)
00043 : m_to(to),
00044 m_from(from),
00045 m_sessionId(session_id),
00046 m_searchspace(searchSpace),
00047 m_status(search_status_incomplete),
00048 m_startCoordInt(searchSpace->convertCoordToInt(from.getLayerCoordinates())),
00049 m_destCoordInt(searchSpace->convertCoordToInt(to.getLayerCoordinates())),
00050 m_next(0),
00051 m_heuristic(Heuristic::getHeuristic(searchSpace->getLayer()->getCellGrid()->getType()))
00052 {
00053 m_sortedfrontier.pushElement(PriorityQueue<int, float>::value_type(m_startCoordInt, 0.0f));
00054 int max_index = m_searchspace->getMaxIndex();
00055 m_spt.resize(max_index + 1, -1);
00056 m_sf.resize(max_index + 1, -1);
00057 m_gCosts.resize(max_index + 1, 0.0f);;
00058 }
00059
00060
00061 void RoutePatherSearch::updateSearch() {
00062 if(m_sortedfrontier.empty()) {
00063 setSearchStatus(search_status_failed);
00064 return;
00065 }
00066 PriorityQueue<int, float>::value_type topvalue = m_sortedfrontier.getPriorityElement();
00067 m_sortedfrontier.popElement();
00068 m_next = topvalue.first;
00069 m_spt[m_next] = m_sf[m_next];
00070 ModelCoordinate destCoord = m_to.getLayerCoordinates();
00071 if(m_destCoordInt == m_next) {
00072 setSearchStatus(search_status_complete);
00073 return;
00074 }
00075
00076
00077 ModelCoordinate nextCoord = m_searchspace->convertIntToCoord(m_next);
00078 std::vector<ModelCoordinate> adjacents;
00079 m_searchspace->getLayer()->getCellGrid()->getAccessibleCoordinates(nextCoord, adjacents);
00080 for(std::vector<ModelCoordinate>::iterator i = adjacents.begin(); i != adjacents.end(); ++i) {
00081
00082 Location loc;
00083 loc.setLayer(m_searchspace->getLayer());
00084 loc.setLayerCoordinates((*i));
00085 int adjacentInt = m_searchspace->convertCoordToInt((*i));
00086 if(m_searchspace->isInSearchSpace(loc)) {
00087 if((adjacentInt == m_next || loc.getLayer()->cellContainsBlockingInstance(loc.getLayerCoordinates())) &&
00088 adjacentInt != m_destCoordInt) {
00089 continue;
00090 }
00091 float hCost = m_heuristic->calculate((*i), destCoord);
00092
00093 float gCost = m_gCosts[m_next] + loc.getLayer()->getCellGrid()->getAdjacentCost(nextCoord, (*i));
00094 if(m_sf[adjacentInt] == -1) {
00095 m_sortedfrontier.pushElement(PriorityQueue<int, float>::value_type(adjacentInt, gCost + hCost));
00096 m_gCosts[adjacentInt] = gCost;
00097 m_sf[adjacentInt] = m_next;
00098 }
00099 else if(gCost < m_gCosts[adjacentInt] && m_spt[adjacentInt] == -1) {
00100 m_sortedfrontier.changeElementPriority(adjacentInt, gCost + hCost);
00101 m_gCosts[adjacentInt] = gCost;
00102 m_sf[adjacentInt] = m_next;
00103 }
00104 }
00105 }
00106 }
00107
00108 RoutePatherSearch::Path RoutePatherSearch::calcPath() {
00109 int current = m_destCoordInt;
00110 int end = m_startCoordInt;
00111 Path path;
00112
00113 Location to(m_to);
00114 to.setExactLayerCoordinates(FIFE::intPt2doublePt(to.getLayerCoordinates()));
00115 path.push_back(to);
00116 while(current != end) {
00117 if(m_spt[current] < 0 ) {
00118
00119 setSearchStatus(search_status_failed);
00120 break;
00121 }
00122 current = m_spt[current];
00123 Location newnode;
00124 newnode.setLayer(m_searchspace->getLayer());
00125 ModelCoordinate currentCoord = m_searchspace->convertIntToCoord(current);
00126 newnode.setLayerCoordinates(currentCoord);
00127 path.push_front(newnode);
00128 }
00129 path.front().setExactLayerCoordinates(m_from.getExactLayerCoordinates());
00130 return path;
00131 }
00132 }