FIFE  2008.0
routepathersearch.cpp
00001 /***************************************************************************
00002  *   Copyright (C) 2005-2008 by the FIFE team                              *
00003  *   http://www.fifengine.de                                               *
00004  *   This file is part of FIFE.                                            *
00005  *                                                                         *
00006  *   FIFE is free software; you can redistribute it and/or                 *
00007  *   modify it under the terms of the GNU Lesser General Public            *
00008  *   License as published by the Free Software Foundation; either          *
00009  *   version 2.1 of the License, or (at your option) any later version.    *
00010  *                                                                         *
00011  *   This library is distributed in the hope that it will be useful,       *
00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00014  *   Lesser General Public License for more details.                       *
00015  *                                                                         *
00016  *   You should have received a copy of the GNU Lesser General Public      *
00017  *   License along with this library; if not, write to the                 *
00018  *   Free Software Foundation, Inc.,                                       *
00019  *   51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA          *
00020  ***************************************************************************/
00021 
00022 // Standard C++ library includes
00023 #include <algorithm>
00024 
00025 // 3rd party library includes
00026 
00027 // FIFE includes
00028 // These includes are split up in two parts, separated by one empty line
00029 // First block: files included from the FIFE root src directory
00030 // Second block: files included from the same folder
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         //use destination layer for getting the cell coordinates for now, this should be moved
00076         //into search space.
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             //first determine if coordinate is in search space.
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                 //float hCost = Heuristic::getHeuristic(m_searchspace->getLayer()->getCellGrid()->getType())->calculate((*i), destCoord);
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         //This assures that the agent always steps into the center of the cell.
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                              // This is when the size of m_spt can not handle the distance of the location
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 }