Mercurial > fife-parpg

comparison engine/core/pathfinder/routepather/routepathersearch.cpp @ 0:4a0efb7baf70

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
* Datasets becomes the new trunk and retires after that :-)
author mvbarracuda@33b003aa-7bff-0310-803a-e67f0ece8222
date Sun, 29 Jun 2008 18:44:17 +0000
parents
children 90005975cdbb
comparison
equal deleted inserted replaced
-1:000000000000 0:4a0efb7baf70
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 modify *
7 * it under the terms of the GNU General Public License as published by *
8 * the Free Software Foundation; either version 2 of the License, or *
9 * (at your option) any later version. *
10 * *
11 * This program 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 *
14 * GNU General Public License for more details. *
15 * *
16 * You should have received a copy of the GNU General Public License *
17 * along with this program; if not, write to the *
18 * Free Software Foundation, Inc., *
19 * 51 Franklin St, 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 int session_id, const Location& from, const Location& to, SearchSpace* searchSpace)
43 : Search(session_id, from, to, searchSpace), m_destCoordInt(0), m_startCoordInt(0), m_next(0) {
44 m_startCoordInt = m_searchspace->convertCoordToInt(from.getLayerCoordinates());
45 int max_index = m_searchspace->getMaxIndex();
46 m_destCoordInt = m_searchspace->convertCoordToInt(to.getLayerCoordinates());
47 m_sortedfrontier.pushElement(PriorityQueue<int, float>::value_type(m_startCoordInt, 0.0f));
48 m_spt.resize(max_index + 1, -1);
49 m_sf.resize(max_index + 1, -1);
50 m_gCosts.resize(max_index + 1, 0.0f);
51 m_heuristic = Heuristic::getHeuristic(searchSpace->getLayer()->getCellGrid()->getType());
52 }
53
54 void RoutePatherSearch::updateSearch() {
55 if(m_sortedfrontier.empty()) {
56 setSearchStatus(search_status_failed);
57 return;
58 }
59 PriorityQueue<int, float>::value_type topvalue = m_sortedfrontier.getPriorityElement();
60 m_sortedfrontier.popElement();
61 m_next = topvalue.first;
62 m_spt[m_next] = m_sf[m_next];
63 ModelCoordinate destCoord = m_to.getLayerCoordinates();
64 if(m_destCoordInt == m_next) {
65 setSearchStatus(search_status_complete);
66 return;
67 }
68 //use destination layer for getting the cell coordinates for now, this should be moved
69 //into search space.
70 ModelCoordinate nextCoord = m_searchspace->convertIntToCoord(m_next);
71 std::vector<ModelCoordinate> adjacents;
72 m_searchspace->getLayer()->getCellGrid()->getAccessibleCoordinates(nextCoord, adjacents);
73 for(std::vector<ModelCoordinate>::iterator i = adjacents.begin(); i != adjacents.end(); ++i) {
74 //first determine if coordinate is in search space.
75 Location loc;
76 loc.setLayer(m_searchspace->getLayer());
77 loc.setLayerCoordinates((*i));
78 int adjacentInt = m_searchspace->convertCoordToInt((*i));
79 if(m_searchspace->isInSearchSpace(loc)) {
80 if((adjacentInt == m_next || loc.getLayer()->cellContainsBlockingInstance(loc.getLayerCoordinates())) &&
81 adjacentInt != m_destCoordInt) {
82 continue;
83 }
84
85 float hCost = m_heuristic->calculate((*i), destCoord);
86 float gCost = m_gCosts[m_next] + loc.getLayer()->getCellGrid()->getAdjacentCost(nextCoord, (*i));
87 if(m_sf[adjacentInt] == -1) {
88 m_sortedfrontier.pushElement(PriorityQueue<int, float>::value_type(adjacentInt, gCost + hCost));
89 m_gCosts[adjacentInt] = gCost;
90 m_sf[adjacentInt] = m_next;
91 }
92 else if(gCost < m_gCosts[adjacentInt] && m_spt[adjacentInt] == -1) {
93 m_sortedfrontier.changeElementPriority(adjacentInt, gCost + hCost);
94 m_gCosts[adjacentInt] = gCost;
95 m_sf[adjacentInt] = m_next;
96 }
97 }
98 }
99 }
100
101 RoutePatherSearch::Path RoutePatherSearch::calcPath() {
102 int current = m_destCoordInt;
103 int end = m_startCoordInt;
104 Path path;
105 //This assures that the agent always steps into the center of the cell.
106 Location to(m_to);
107 to.setExactLayerCoordinates(FIFE::intPt2doublePt(to.getLayerCoordinates()));
108 path.push_back(to);
109 while(current != end) {
110 current = m_spt[current];
111 Location newnode;
112 newnode.setLayer(m_searchspace->getLayer());
113 ModelCoordinate currentCoord = m_searchspace->convertIntToCoord(current);
114 newnode.setLayerCoordinates(currentCoord);
115 path.push_front(newnode);
116 }
117 path.front().setExactLayerCoordinates(m_from.getExactLayerCoordinates());
118 return path;
119 }
120 }