FIFE 2008.0
|
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 }