mirror of
https://github.com/libretro/scummvm.git
synced 2024-12-13 21:31:53 +00:00
349 lines
9.8 KiB
C++
349 lines
9.8 KiB
C++
/* 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 "twp/graph.h"
|
|
|
|
namespace Twp {
|
|
|
|
IndexedPriorityQueue::IndexedPriorityQueue(Common::Array<float> &keys)
|
|
: _keys(keys) {
|
|
}
|
|
|
|
void IndexedPriorityQueue::insert(int index) {
|
|
_data.push_back(index);
|
|
reorderUp();
|
|
}
|
|
|
|
int IndexedPriorityQueue::pop() {
|
|
int r = _data[0];
|
|
_data[0] = _data[_data.size() - 1];
|
|
_data.pop_back();
|
|
reorderDown();
|
|
return r;
|
|
}
|
|
|
|
void IndexedPriorityQueue::reorderUp() {
|
|
if (_data.empty())
|
|
return;
|
|
size_t a = _data.size() - 1;
|
|
while (a > 0) {
|
|
if (_keys[_data[a]] >= _keys[_data[a - 1]])
|
|
return;
|
|
int tmp = _data[a];
|
|
_data[a] = _data[a - 1];
|
|
_data[a - 1] = tmp;
|
|
a--;
|
|
}
|
|
}
|
|
|
|
void IndexedPriorityQueue::reorderDown() {
|
|
if (_data.empty())
|
|
return;
|
|
for (int a = 0; a < static_cast<int>(_data.size() - 1); a++) {
|
|
if (_keys[_data[a]] <= _keys[_data[a + 1]])
|
|
return;
|
|
int tmp = _data[a];
|
|
_data[a] = _data[a + 1];
|
|
_data[a + 1] = tmp;
|
|
}
|
|
}
|
|
|
|
bool IndexedPriorityQueue::isEmpty() {
|
|
return _data.empty();
|
|
}
|
|
|
|
Graph::Graph() {}
|
|
|
|
GraphEdge::GraphEdge(int s, int t, float c)
|
|
: start(s), to(t), cost(c) {
|
|
}
|
|
|
|
void Graph::addNode(const Math::Vector2d &node) {
|
|
_nodes.push_back(node);
|
|
_edges.push_back(Common::Array<GraphEdge>());
|
|
}
|
|
|
|
AStar::AStar(Graph *graph)
|
|
: _fCost(graph->_nodes.size()), _gCost(graph->_nodes.size()), _spt(graph->_nodes.size()), _sf(graph->_nodes.size()) {
|
|
_graph = graph;
|
|
}
|
|
|
|
void AStar::search(int source, int target) {
|
|
IndexedPriorityQueue pq(_fCost);
|
|
pq.insert(source);
|
|
while (!pq.isEmpty()) {
|
|
int NCN = pq.pop();
|
|
_spt[NCN] = _sf[NCN];
|
|
if (NCN != target) {
|
|
for (size_t i = 0; i < _graph->_edges[NCN].size(); i++) {
|
|
GraphEdge &edge = _graph->_edges[NCN][i];
|
|
float Hcost = length(_graph->_nodes[edge.to] - _graph->_nodes[target]);
|
|
float Gcost = _gCost[NCN] + edge.cost;
|
|
if (!_sf[edge.to]) {
|
|
_fCost[edge.to] = Gcost + Hcost;
|
|
_gCost[edge.to] = Gcost;
|
|
pq.insert(edge.to);
|
|
_sf[edge.to] = &edge;
|
|
} else if (Gcost < _gCost[edge.to] && !_spt[edge.to]) {
|
|
_fCost[edge.to] = Gcost + Hcost;
|
|
_gCost[edge.to] = Gcost;
|
|
pq.reorderUp();
|
|
_sf[edge.to] = &edge;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void Graph::addEdge(const GraphEdge &e) {
|
|
if (!edge(e.start, e.to)) {
|
|
_edges[e.start].push_back(e);
|
|
}
|
|
if (!edge(e.to, e.start)) {
|
|
GraphEdge e2(e.to, e.start, e.cost);
|
|
_edges[e.to].push_back(e2);
|
|
}
|
|
}
|
|
|
|
GraphEdge *Graph::edge(int start, int to) {
|
|
Common::Array<GraphEdge> &edges = _edges[start];
|
|
for (size_t i = 0; i < edges.size(); i++) {
|
|
GraphEdge *e = &edges[i];
|
|
if (e->to == to)
|
|
return e;
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
Common::Array<int> Graph::getPath(int source, int target) {
|
|
Common::Array<int> result;
|
|
AStar astar(this);
|
|
if (target >= 0) {
|
|
astar.search(source, target);
|
|
int nd = target;
|
|
result.push_back(nd);
|
|
while ((nd != source) && (astar._spt[nd] != nullptr)) {
|
|
nd = astar._spt[nd]->start;
|
|
result.push_back(nd);
|
|
}
|
|
return reverse(result);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void PathFinder::setWalkboxes(const Common::Array<Walkbox> &walkboxes) {
|
|
_walkboxes = walkboxes;
|
|
_graph = nullptr;
|
|
}
|
|
|
|
Math::Vector2d Walkbox::getClosestPointOnEdge(const Math::Vector2d &p) const {
|
|
int vi1 = -1;
|
|
int vi2 = -1;
|
|
float minDist = 100000.0f;
|
|
|
|
const Common::Array<Vector2i> &polygon = getPoints();
|
|
for (size_t i = 0; i < polygon.size(); i++) {
|
|
float dist = distanceToSegment(p, (Math::Vector2d)polygon[i], (Math::Vector2d)polygon[(i + 1) % polygon.size()]);
|
|
if (dist < minDist) {
|
|
minDist = dist;
|
|
vi1 = i;
|
|
vi2 = (i + 1) % polygon.size();
|
|
}
|
|
}
|
|
|
|
Math::Vector2d p1 = (Math::Vector2d)polygon[vi1];
|
|
Math::Vector2d p2 = (Math::Vector2d)polygon[vi2];
|
|
|
|
float x1 = p1.getX();
|
|
float y1 = p1.getY();
|
|
float x2 = p2.getX();
|
|
float y2 = p2.getY();
|
|
float x3 = p.getX();
|
|
float y3 = p.getY();
|
|
|
|
float u = (((x3 - x1) * (x2 - x1)) + ((y3 - y1) * (y2 - y1))) / (((x2 - x1) * (x2 - x1)) + ((y2 - y1) * (y2 - y1)));
|
|
|
|
float xu = x1 + u * (x2 - x1);
|
|
float yu = y1 + u * (y2 - y1);
|
|
|
|
if (u < 0)
|
|
return Math::Vector2d(x1, y1);
|
|
if (u > 1)
|
|
return Math::Vector2d(x2, y2);
|
|
return Math::Vector2d(xu, yu);
|
|
}
|
|
|
|
bool PathFinder::inLineOfSight(const Math::Vector2d &start, const Math::Vector2d &to) {
|
|
const float epsilon = 0.5f;
|
|
|
|
// Not in LOS if any of the ends is outside the polygon
|
|
if (!_walkboxes[0].contains(start) || !_walkboxes[0].contains(to))
|
|
return false;
|
|
|
|
// In LOS if it's the same start and end location
|
|
if (length(start - to) < epsilon)
|
|
return true;
|
|
|
|
// Not in LOS if any edge is intersected by the start-end line segment
|
|
for (uint i = 0; i < _walkboxes.size(); i++) {
|
|
const Walkbox &walkbox = _walkboxes[i];
|
|
const Common::Array<Vector2i> &polygon = walkbox.getPoints();
|
|
const uint size = polygon.size();
|
|
for (uint j = 0; j < size; j++) {
|
|
Math::Vector2d v1 = (Math::Vector2d)polygon[j];
|
|
Math::Vector2d v2 = (Math::Vector2d)polygon[(j + 1) % size];
|
|
if (!lineSegmentsCross(start, to, v1, v2))
|
|
continue;
|
|
|
|
// In some cases a 'snapped' endpoint is just a little over the line due to rounding errors. So a 0.5 margin is used to tackle those cases.
|
|
if ((distanceToSegment(start, v1, v2) > epsilon) && (distanceToSegment(to, v1, v2) > epsilon))
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Finally the middle point in the segment determines if in LOS or not
|
|
const Math::Vector2d v2 = (start + to) / 2.0f;
|
|
if (!_walkboxes[0].contains(v2))
|
|
return false;
|
|
for (uint i = 1; i < _walkboxes.size(); i++) {
|
|
if (_walkboxes[i].contains(v2, false))
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
Common::SharedPtr<Graph> PathFinder::createGraph() {
|
|
Common::SharedPtr<Graph> result(new Graph());
|
|
for (uint i = 0; i < _walkboxes.size(); i++) {
|
|
const Walkbox &walkbox = _walkboxes[i];
|
|
if (walkbox.getPoints().size() > 2) {
|
|
bool firstWalkbox = (i == 0);
|
|
if (!walkbox.isVisible())
|
|
firstWalkbox = true;
|
|
for (uint j = 0; j < walkbox.getPoints().size(); j++) {
|
|
if (walkbox.concave(j) == firstWalkbox) {
|
|
Math::Vector2d vertex = (Math::Vector2d)walkbox.getPoints()[j];
|
|
result->_concaveVertices.push_back(vertex);
|
|
result->addNode(vertex);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
for (uint i = 0; i < result->_concaveVertices.size(); i++) {
|
|
for (uint j = 0; j < result->_concaveVertices.size(); j++) {
|
|
const Math::Vector2d c1(result->_concaveVertices[i]);
|
|
const Math::Vector2d c2(result->_concaveVertices[j]);
|
|
if (inLineOfSight(c1, c2)) {
|
|
const float d = distance(c1, c2);
|
|
result->addEdge(GraphEdge(i, j, d));
|
|
}
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Common::Array<Math::Vector2d> PathFinder::calculatePath(const Math::Vector2d &s, const Math::Vector2d &t) {
|
|
Math::Vector2d start(s);
|
|
Math::Vector2d to(t);
|
|
Common::Array<Math::Vector2d> result;
|
|
if (!_walkboxes.empty()) {
|
|
// find the walkbox where the actor is and put it first
|
|
for (uint i = 0; i < _walkboxes.size(); i++) {
|
|
const Walkbox &wb = _walkboxes[i];
|
|
if (wb.contains(start) && (i != 0)) {
|
|
_graph.reset();
|
|
SWAP(_walkboxes[0], _walkboxes[i]);
|
|
break;
|
|
}
|
|
}
|
|
|
|
// if no walkbox has been found => find the nearest walkbox
|
|
if (!_walkboxes[0].contains(start)) {
|
|
Common::Array<float> dists(_walkboxes.size());
|
|
for (uint i = 0; i < _walkboxes.size(); i++) {
|
|
const Walkbox &wb = _walkboxes[i];
|
|
dists[i] = distance(wb.getClosestPointOnEdge(start), start);
|
|
}
|
|
|
|
const size_t index = minIndex(dists);
|
|
if (index != 0) {
|
|
_graph.reset();
|
|
SWAP(_walkboxes[0], _walkboxes[index]);
|
|
}
|
|
}
|
|
|
|
if (!_graph)
|
|
_graph = createGraph();
|
|
|
|
// create new node on start position
|
|
_walkgraph = *_graph;
|
|
const uint startNodeIndex = _walkgraph._nodes.size();
|
|
|
|
// if destination is not inside current walkable area, then get the closest point
|
|
const Walkbox &wb = _walkboxes[0];
|
|
if (wb.isVisible() && !wb.contains(start)) {
|
|
start = wb.getClosestPointOnEdge(start);
|
|
}
|
|
if (wb.isVisible() && !wb.contains(to)) {
|
|
to = wb.getClosestPointOnEdge(to);
|
|
}
|
|
// we don't want the actor to walk in a different walkbox
|
|
// then check if endpoint is inside one of the other walkboxes and find the closest point on edge
|
|
for (uint i = 1; i < _walkboxes.size(); i++) {
|
|
if (_walkboxes[i].contains(to)) {
|
|
to = _walkboxes[i].getClosestPointOnEdge(to);
|
|
break;
|
|
}
|
|
}
|
|
|
|
_walkgraph.addNode(start);
|
|
|
|
for (uint i = 0; i < _walkgraph._concaveVertices.size(); i++) {
|
|
const Math::Vector2d c = _walkgraph._concaveVertices[i];
|
|
if (inLineOfSight(start, c))
|
|
_walkgraph.addEdge(GraphEdge(startNodeIndex, i, distance(start, c)));
|
|
}
|
|
|
|
// create new node on end position
|
|
const uint endNodeIndex = _walkgraph._nodes.size();
|
|
_walkgraph.addNode(to);
|
|
|
|
for (uint i = 0; i < _walkgraph._concaveVertices.size(); i++) {
|
|
const Math::Vector2d c = _walkgraph._concaveVertices[i];
|
|
if (inLineOfSight(to, c))
|
|
_walkgraph.addEdge(GraphEdge(i, endNodeIndex, distance(to, c)));
|
|
}
|
|
|
|
if (inLineOfSight(start, to))
|
|
_walkgraph.addEdge(GraphEdge(startNodeIndex, endNodeIndex, distance(start, to)));
|
|
|
|
const Common::Array<int> indices = _walkgraph.getPath(startNodeIndex, endNodeIndex);
|
|
for (uint i = 0; i < indices.size(); i++) {
|
|
const int index = indices[i];
|
|
result.push_back(_walkgraph._nodes[index]);
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
} // namespace Twp
|