893 lines
31 KiB
C++
893 lines
31 KiB
C++
//========= Copyright Valve Corporation, All rights reserved. ============//
|
|
//
|
|
// Purpose:
|
|
//
|
|
// $NoKeywords: $
|
|
//
|
|
//=============================================================================//
|
|
// nav_pathfind.h
|
|
// Path-finding mechanisms using the Navigation Mesh
|
|
// Author: Michael S. Booth (mike@turtlerockstudios.com), January 2003
|
|
|
|
#ifndef _NAV_PATHFIND_H_
|
|
#define _NAV_PATHFIND_H_
|
|
|
|
#include "mathlib/ssemath.h"
|
|
#include "nav_area.h"
|
|
#include "tier0/vprof.h"
|
|
|
|
#ifdef STAGING_ONLY
|
|
extern int g_DebugPathfindCounter;
|
|
#endif
|
|
|
|
//-------------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Used when building a path to determine the kind of path to build
|
|
*/
|
|
enum RouteType {
|
|
DEFAULT_ROUTE,
|
|
FASTEST_ROUTE,
|
|
SAFEST_ROUTE,
|
|
RETREAT_ROUTE,
|
|
};
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Functor used with NavAreaBuildPath()
|
|
*/
|
|
class ShortestPathCost {
|
|
public:
|
|
float operator()(CNavArea *area, CNavArea *fromArea,
|
|
const CNavLadder *ladder, const CFuncElevator *elevator,
|
|
float length) {
|
|
if (fromArea == NULL) {
|
|
// first area in path, no cost
|
|
return 0.0f;
|
|
} else {
|
|
// compute distance traveled along path so far
|
|
float dist;
|
|
|
|
if (ladder) {
|
|
dist = ladder->m_length;
|
|
} else if (length > 0.0) {
|
|
dist = length;
|
|
} else {
|
|
dist = (area->GetCenter() - fromArea->GetCenter()).Length();
|
|
}
|
|
|
|
float cost = dist + fromArea->GetCostSoFar();
|
|
|
|
// if this is a "crouch" area, add penalty
|
|
if (area->GetAttributes() & NAV_MESH_CROUCH) {
|
|
const float crouchPenalty = 20.0f; // 10
|
|
cost += crouchPenalty * dist;
|
|
}
|
|
|
|
// if this is a "jump" area, add penalty
|
|
if (area->GetAttributes() & NAV_MESH_JUMP) {
|
|
const float jumpPenalty = 5.0f;
|
|
cost += jumpPenalty * dist;
|
|
}
|
|
|
|
return cost;
|
|
}
|
|
}
|
|
};
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Find path from startArea to goalArea via an A* search, using supplied cost
|
|
* heuristic. If cost functor returns -1 for an area, that area is considered a
|
|
* dead end. This doesn't actually build a path, but the path is defined by
|
|
* following parent pointers back from goalArea to startArea. If 'closestArea'
|
|
* is non-NULL, the closest area to the goal is returned (useful if the path
|
|
* fails). If 'goalArea' is NULL, will compute a path as close as possible to
|
|
* 'goalPos'. If 'goalPos' is NULL, will use the center of 'goalArea' as the
|
|
* goal position. If 'maxPathLength' is nonzero, path building will stop when
|
|
* this length is reached. Returns true if a path exists.
|
|
*/
|
|
#define IGNORE_NAV_BLOCKERS true
|
|
template <typename CostFunctor>
|
|
bool NavAreaBuildPath(CNavArea *startArea, CNavArea *goalArea,
|
|
const Vector *goalPos, CostFunctor &costFunc,
|
|
CNavArea **closestArea = NULL, float maxPathLength = 0.0f,
|
|
int teamID = TEAM_ANY, bool ignoreNavBlockers = false) {
|
|
VPROF_BUDGET("NavAreaBuildPath", "NextBotSpiky");
|
|
|
|
if (closestArea) {
|
|
*closestArea = startArea;
|
|
}
|
|
|
|
#ifdef STAGING_ONLY
|
|
bool isDebug = (g_DebugPathfindCounter-- > 0);
|
|
#endif
|
|
|
|
if (startArea == NULL) return false;
|
|
|
|
startArea->SetParent(NULL);
|
|
|
|
if (goalArea != NULL && goalArea->IsBlocked(teamID, ignoreNavBlockers))
|
|
goalArea = NULL;
|
|
|
|
if (goalArea == NULL && goalPos == NULL) return false;
|
|
|
|
// if we are already in the goal area, build trivial path
|
|
if (startArea == goalArea) {
|
|
return true;
|
|
}
|
|
|
|
// determine actual goal position
|
|
Vector actualGoalPos = (goalPos) ? *goalPos : goalArea->GetCenter();
|
|
|
|
// start search
|
|
CNavArea::ClearSearchLists();
|
|
|
|
// compute estimate of path length
|
|
/// @todo Cost might work as "manhattan distance"
|
|
startArea->SetTotalCost((startArea->GetCenter() - actualGoalPos).Length());
|
|
|
|
float initCost = costFunc(startArea, NULL, NULL, NULL, -1.0f);
|
|
if (initCost < 0.0f) return false;
|
|
startArea->SetCostSoFar(initCost);
|
|
startArea->SetPathLengthSoFar(0.0);
|
|
|
|
startArea->AddToOpenList();
|
|
|
|
// keep track of the area we visit that is closest to the goal
|
|
float closestAreaDist = startArea->GetTotalCost();
|
|
|
|
// do A* search
|
|
while (!CNavArea::IsOpenListEmpty()) {
|
|
// get next area to check
|
|
CNavArea *area = CNavArea::PopOpenList();
|
|
|
|
#ifdef STAGING_ONLY
|
|
if (isDebug) {
|
|
area->DrawFilled(0, 255, 0, 128, 30.0f);
|
|
}
|
|
#endif
|
|
|
|
// don't consider blocked areas
|
|
if (area->IsBlocked(teamID, ignoreNavBlockers)) continue;
|
|
|
|
// check if we have found the goal area or position
|
|
if (area == goalArea ||
|
|
(goalArea == NULL && goalPos && area->Contains(*goalPos))) {
|
|
if (closestArea) {
|
|
*closestArea = area;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
// search adjacent areas
|
|
enum SearchType { SEARCH_FLOOR, SEARCH_LADDERS, SEARCH_ELEVATORS };
|
|
SearchType searchWhere = SEARCH_FLOOR;
|
|
int searchIndex = 0;
|
|
|
|
int dir = NORTH;
|
|
const NavConnectVector *floorList = area->GetAdjacentAreas(NORTH);
|
|
|
|
bool ladderUp = true;
|
|
const NavLadderConnectVector *ladderList = NULL;
|
|
enum { AHEAD = 0, LEFT, RIGHT, BEHIND, NUM_TOP_DIRECTIONS };
|
|
int ladderTopDir = AHEAD;
|
|
bool bHaveMaxPathLength = (maxPathLength > 0.0f);
|
|
float length = -1;
|
|
|
|
while (true) {
|
|
CNavArea *newArea = NULL;
|
|
NavTraverseType how;
|
|
const CNavLadder *ladder = NULL;
|
|
const CFuncElevator *elevator = NULL;
|
|
|
|
//
|
|
// Get next adjacent area - either on floor or via ladder
|
|
//
|
|
if (searchWhere == SEARCH_FLOOR) {
|
|
// if exhausted adjacent connections in current direction, begin
|
|
// checking next direction
|
|
if (searchIndex >= floorList->Count()) {
|
|
++dir;
|
|
|
|
if (dir == NUM_DIRECTIONS) {
|
|
// checked all directions on floor - check ladders next
|
|
searchWhere = SEARCH_LADDERS;
|
|
|
|
ladderList = area->GetLadders(CNavLadder::LADDER_UP);
|
|
searchIndex = 0;
|
|
ladderTopDir = AHEAD;
|
|
} else {
|
|
// start next direction
|
|
floorList = area->GetAdjacentAreas((NavDirType)dir);
|
|
searchIndex = 0;
|
|
}
|
|
|
|
continue;
|
|
}
|
|
|
|
const NavConnect &floorConnect =
|
|
floorList->Element(searchIndex);
|
|
newArea = floorConnect.area;
|
|
length = floorConnect.length;
|
|
how = (NavTraverseType)dir;
|
|
++searchIndex;
|
|
|
|
if (IsX360() && searchIndex < floorList->Count()) {
|
|
PREFETCH360(floorList->Element(searchIndex).area, 0);
|
|
}
|
|
} else if (searchWhere == SEARCH_LADDERS) {
|
|
if (searchIndex >= ladderList->Count()) {
|
|
if (!ladderUp) {
|
|
// checked both ladder directions - check elevators next
|
|
searchWhere = SEARCH_ELEVATORS;
|
|
searchIndex = 0;
|
|
ladder = NULL;
|
|
} else {
|
|
// check down ladders
|
|
ladderUp = false;
|
|
ladderList = area->GetLadders(CNavLadder::LADDER_DOWN);
|
|
searchIndex = 0;
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if (ladderUp) {
|
|
ladder = ladderList->Element(searchIndex).ladder;
|
|
|
|
// do not use BEHIND connection, as its very hard to get to
|
|
// when going up a ladder
|
|
if (ladderTopDir == AHEAD) {
|
|
newArea = ladder->m_topForwardArea;
|
|
} else if (ladderTopDir == LEFT) {
|
|
newArea = ladder->m_topLeftArea;
|
|
} else if (ladderTopDir == RIGHT) {
|
|
newArea = ladder->m_topRightArea;
|
|
} else {
|
|
++searchIndex;
|
|
ladderTopDir = AHEAD;
|
|
continue;
|
|
}
|
|
|
|
how = GO_LADDER_UP;
|
|
++ladderTopDir;
|
|
} else {
|
|
newArea =
|
|
ladderList->Element(searchIndex).ladder->m_bottomArea;
|
|
how = GO_LADDER_DOWN;
|
|
ladder = ladderList->Element(searchIndex).ladder;
|
|
++searchIndex;
|
|
}
|
|
|
|
if (newArea == NULL) continue;
|
|
|
|
length = -1.0f;
|
|
} else // if ( searchWhere == SEARCH_ELEVATORS )
|
|
{
|
|
const NavConnectVector &elevatorAreas =
|
|
area->GetElevatorAreas();
|
|
|
|
elevator = area->GetElevator();
|
|
|
|
if (elevator == NULL || searchIndex >= elevatorAreas.Count()) {
|
|
// done searching connected areas
|
|
elevator = NULL;
|
|
break;
|
|
}
|
|
|
|
newArea = elevatorAreas[searchIndex++].area;
|
|
if (newArea->GetCenter().z > area->GetCenter().z) {
|
|
how = GO_ELEVATOR_UP;
|
|
} else {
|
|
how = GO_ELEVATOR_DOWN;
|
|
}
|
|
|
|
length = -1.0f;
|
|
}
|
|
|
|
// don't backtrack
|
|
Assert(newArea);
|
|
if (newArea == area->GetParent()) continue;
|
|
if (newArea == area) // self neighbor?
|
|
continue;
|
|
|
|
// don't consider blocked areas
|
|
if (newArea->IsBlocked(teamID, ignoreNavBlockers)) continue;
|
|
|
|
float newCostSoFar =
|
|
costFunc(newArea, area, ladder, elevator, length);
|
|
|
|
// NaNs really mess this function up causing tough to track down
|
|
// hangs. If
|
|
// we get inf back, clamp it down to a really high number.
|
|
DebuggerBreakOnNaN_StagingOnly(newCostSoFar);
|
|
if (IS_NAN(newCostSoFar)) newCostSoFar = 1e30f;
|
|
|
|
// check if cost functor says this area is a dead-end
|
|
if (newCostSoFar < 0.0f) continue;
|
|
|
|
// Safety check against a bogus functor. The cost of the path
|
|
// A...B, C should always be at least as big as the path A...B.
|
|
Assert(newCostSoFar >= area->GetCostSoFar());
|
|
|
|
// And now that we've asserted, let's be a bit more defensive.
|
|
// Make sure that any jump to a new area incurs some pathfinsing
|
|
// cost, to avoid us spinning our wheels over insignificant cost
|
|
// benefit, floating point precision bug, or busted cost functor.
|
|
float minNewCostSoFar = area->GetCostSoFar() * 1.00001f + 0.00001f;
|
|
newCostSoFar = Max(newCostSoFar, minNewCostSoFar);
|
|
|
|
// stop if path length limit reached
|
|
if (bHaveMaxPathLength) {
|
|
// keep track of path length so far
|
|
float deltaLength =
|
|
(newArea->GetCenter() - area->GetCenter()).Length();
|
|
float newLengthSoFar = area->GetPathLengthSoFar() + deltaLength;
|
|
if (newLengthSoFar > maxPathLength) continue;
|
|
|
|
newArea->SetPathLengthSoFar(newLengthSoFar);
|
|
}
|
|
|
|
if ((newArea->IsOpen() || newArea->IsClosed()) &&
|
|
newArea->GetCostSoFar() <= newCostSoFar) {
|
|
// this is a worse path - skip it
|
|
continue;
|
|
} else {
|
|
// compute estimate of distance left to go
|
|
float distSq =
|
|
(newArea->GetCenter() - actualGoalPos).LengthSqr();
|
|
float newCostRemaining =
|
|
(distSq > 0.0) ? FastSqrt(distSq) : 0.0;
|
|
|
|
// track closest area to goal in case path fails
|
|
if (closestArea && newCostRemaining < closestAreaDist) {
|
|
*closestArea = newArea;
|
|
closestAreaDist = newCostRemaining;
|
|
}
|
|
|
|
newArea->SetCostSoFar(newCostSoFar);
|
|
newArea->SetTotalCost(newCostSoFar + newCostRemaining);
|
|
|
|
if (newArea->IsClosed()) {
|
|
newArea->RemoveFromClosedList();
|
|
}
|
|
|
|
if (newArea->IsOpen()) {
|
|
// area already on open list, update the list order to keep
|
|
// costs sorted
|
|
newArea->UpdateOnOpenList();
|
|
} else {
|
|
newArea->AddToOpenList();
|
|
}
|
|
|
|
newArea->SetParent(area, how);
|
|
}
|
|
}
|
|
|
|
// we have searched this area
|
|
area->AddToClosedList();
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Compute distance between two areas. Return -1 if can't reach 'endArea' from
|
|
* 'startArea'.
|
|
*/
|
|
template <typename CostFunctor>
|
|
float NavAreaTravelDistance(CNavArea *startArea, CNavArea *endArea,
|
|
CostFunctor &costFunc, float maxPathLength = 0.0f) {
|
|
if (startArea == NULL) return -1.0f;
|
|
|
|
if (endArea == NULL) return -1.0f;
|
|
|
|
if (startArea == endArea) return 0.0f;
|
|
|
|
// compute path between areas using given cost heuristic
|
|
if (NavAreaBuildPath(startArea, endArea, NULL, costFunc, NULL,
|
|
maxPathLength) == false)
|
|
return -1.0f;
|
|
|
|
// compute distance along path
|
|
float distance = 0.0f;
|
|
for (CNavArea *area = endArea; area->GetParent();
|
|
area = area->GetParent()) {
|
|
distance +=
|
|
(area->GetCenter() - area->GetParent()->GetCenter()).Length();
|
|
}
|
|
|
|
return distance;
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Do a breadth-first search, invoking functor on each area.
|
|
* If functor returns 'true', continue searching from this area.
|
|
* If functor returns 'false', the area's adjacent areas are not explored (dead
|
|
* end). If 'maxRange' is 0 or less, no range check is done (all areas will be
|
|
* examined).
|
|
*
|
|
* NOTE: Returns all areas that overlap range, even partially
|
|
*
|
|
* @todo Use ladder connections
|
|
*/
|
|
|
|
// helper function
|
|
inline void AddAreaToOpenList(CNavArea *area, CNavArea *parent,
|
|
const Vector &startPos, float maxRange) {
|
|
if (area == NULL) return;
|
|
|
|
if (!area->IsMarked()) {
|
|
area->Mark();
|
|
area->SetTotalCost(0.0f);
|
|
area->SetParent(parent);
|
|
|
|
if (maxRange > 0.0f) {
|
|
// make sure this area overlaps range
|
|
Vector closePos;
|
|
area->GetClosestPointOnArea(startPos, &closePos);
|
|
if ((closePos - startPos).AsVector2D().IsLengthLessThan(maxRange)) {
|
|
// compute approximate distance along path to limit travel
|
|
// range, too
|
|
float distAlong = parent->GetCostSoFar();
|
|
distAlong += (area->GetCenter() - parent->GetCenter()).Length();
|
|
area->SetCostSoFar(distAlong);
|
|
|
|
// allow for some fudge due to large size areas
|
|
if (distAlong <= 1.5f * maxRange) area->AddToOpenList();
|
|
}
|
|
} else {
|
|
// infinite range
|
|
area->AddToOpenList();
|
|
}
|
|
}
|
|
}
|
|
|
|
/****************************************************************
|
|
* DEPRECATED: Use filter-based SearchSurroundingAreas below
|
|
****************************************************************/
|
|
#define INCLUDE_INCOMING_CONNECTIONS 0x1
|
|
#define INCLUDE_BLOCKED_AREAS 0x2
|
|
#define EXCLUDE_OUTGOING_CONNECTIONS 0x4
|
|
#define EXCLUDE_ELEVATORS 0x8
|
|
template <typename Functor>
|
|
void SearchSurroundingAreas(CNavArea *startArea, const Vector &startPos,
|
|
Functor &func, float maxRange = -1.0f,
|
|
unsigned int options = 0, int teamID = TEAM_ANY) {
|
|
if (startArea == NULL) return;
|
|
|
|
CNavArea::MakeNewMarker();
|
|
CNavArea::ClearSearchLists();
|
|
|
|
startArea->AddToOpenList();
|
|
startArea->SetTotalCost(0.0f);
|
|
startArea->SetCostSoFar(0.0f);
|
|
startArea->SetParent(NULL);
|
|
startArea->Mark();
|
|
|
|
while (!CNavArea::IsOpenListEmpty()) {
|
|
// get next area to check
|
|
CNavArea *area = CNavArea::PopOpenList();
|
|
|
|
// don't use blocked areas
|
|
if (area->IsBlocked(teamID) && !(options & INCLUDE_BLOCKED_AREAS))
|
|
continue;
|
|
|
|
// invoke functor on area
|
|
if (func(area)) {
|
|
// explore adjacent floor areas
|
|
for (int dir = 0; dir < NUM_DIRECTIONS; ++dir) {
|
|
int count = area->GetAdjacentCount((NavDirType)dir);
|
|
for (int i = 0; i < count; ++i) {
|
|
CNavArea *adjArea =
|
|
area->GetAdjacentArea((NavDirType)dir, i);
|
|
if (options & EXCLUDE_OUTGOING_CONNECTIONS) {
|
|
if (!adjArea->IsConnected(area, NUM_DIRECTIONS)) {
|
|
continue; // skip this outgoing connection
|
|
}
|
|
}
|
|
|
|
AddAreaToOpenList(adjArea, area, startPos, maxRange);
|
|
}
|
|
}
|
|
|
|
// potentially include areas that connect TO this area via a one-way
|
|
// link
|
|
if (options & INCLUDE_INCOMING_CONNECTIONS) {
|
|
for (int dir = 0; dir < NUM_DIRECTIONS; ++dir) {
|
|
const NavConnectVector *list =
|
|
area->GetIncomingConnections((NavDirType)dir);
|
|
|
|
FOR_EACH_VEC((*list), it) {
|
|
NavConnect connect = (*list)[it];
|
|
|
|
AddAreaToOpenList(connect.area, area, startPos,
|
|
maxRange);
|
|
}
|
|
}
|
|
}
|
|
|
|
// explore adjacent areas connected by ladders
|
|
|
|
// check up ladders
|
|
const NavLadderConnectVector *ladderList =
|
|
area->GetLadders(CNavLadder::LADDER_UP);
|
|
if (ladderList) {
|
|
FOR_EACH_VEC((*ladderList), it) {
|
|
const CNavLadder *ladder = (*ladderList)[it].ladder;
|
|
|
|
// do not use BEHIND connection, as its very hard to get to
|
|
// when going up a ladder
|
|
AddAreaToOpenList(ladder->m_topForwardArea, area, startPos,
|
|
maxRange);
|
|
AddAreaToOpenList(ladder->m_topLeftArea, area, startPos,
|
|
maxRange);
|
|
AddAreaToOpenList(ladder->m_topRightArea, area, startPos,
|
|
maxRange);
|
|
}
|
|
}
|
|
|
|
// check down ladders
|
|
ladderList = area->GetLadders(CNavLadder::LADDER_DOWN);
|
|
if (ladderList) {
|
|
FOR_EACH_VEC((*ladderList), it) {
|
|
const CNavLadder *ladder = (*ladderList)[it].ladder;
|
|
|
|
AddAreaToOpenList(ladder->m_bottomArea, area, startPos,
|
|
maxRange);
|
|
}
|
|
}
|
|
|
|
if ((options & EXCLUDE_ELEVATORS) == 0) {
|
|
const NavConnectVector &elevatorList = area->GetElevatorAreas();
|
|
FOR_EACH_VEC(elevatorList, it) {
|
|
CNavArea *elevatorArea = elevatorList[it].area;
|
|
AddAreaToOpenList(elevatorArea, area, startPos, maxRange);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Derive your own custom search functor from this interface method for use with
|
|
* SearchSurroundingAreas below.
|
|
*/
|
|
class ISearchSurroundingAreasFunctor {
|
|
public:
|
|
virtual ~ISearchSurroundingAreasFunctor() {}
|
|
|
|
/**
|
|
* Perform user-defined action on area.
|
|
* Return 'false' to end the search (ie: you found what you were looking
|
|
* for)
|
|
*/
|
|
virtual bool operator()(CNavArea *area, CNavArea *priorArea,
|
|
float travelDistanceSoFar) = 0;
|
|
|
|
// return true if 'adjArea' should be included in the ongoing search
|
|
virtual bool ShouldSearch(CNavArea *adjArea, CNavArea *currentArea,
|
|
float travelDistanceSoFar) {
|
|
return !adjArea->IsBlocked(TEAM_ANY);
|
|
}
|
|
|
|
/**
|
|
* Collect adjacent areas to continue the search by calling
|
|
* 'IncludeInSearch' on each
|
|
*/
|
|
virtual void IterateAdjacentAreas(CNavArea *area, CNavArea *priorArea,
|
|
float travelDistanceSoFar) {
|
|
// search adjacent outgoing connections
|
|
for (int dir = 0; dir < NUM_DIRECTIONS; ++dir) {
|
|
int count = area->GetAdjacentCount((NavDirType)dir);
|
|
for (int i = 0; i < count; ++i) {
|
|
CNavArea *adjArea = area->GetAdjacentArea((NavDirType)dir, i);
|
|
|
|
if (ShouldSearch(adjArea, area, travelDistanceSoFar)) {
|
|
IncludeInSearch(adjArea, area);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Invoked after the search has completed
|
|
virtual void PostSearch(void) {}
|
|
|
|
// consider 'area' in upcoming search steps
|
|
void IncludeInSearch(CNavArea *area, CNavArea *priorArea) {
|
|
if (area == NULL) return;
|
|
|
|
if (!area->IsMarked()) {
|
|
area->Mark();
|
|
area->SetTotalCost(0.0f);
|
|
area->SetParent(priorArea);
|
|
|
|
// compute approximate travel distance from start area of search
|
|
if (priorArea) {
|
|
float distAlong = priorArea->GetCostSoFar();
|
|
distAlong +=
|
|
(area->GetCenter() - priorArea->GetCenter()).Length();
|
|
area->SetCostSoFar(distAlong);
|
|
} else {
|
|
area->SetCostSoFar(0.0f);
|
|
}
|
|
|
|
// adding an area to the open list also marks it
|
|
area->AddToOpenList();
|
|
}
|
|
}
|
|
};
|
|
|
|
/**
|
|
* Do a breadth-first search starting from 'startArea' and continuing outward
|
|
* based on adjacent areas that pass the given filter
|
|
*/
|
|
inline void SearchSurroundingAreas(CNavArea *startArea,
|
|
ISearchSurroundingAreasFunctor &func,
|
|
float travelDistanceLimit = -1.0f) {
|
|
if (startArea) {
|
|
CNavArea::MakeNewMarker();
|
|
CNavArea::ClearSearchLists();
|
|
|
|
startArea->AddToOpenList();
|
|
startArea->SetTotalCost(0.0f);
|
|
startArea->SetCostSoFar(0.0f);
|
|
startArea->SetParent(NULL);
|
|
startArea->Mark();
|
|
|
|
CUtlVector<CNavArea *> adjVector;
|
|
|
|
while (!CNavArea::IsOpenListEmpty()) {
|
|
// get next area to check
|
|
CNavArea *area = CNavArea::PopOpenList();
|
|
|
|
if (travelDistanceLimit > 0.0f &&
|
|
area->GetCostSoFar() > travelDistanceLimit)
|
|
continue;
|
|
|
|
if (func(area, area->GetParent(), area->GetCostSoFar())) {
|
|
func.IterateAdjacentAreas(area, area->GetParent(),
|
|
area->GetCostSoFar());
|
|
} else {
|
|
// search aborted
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
func.PostSearch();
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Starting from 'startArea', collect adjacent areas via a breadth-first search
|
|
* continuing outward until 'travelDistanceLimit' is reached. Areas in the
|
|
* collection will be "marked", returning true for IsMarked(). Each area in the
|
|
* collection's GetCostSoFar() will be approximate travel distance from
|
|
* 'startArea'.
|
|
*/
|
|
inline void CollectSurroundingAreas(CUtlVector<CNavArea *> *nearbyAreaVector,
|
|
CNavArea *startArea,
|
|
float travelDistanceLimit = 1500.0f,
|
|
float maxStepUpLimit = StepHeight,
|
|
float maxDropDownLimit = 100.0f) {
|
|
nearbyAreaVector->RemoveAll();
|
|
|
|
if (startArea) {
|
|
CNavArea::MakeNewMarker();
|
|
CNavArea::ClearSearchLists();
|
|
|
|
startArea->AddToOpenList();
|
|
startArea->SetTotalCost(0.0f);
|
|
startArea->SetCostSoFar(0.0f);
|
|
startArea->SetParent(NULL);
|
|
startArea->Mark();
|
|
|
|
CUtlVector<CNavArea *> adjVector;
|
|
|
|
while (!CNavArea::IsOpenListEmpty()) {
|
|
// get next area to check
|
|
CNavArea *area = CNavArea::PopOpenList();
|
|
|
|
if (travelDistanceLimit > 0.0f &&
|
|
area->GetCostSoFar() > travelDistanceLimit)
|
|
continue;
|
|
|
|
if (area->GetParent()) {
|
|
float deltaZ =
|
|
area->GetParent()->ComputeAdjacentConnectionHeightChange(
|
|
area);
|
|
|
|
if (deltaZ > maxStepUpLimit) continue;
|
|
|
|
if (deltaZ < -maxDropDownLimit) continue;
|
|
}
|
|
|
|
nearbyAreaVector->AddToTail(area);
|
|
|
|
// mark here to ensure all marked areas are also valid areas that
|
|
// are in the collection
|
|
area->Mark();
|
|
|
|
// search adjacent outgoing connections
|
|
for (int dir = 0; dir < NUM_DIRECTIONS; ++dir) {
|
|
int count = area->GetAdjacentCount((NavDirType)dir);
|
|
for (int i = 0; i < count; ++i) {
|
|
CNavArea *adjArea =
|
|
area->GetAdjacentArea((NavDirType)dir, i);
|
|
|
|
if (adjArea->IsBlocked(TEAM_ANY)) {
|
|
continue;
|
|
}
|
|
|
|
if (!adjArea->IsMarked()) {
|
|
adjArea->SetTotalCost(0.0f);
|
|
adjArea->SetParent(area);
|
|
|
|
// compute approximate travel distance from start area
|
|
// of search
|
|
float distAlong = area->GetCostSoFar();
|
|
distAlong +=
|
|
(adjArea->GetCenter() - area->GetCenter()).Length();
|
|
adjArea->SetCostSoFar(distAlong);
|
|
adjArea->AddToOpenList();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------------------
|
|
/**
|
|
* Functor that returns lowest cost for farthest away areas
|
|
* For use with FindMinimumCostArea()
|
|
*/
|
|
class FarAwayFunctor {
|
|
public:
|
|
float operator()(CNavArea *area, CNavArea *fromArea,
|
|
const CNavLadder *ladder) {
|
|
if (area == fromArea) return 9999999.9f;
|
|
|
|
return 1.0f / (fromArea->GetCenter() - area->GetCenter()).Length();
|
|
}
|
|
};
|
|
|
|
/**
|
|
* Functor that returns lowest cost for areas farthest from given position
|
|
* For use with FindMinimumCostArea()
|
|
*/
|
|
class FarAwayFromPositionFunctor {
|
|
public:
|
|
FarAwayFromPositionFunctor(const Vector &pos) : m_pos(pos) {}
|
|
|
|
float operator()(CNavArea *area, CNavArea *fromArea,
|
|
const CNavLadder *ladder) {
|
|
return 1.0f / (m_pos - area->GetCenter()).Length();
|
|
}
|
|
|
|
private:
|
|
const Vector &m_pos;
|
|
};
|
|
|
|
/**
|
|
* Pick a low-cost area of "decent" size
|
|
*/
|
|
template <typename CostFunctor>
|
|
CNavArea *FindMinimumCostArea(CNavArea *startArea, CostFunctor &costFunc) {
|
|
const float minSize = 150.0f;
|
|
|
|
// collect N low-cost areas of a decent size
|
|
enum { NUM_CHEAP_AREAS = 32 };
|
|
struct {
|
|
CNavArea *area;
|
|
float cost;
|
|
} cheapAreaSet[NUM_CHEAP_AREAS] = {};
|
|
int cheapAreaSetCount = 0;
|
|
|
|
FOR_EACH_VEC(TheNavAreas, iter) {
|
|
CNavArea *area = TheNavAreas[iter];
|
|
|
|
// skip the small areas
|
|
if (area->GetSizeX() < minSize || area->GetSizeY() < minSize) continue;
|
|
|
|
// compute cost of this area
|
|
|
|
// HPE_FIX[pfreese]: changed this to only pass three parameters, in
|
|
// accord with the two functors above
|
|
float cost = costFunc(area, startArea, NULL);
|
|
|
|
if (cheapAreaSetCount < NUM_CHEAP_AREAS) {
|
|
cheapAreaSet[cheapAreaSetCount].area = area;
|
|
cheapAreaSet[cheapAreaSetCount++].cost = cost;
|
|
} else {
|
|
// replace most expensive cost if this is cheaper
|
|
int expensive = 0;
|
|
for (int i = 1; i < NUM_CHEAP_AREAS; ++i)
|
|
if (cheapAreaSet[i].cost > cheapAreaSet[expensive].cost)
|
|
expensive = i;
|
|
|
|
if (cheapAreaSet[expensive].cost > cost) {
|
|
cheapAreaSet[expensive].area = area;
|
|
cheapAreaSet[expensive].cost = cost;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (cheapAreaSetCount) {
|
|
// pick one of the areas at random
|
|
return cheapAreaSet[RandomInt(0, cheapAreaSetCount - 1)].area;
|
|
} else {
|
|
// degenerate case - no decent sized areas - pick a random area
|
|
int numAreas = TheNavAreas.Count();
|
|
int which = RandomInt(0, numAreas - 1);
|
|
|
|
FOR_EACH_VEC(TheNavAreas, iter) {
|
|
if (which-- == 0) return TheNavAreas[iter];
|
|
}
|
|
}
|
|
return cheapAreaSet[RandomInt(0, cheapAreaSetCount - 1)].area;
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------------
|
|
//
|
|
// Given a vector of CNavAreas (or derived types), 'inVector', populate
|
|
// 'outVector' with a randomly shuffled set of 'maxCount' areas that are at
|
|
// least 'minSeparation' travel distance apart from each other.
|
|
//
|
|
template <typename T>
|
|
void SelectSeparatedShuffleSet(int maxCount, float minSeparation,
|
|
const CUtlVector<T *> &inVector,
|
|
CUtlVector<T *> *outVector) {
|
|
if (!outVector) return;
|
|
|
|
outVector->RemoveAll();
|
|
|
|
CUtlVector<T *> shuffledVector;
|
|
|
|
int i, j;
|
|
|
|
for (i = 0; i < inVector.Count(); ++i) {
|
|
shuffledVector.AddToTail(inVector[i]);
|
|
}
|
|
|
|
// randomly shuffle the order
|
|
int n = shuffledVector.Count();
|
|
while (n > 1) {
|
|
int k = RandomInt(0, n - 1);
|
|
n--;
|
|
|
|
T *tmp = shuffledVector[n];
|
|
shuffledVector[n] = shuffledVector[k];
|
|
shuffledVector[k] = tmp;
|
|
}
|
|
|
|
// enforce minSeparation between shuffled areas
|
|
for (i = 0; i < shuffledVector.Count(); ++i) {
|
|
T *area = shuffledVector[i];
|
|
|
|
CUtlVector<CNavArea *> nearVector;
|
|
CollectSurroundingAreas(&nearVector, area, minSeparation,
|
|
2.0f * StepHeight, 2.0f * StepHeight);
|
|
|
|
for (j = 0; j < i; ++j) {
|
|
if (nearVector.HasElement((CNavArea *)shuffledVector[j])) {
|
|
// this area is too near an area earlier in the vector
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (j == i) {
|
|
// separated from all prior areas
|
|
outVector->AddToTail(area);
|
|
|
|
if (outVector->Count() >= maxCount) return;
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif // _NAV_PATHFIND_H_
|