1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
|
/* ScummVM - Graphic Adventure Engine
*
* ScummVM is the legal property of its developers, whose names
* are too numerous to list here. Please refer to the COPYRIGHT
* file distributed with this source distribution.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
#include "engines/stark/movement/shortestpath.h"
#include "common/hash-ptr.h"
#include "engines/stark/resources/floor.h"
namespace Stark {
ShortestPath::NodeList ShortestPath::search(const Resources::FloorEdge *start, const Resources::FloorEdge *goal) {
NodeList frontier;
NodePrecedenceMap cameFrom;
NodeCostMap costSoFar;
frontier.push_back(start);
cameFrom[start] = nullptr;
costSoFar[start] = 0;
while (!frontier.empty()) {
const Resources::FloorEdge *current = popEdgeWithLowestCost(frontier, costSoFar);
if (current == goal)
break;
Common::Array<Resources::FloorEdge *> neighbours = current->getNeighbours();
for (uint i = 0; i < neighbours.size(); i++) {
const Resources::FloorEdge *next = neighbours[i];
if (!next->isEnabled())
continue;
float newCost = costSoFar[current] + current->costTo(next);
if (!costSoFar.contains(next) || newCost < costSoFar[next]) {
frontier.push_back(next);
cameFrom[next] = current;
costSoFar[next] = newCost;
}
}
}
return rebuildPath(start, goal, cameFrom);
}
ShortestPath::NodeList ShortestPath::rebuildPath(const Resources::FloorEdge *start, const Resources::FloorEdge *goal,
const NodePrecedenceMap &cameFrom) const {
NodeList path;
const Resources::FloorEdge *current = goal;
path.push_front(goal);
while (current && current != start) {
current = cameFrom.getValOrDefault(current, nullptr);
path.push_front(current);
}
if (current != start) {
// No path has been found from start to goal
return NodeList();
}
path.push_front(start);
return path;
}
const Resources::FloorEdge *ShortestPath::popEdgeWithLowestCost(NodeList &frontier, const NodeCostMap &costSoFar) const {
// Poor man's priority queue using a list ...
NodeList::iterator lowestCostItem = frontier.begin();
for (NodeList::iterator it = frontier.begin(); it != frontier.end(); it++) {
if (costSoFar[*it] < costSoFar[*lowestCostItem]) {
lowestCostItem = it;
}
}
const Resources::FloorEdge *result = *lowestCostItem;
frontier.erase(lowestCostItem);
return result;
}
} // End of namespace Stark
|
