mirror of
https://github.com/scummvm/scummvm.git
synced 2025-04-02 10:52:32 -04:00
312 lines
8.9 KiB
C++
312 lines
8.9 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/>.
|
|
*
|
|
*/
|
|
|
|
/*
|
|
* This code is based on the CRAB engine
|
|
*
|
|
* Copyright (c) Arvind Raja Yadav
|
|
*
|
|
* Licensed under MIT
|
|
*
|
|
*/
|
|
|
|
#include "common/system.h"
|
|
#include "crab/crab.h"
|
|
#include "crab/PathfindingAgent.h"
|
|
|
|
namespace Crab {
|
|
|
|
// This keeps the PriorityQueue organized based on the cost of the paths.
|
|
static bool compareNodes(PlannerNode const *nodeA, PlannerNode const *nodeB) {
|
|
return nodeA->getFinalCost() > nodeB->getFinalCost();
|
|
}
|
|
|
|
PathfindingAgent::PathfindingAgent() : _nodeQueue(compareNodes) {
|
|
_grid = nullptr;
|
|
|
|
_destinationSet = false;
|
|
_destinationReachable = false;
|
|
_nodeBufferDistance = 1.0f;
|
|
_solutionFound = _noSolution = false;
|
|
|
|
_startTile = nullptr;
|
|
_goalTile = nullptr;
|
|
_clickedTile = nullptr;
|
|
}
|
|
|
|
PathfindingAgent::~PathfindingAgent() {
|
|
}
|
|
|
|
void PathfindingAgent::initialize(PathfindingGrid *g) {
|
|
_grid = g;
|
|
|
|
_nodeBufferDistance = _grid->getCellSize().x / 2.0f;
|
|
|
|
_nodeBufferDistance *= _nodeBufferDistance;
|
|
}
|
|
|
|
void PathfindingAgent::setDestination(Vector2i d, bool r) {
|
|
Vector2f iVec = Vector2f((float)d.x, (float)d.y);
|
|
|
|
setDestination(iVec, r);
|
|
}
|
|
|
|
void PathfindingAgent::setDestination(Vector2i d) {
|
|
setDestination(d, true);
|
|
}
|
|
|
|
void PathfindingAgent::setDestination(Vector2f d) {
|
|
setDestination(d, true);
|
|
}
|
|
|
|
void PathfindingAgent::setDestination(Vector2f d, bool r) {
|
|
if (_grid == nullptr)
|
|
return;
|
|
|
|
_destination = d;
|
|
|
|
// TODO: This could be optimized to cache the route somehow... (SZ)
|
|
reset();
|
|
|
|
_startTile = _grid->getNodeAtPoint(_position);
|
|
// m_pGoalTile = grid->GetNodeAtPoint(d);
|
|
|
|
// I am now tracking the goal node and the clicked tile separately to solve problems
|
|
// with hangups and trying to reach un-reachable destinations.
|
|
_clickedTile = _grid->getNodeAtPoint(d);
|
|
_goalTile = _grid->getNearestOpenNode(d, _position);
|
|
|
|
PlannerNode *startingNode = new PlannerNode();
|
|
startingNode->setLocation(_startTile);
|
|
startingNode->setHCost((_position - _destination).magnitude());
|
|
startingNode->setFinalCost((_position - _destination).magnitude());
|
|
startingNode->setGivenCost(0.0);
|
|
|
|
_nodeQueue.push(startingNode);
|
|
_createdList[_startTile] = (startingNode);
|
|
|
|
_destinationSet = true;
|
|
_solutionFound = _noSolution = false;
|
|
|
|
_destinationReachable = r;
|
|
}
|
|
|
|
void PathfindingAgent::update(uint32 timeslice) {
|
|
|
|
uint32 prevTime = g_system->getMillis();
|
|
uint32 timeLeft = timeslice;
|
|
|
|
double dTempCost;
|
|
|
|
if (_solutionFound) {
|
|
if (_vSolution.size() > 0) {
|
|
float distSqr = (_position - _vSolution.back()->getPosition()).magSqr();
|
|
if (distSqr < _nodeBufferDistance) { // Have to find the right deadzone buffer
|
|
_vSolution.pop_back();
|
|
}
|
|
}
|
|
if (_vSolution.size() > 0) {
|
|
_immediateDest = Vector2i(_vSolution.back()->getPosition().x, _vSolution.back()->getPosition().y);
|
|
} else {
|
|
if (_destinationReachable)
|
|
_immediateDest = Vector2i((int)_destination.x, (int)_destination.y);
|
|
else
|
|
_immediateDest = Vector2i((int)_position.x, (int)_position.y);
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
// No nodes, no pathing.
|
|
if (_nodeQueue.empty()) {
|
|
return;
|
|
}
|
|
|
|
Common::StableMap<PathfindingGraphNode *, PlannerNode *>::iterator currentIter;
|
|
|
|
do {
|
|
PlannerNode *current = _nodeQueue.front();
|
|
_nodeQueue.pop();
|
|
|
|
if (current->getLocation() == _goalTile) { // We're done.
|
|
// m_vSolution = getSolution();
|
|
_vSolution = getPrunedSolution(nullptr);
|
|
_solutionFound = true;
|
|
return;
|
|
} else if (current->getLocation()->getMovementCost() > 0 && current->getLocation()->adjacentToNode(_clickedTile) && _clickedTile->getMovementCost() < 0) {
|
|
_vSolution = getPrunedSolution(current->getLocation());
|
|
_solutionFound = true;
|
|
return;
|
|
}
|
|
|
|
for (auto &i : current->_location->_neighborNodes) {
|
|
if (i->getMovementCost() > 0) {
|
|
// Compute the temp given cost
|
|
dTempCost = current->getGivenCost() + i->getMovementCost() * distExact(i, current->getLocation());
|
|
|
|
// If it's a duplicate...
|
|
currentIter = _createdList.find(i);
|
|
if (currentIter != _createdList.end()) {
|
|
if (dTempCost < currentIter->second->getGivenCost()) {
|
|
// If the current planner node has already been added, but the current path is cheaper,
|
|
// replace it.
|
|
|
|
_nodeQueue.remove(currentIter->second);
|
|
|
|
currentIter->second->setGivenCost(dTempCost);
|
|
currentIter->second->setFinalCost(
|
|
currentIter->second->getHCost() * 1.1 +
|
|
currentIter->second->getGivenCost());
|
|
|
|
currentIter->second->setParent(current);
|
|
|
|
_nodeQueue.push(currentIter->second);
|
|
}
|
|
} else { // Otherwise...
|
|
PlannerNode *successor = new PlannerNode();
|
|
successor->setLocation(i);
|
|
|
|
// Set the new heuristic (distance from node to the goal)
|
|
successor->setHCost(distExact(i, _goalTile));
|
|
successor->setGivenCost(dTempCost);
|
|
// Final cost is the distance to goal (scaled by 10%) plus the distance of the path.
|
|
successor->setFinalCost(successor->getHCost() * 1.1 + successor->getGivenCost());
|
|
|
|
successor->setParent(current);
|
|
|
|
_createdList[i] = (successor);
|
|
_nodeQueue.push(successor); // When the node is pushed onto the PriorityQueue it ends up beings sorted cheapest -> most expensive
|
|
}
|
|
}
|
|
}
|
|
|
|
// Update the time
|
|
if (timeslice != 0) {
|
|
timeLeft -= (g_system->getMillis() - prevTime);
|
|
prevTime = g_system->getMillis();
|
|
}
|
|
|
|
} while (!isDone() && ((int32)timeLeft >= 0 || timeslice == 0));
|
|
_noSolution = true; // You can't get there from here (SZ)
|
|
}
|
|
|
|
bool PathfindingAgent::isDone() const {
|
|
if (_nodeQueue.empty())
|
|
return true;
|
|
|
|
return false;
|
|
}
|
|
|
|
// Clear everything.
|
|
void PathfindingAgent::reset() {
|
|
for (auto &iter : _createdList)
|
|
delete iter.second;
|
|
|
|
_nodeQueue.clear();
|
|
_createdList.clear();
|
|
_vSolution.clear();
|
|
|
|
_solutionFound = false;
|
|
|
|
_goalTile = nullptr;
|
|
_startTile = nullptr;
|
|
}
|
|
|
|
void PathfindingAgent::shutdown() {
|
|
reset();
|
|
|
|
_grid = nullptr;
|
|
}
|
|
|
|
Common::Array<PathfindingGraphNode const *> const PathfindingAgent::getSolution(PathfindingGraphNode *destNode) const {
|
|
Common::Array<PathfindingGraphNode const *> temp;
|
|
|
|
PlannerNode *current = nullptr;
|
|
|
|
if (_createdList.find(_goalTile) != _createdList.end()) {
|
|
current = _createdList.find(_goalTile)->second;
|
|
} else if (destNode != nullptr) {
|
|
// If the dest node passed in is not null, that means we did not reach the goal but came close
|
|
// so we should start with that node instead when we are constructing our path
|
|
current = _createdList.find(destNode)->second;
|
|
}
|
|
|
|
// Iterate through the planner nodes to create a vector to return.
|
|
while (current) {
|
|
if (current->getLocation() != _startTile) {
|
|
// You don't have to path to the start
|
|
if (current->getLocation() != _startTile)
|
|
temp.push_back(current->getLocation());
|
|
}
|
|
|
|
current = current->getParent();
|
|
}
|
|
|
|
return temp;
|
|
}
|
|
|
|
Common::Array<PathfindingGraphNode const *> const PathfindingAgent::getPrunedSolution(PathfindingGraphNode *destNode) {
|
|
Common::Array<PathfindingGraphNode const *> temp = getSolution(destNode);
|
|
|
|
Common::Array<PathfindingGraphNode const *> returnVec = temp;
|
|
|
|
// Any node that is not adjacent to an obstacle or an obstacle corner can be removed.
|
|
for (int i = 0; (uint)i < temp.size(); ++i) {
|
|
if (!temp[i]->adjacentToObstacle()) {
|
|
if (i > 0 && (uint)i < temp.size() - 1) {
|
|
// This check to see if the node is a "corner" to an obstacle that should not be pruned
|
|
// to prevent hanging on corners.
|
|
Common::Array<PathfindingGraphNode *> corners = _grid->cornerCheck(temp[i - 1], temp[i + 1]);
|
|
|
|
if (corners.size() == 0) {
|
|
Common::Array<PathfindingGraphNode const *>::iterator theEnd = Common::remove(returnVec.begin(), returnVec.end(), temp[i]);
|
|
returnVec.erase(theEnd);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return returnVec;
|
|
}
|
|
|
|
double PathfindingAgent::distSquared(PathfindingGraphNode *tileA, PathfindingGraphNode *tileB) {
|
|
Vector2f vecTo = tileA->getPosition() - tileB->getPosition();
|
|
|
|
return vecTo.magSqr();
|
|
}
|
|
|
|
double PathfindingAgent::distExact(PathfindingGraphNode *tileA, PathfindingGraphNode *tileB) {
|
|
Vector2f vecTo = tileA->getPosition() - tileB->getPosition();
|
|
|
|
return vecTo.magnitude();
|
|
}
|
|
|
|
bool PathfindingAgent::adjacentToGoal(PathfindingGraphNode *node) {
|
|
for (const auto &iter : node->_neighborNodes) {
|
|
if (iter == _goalTile) {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
} // End of namespace Crab
|