IPath::SearchResult CPathFinder::InitSearch(const MoveDef& moveDef, const CPathFinderDef& pfDef, const CSolidObject* owner, bool synced) {
// If exact path is reqired and the goal is blocked, then no search is needed.
if (exactPath && pfDef.GoalIsBlocked(moveDef, CMoveMath::BLOCK_STRUCTURE, owner))
return IPath::CantGetCloser;
// Clamp the start position
if (startxSqr >= gs->mapx) {
startxSqr = gs->mapxm1;
}
if (startzSqr >= gs->mapy) {
startzSqr = gs->mapym1;
}
const bool isStartGoal = pfDef.IsGoal(startxSqr, startzSqr);
// although our starting square may be inside the goal radius, the starting coordinate may be outside.
// in this case we do not want to return CantGetCloser, but instead a path to our starting square.
if (isStartGoal && pfDef.startInGoalRadius)
return IPath::CantGetCloser;
// Clear the system from last search.
ResetSearch();
// Marks and store the start-square.
squareStates.nodeMask[mStartSquareIdx] = (PATHOPT_START | PATHOPT_OPEN);
squareStates.fCost[mStartSquareIdx] = 0.0f;
squareStates.gCost[mStartSquareIdx] = 0.0f;
squareStates.SetMaxCost(NODE_COST_F, 0.0f);
squareStates.SetMaxCost(NODE_COST_G, 0.0f);
dirtySquares.push_back(mStartSquareIdx);
// Make the beginning the fest square found.
mGoalSquareIdx = mStartSquareIdx;
mGoalHeuristic = pfDef.Heuristic(startxSqr, startzSqr);
// Adding the start-square to the queue.
openSquareBuffer.SetSize(0);
PathNode* os = openSquareBuffer.GetNode(openSquareBuffer.GetSize());
os->fCost = 0.0f;
os->gCost = 0.0f;
os->nodePos.x = startxSqr;
os->nodePos.y = startzSqr;
os->nodeNum = mStartSquareIdx;
openSquares.push(os);
// perform the search
IPath::SearchResult result = DoSearch(moveDef, pfDef, owner, synced);
// if no improvements are found, then return CantGetCloser instead
if ((mGoalSquareIdx == mStartSquareIdx && (!isStartGoal || pfDef.startInGoalRadius)) || mGoalSquareIdx == 0) {
return IPath::CantGetCloser;
}
return result;
}
// set up the starting point of the search
IPath::SearchResult IPathFinder::InitSearch(const MoveDef& moveDef, const CPathFinderDef& pfDef, const CSolidObject* owner)
{
int2 square = mStartBlock;
if (isEstimator)
square = blockStates.peNodeOffsets[moveDef.pathType][mStartBlockIdx];
const bool isStartGoal = pfDef.IsGoal(square.x, square.y);
const bool startInGoal = pfDef.startInGoalRadius;
// although our starting square may be inside the goal radius, the starting coordinate may be outside.
// in this case we do not want to return CantGetCloser, but instead a path to our starting square.
if (isStartGoal && startInGoal)
return IPath::CantGetCloser;
// no, clean the system from last search
ResetSearch();
// mark and store the start-block
blockStates.nodeMask[mStartBlockIdx] &= PATHOPT_OBSOLETE; // clear all except PATHOPT_OBSOLETE
blockStates.nodeMask[mStartBlockIdx] |= PATHOPT_OPEN;
blockStates.fCost[mStartBlockIdx] = 0.0f;
blockStates.gCost[mStartBlockIdx] = 0.0f;
blockStates.SetMaxCost(NODE_COST_F, 0.0f);
blockStates.SetMaxCost(NODE_COST_G, 0.0f);
dirtyBlocks.push_back(mStartBlockIdx);
// start a new search and
// add the starting block to the open-blocks-queue
openBlockBuffer.SetSize(0);
PathNode* ob = openBlockBuffer.GetNode(openBlockBuffer.GetSize());
ob->fCost = 0.0f;
ob->gCost = 0.0f;
ob->nodePos = mStartBlock;
ob->nodeNum = mStartBlockIdx;
openBlocks.push(ob);
// mark starting point as best found position
mGoalBlockIdx = mStartBlockIdx;
mGoalHeuristic = pfDef.Heuristic(square.x, square.y);
// perform the search
const IPath::SearchResult result = DoSearch(moveDef, pfDef, owner);
if (result == IPath::Ok)
return result;
if (mGoalBlockIdx != mStartBlockIdx)
return result;
// if start and goal are within the same block, but distinct squares
// or considered a single point for search purposes, then we probably
// can not get closer
return (!isStartGoal || startInGoal)? IPath::CantGetCloser: result;
}
// set up the starting point of the search
IPath::SearchResult CPathFinder::InitSearch(const MoveData& moveData, const CPathFinderDef& pfDef, int ownerId, bool synced) {
// If exact path is reqired and the goal is blocked, then no search is needed.
if (exactPath && pfDef.GoalIsBlocked(moveData, (CMoveMath::BLOCK_STRUCTURE | CMoveMath::BLOCK_TERRAIN)))
return IPath::CantGetCloser;
// Clamp the start position
if (startxSqr < 0) { startxSqr = 0; }
if (startxSqr >= gs->mapx) { startxSqr = gs->mapx - 1; }
if (startzSqr < 0) { startzSqr = 0; }
if (startzSqr >= gs->mapy) { startzSqr = gs->mapy - 1; }
// If the starting position is a goal position, then no search need to be performed.
if (pfDef.IsGoal(startxSqr, startzSqr))
return IPath::CantGetCloser;
// Clear the system from last search.
ResetSearch();
// Marks and store the start-square.
squareStates[startSquare].nodeMask = (PATHOPT_START | PATHOPT_OPEN);
squareStates[startSquare].fCost = 0.0f;
squareStates[startSquare].gCost = 0.0f;
squareStates.SetMaxFCost(0.0f);
squareStates.SetMaxGCost(0.0f);
dirtySquares.push_back(startSquare);
// Make the beginning the fest square found.
goalSquare = startSquare;
goalHeuristic = pfDef.Heuristic(startxSqr, startzSqr);
// Adding the start-square to the queue.
openSquareBuffer.SetSize(0);
PathNode* os = openSquareBuffer.GetNode(openSquareBuffer.GetSize());
os->fCost = 0.0f;
os->gCost = 0.0f;
os->nodePos.x = startxSqr;
os->nodePos.y = startzSqr;
os->nodeNum = startSquare;
openSquares.push(os);
// perform the search
IPath::SearchResult result = DoSearch(moveData, pfDef, ownerId, synced);
// if no improvements are found, then return CantGetCloser instead
if (goalSquare == startSquare || goalSquare == 0) {
return IPath::CantGetCloser;
}
return result;
}
// set up the starting point of the search
IPath::SearchResult CPathEstimator::InitSearch(const MoveData& moveData, const CPathFinderDef& peDef, bool synced) {
// is starting square inside goal area?
const int xSquare = blockStates[startBlocknr].nodeOffsets[moveData.pathType].x;
const int zSquare = blockStates[startBlocknr].nodeOffsets[moveData.pathType].y;
if (peDef.IsGoal(xSquare, zSquare))
return IPath::CantGetCloser;
// no, clean the system from last search
ResetSearch();
// mark and store the start-block
blockStates[startBlocknr].nodeMask |= PATHOPT_OPEN;
blockStates[startBlocknr].fCost = 0.0f;
blockStates[startBlocknr].gCost = 0.0f;
blockStates.SetMaxFCost(0.0f);
blockStates.SetMaxGCost(0.0f);
dirtyBlocks.push_back(startBlocknr);
openBlockBuffer.SetSize(0);
// add the starting block to the open-blocks-queue
PathNode* ob = openBlockBuffer.GetNode(openBlockBuffer.GetSize());
ob->fCost = 0.0f;
ob->gCost = 0.0f;
ob->nodePos = startBlock;
ob->nodeNum = startBlocknr;
openBlocks.push(ob);
// mark starting point as best found position
goalBlock = startBlock;
goalHeuristic = peDef.Heuristic(xSquare, zSquare);
// get the goal square offset
goalSqrOffset = peDef.GoalSquareOffset(BLOCK_SIZE);
// perform the search
IPath::SearchResult result = DoSearch(moveData, peDef, synced);
// if no improvements are found, then return CantGetCloser instead
if (goalBlock.x == startBlock.x && goalBlock.y == startBlock.y) {
return IPath::CantGetCloser;
}
return result;
}
/*
Setting up the starting point of the search.
*/
IPath::SearchResult CPathFinder::InitSearch(const MoveData& moveData, const CPathFinderDef& pfDef) {
//If exact path is reqired and the goal is blocked, then no search is needed.
if(exactPath && pfDef.GoalIsBlocked(moveData, (CMoveMath::BLOCK_STRUCTURE | CMoveMath::BLOCK_TERRAIN)))
return CantGetCloser;
//Clamp the start position
if (startxSqr < 0) startxSqr=0;
if (startxSqr >= gs->mapx) startxSqr = gs->mapx-1;
if (startzSqr < 0) startzSqr =0;
if (startzSqr >= gs->mapy) startzSqr = gs->mapy-1;
//If the starting position is a goal position, then no search need to be performed.
if(pfDef.IsGoal(startxSqr, startzSqr))
return CantGetCloser;
//Clearing the system from last search.
ResetSearch();
//Marks and store the start-square.
squareState[startSquare].status = (PATHOPT_START | PATHOPT_OPEN);
squareState[startSquare].cost = 0;
dirtySquares.push_back(startSquare);
//Make the beginning the fest square found.
goalSquare = startSquare;
goalHeuristic = pfDef.Heuristic(startxSqr, startzSqr);
//Adding the start-square to the queue.
openSquareBufferPointer = &openSquareBuffer[0];
OpenSquare *os = openSquareBufferPointer; //Taking first OpenSquare in buffer.
os->currentCost = 0;
os->cost = 0;
os->square.x = startxSqr;
os->square.y = startzSqr;
os->sqr = startSquare;
openSquares.push(os);
//Performs the search.
SearchResult result = DoSearch(moveData, pfDef);
//If no improvement has been found then return CantGetCloser instead.
if(goalSquare == startSquare || goalSquare == 0) {
return CantGetCloser;
} else
return result;
}
/**
* Make some initial calculations and preparations
*/
IPath::SearchResult CPathEstimator::InitSearch(const MoveData& moveData, const CPathFinderDef& peDef) {
// is starting square inside goal area?
int xSquare = blockState[startBlocknr].sqrCenter[moveData.pathType].x;
int zSquare = blockState[startBlocknr].sqrCenter[moveData.pathType].y;
if (peDef.IsGoal(xSquare, zSquare))
return CantGetCloser;
// no, clean the system from last search
ResetSearch();
// mark and store the start-block
blockState[startBlocknr].options |= PATHOPT_OPEN;
blockState[startBlocknr].cost = 0;
dirtyBlocks.push_back(startBlocknr);
openBlockBufferIndex = 0;
// add the starting block to the open-blocks-queue
OpenBlock* ob = &openBlockBuffer[openBlockBufferIndex];
ob->cost = 0;
ob->currentCost = 0;
ob->block = startBlock;
ob->blocknr = startBlocknr;
openBlocks.push(ob);
// mark starting point as best found position
goalBlock = startBlock;
goalHeuristic = peDef.Heuristic(xSquare, zSquare);
// get the goal square offset
goalSqrOffset = peDef.GoalSquareOffset(BLOCK_SIZE);
// perform the search
SearchResult result = DoSearch(moveData, peDef);
// if no improvements are found, then return CantGetCloser instead
if (goalBlock.x == startBlock.x && goalBlock.y == startBlock.y)
return CantGetCloser;
else
return result;
}
/*
Making some initial calculations and preparations.
*/
IPath::SearchResult CPathEstimator::InitSearch(const MoveData& moveData, const CPathFinderDef& peDef) {
//Starting square is inside goal area?
int xSquare = blockState[startBlocknr].sqrCenter[moveData.pathType].x;
int zSquare = blockState[startBlocknr].sqrCenter[moveData.pathType].y;
if(peDef.IsGoal(xSquare, zSquare))
return CantGetCloser;
//Cleaning the system from last search.
ResetSearch();
//Marks and store the start-block.
blockState[startBlocknr].options |= PATHOPT_OPEN;
blockState[startBlocknr].cost = 0;
dirtyBlocks.push_back(startBlocknr);
//Adding the starting block to the open-blocks-queue.
OpenBlock* ob = openBlockBufferPointer = openBlockBuffer;
ob->cost = 0;
ob->currentCost = 0;
ob->block = startBlock;
ob->blocknr = startBlocknr;
openBlocks.push(ob);
//Mark starting point as best found position.
goalBlock = startBlock;
goalHeuristic = peDef.Heuristic(xSquare, zSquare);
//Gets goal square offset.
goalSqrOffset = peDef.GoalSquareOffset(BLOCK_SIZE);
//Performs the search.
SearchResult result = DoSearch(moveData, peDef);
//If no improvements are found, then return CantGetCloser instead.
if(goalBlock.x == startBlock.x && goalBlock.y == startBlock.y)
return CantGetCloser;
else
return result;
}
/**
* Test the accessability of a block and its value,
* possibly also add it to the open-blocks pqueue.
*/
void CPathEstimator::TestBlock(
const MoveData& moveData,
const CPathFinderDef& peDef,
PathNode& parentOpenBlock,
unsigned int direction,
bool synced
) {
testedBlocks++;
// initial calculations of the new block
int2 block;
block.x = parentOpenBlock.nodePos.x + directionVector[direction].x;
block.y = parentOpenBlock.nodePos.y + directionVector[direction].y;
const int vertexIdx =
moveData.pathType * blockStates.GetSize() * PATH_DIRECTION_VERTICES +
parentOpenBlock.nodeNum * PATH_DIRECTION_VERTICES +
directionVertex[direction];
const int blockIdx = block.y * nbrOfBlocksX + block.x;
if (block.x < 0 || block.x >= nbrOfBlocksX || block.y < 0 || block.y >= nbrOfBlocksZ) {
// blocks should never be able to lie outside map to the infinite vertices at the edges
return;
}
if (vertexIdx < 0 || (unsigned int)vertexIdx >= vertices.size())
return;
if (vertices[vertexIdx] >= PATHCOST_INFINITY)
return;
// check if the block is unavailable
if (blockStates[blockIdx].nodeMask & (PATHOPT_FORBIDDEN | PATHOPT_BLOCKED | PATHOPT_CLOSED))
return;
const int xSquare = blockStates[blockIdx].nodeOffsets[moveData.pathType].x;
const int zSquare = blockStates[blockIdx].nodeOffsets[moveData.pathType].y;
// check if the block is blocked or out of constraints
if (!peDef.WithinConstraints(xSquare, zSquare)) {
blockStates[blockIdx].nodeMask |= PATHOPT_BLOCKED;
dirtyBlocks.push_back(blockIdx);
return;
}
// evaluate this node (NOTE the max-res. indexing for extraCost)
const float extraCost = blockStates.GetNodeExtraCost(xSquare, zSquare, synced);
const float nodeCost = vertices[vertexIdx] + extraCost;
const float gCost = parentOpenBlock.gCost + nodeCost; // g
const float hCost = peDef.Heuristic(xSquare, zSquare); // h
const float fCost = gCost + hCost; // f
if (blockStates[blockIdx].nodeMask & PATHOPT_OPEN) {
// already in the open set
if (blockStates[blockIdx].fCost <= fCost)
return;
blockStates[blockIdx].nodeMask &= ~PATHOPT_DIRECTION;
}
// look for improvements
if (hCost < goalHeuristic) {
goalBlock = block;
goalHeuristic = hCost;
}
// store this block as open.
openBlockBuffer.SetSize(openBlockBuffer.GetSize() + 1);
assert(openBlockBuffer.GetSize() < MAX_SEARCHED_NODES_PE);
PathNode* ob = openBlockBuffer.GetNode(openBlockBuffer.GetSize());
ob->fCost = fCost;
ob->gCost = gCost;
ob->nodePos = block;
ob->nodeNum = blockIdx;
openBlocks.push(ob);
blockStates.SetMaxFCost(std::max(blockStates.GetMaxFCost(), fCost));
blockStates.SetMaxGCost(std::max(blockStates.GetMaxGCost(), gCost));
// mark this block as open
blockStates[blockIdx].fCost = fCost;
blockStates[blockIdx].gCost = gCost;
blockStates[blockIdx].nodeMask |= (direction | PATHOPT_OPEN);
blockStates[blockIdx].parentNodePos = parentOpenBlock.nodePos;
dirtyBlocks.push_back(blockIdx);
}
IPath::SearchResult CPathManager::ArrangePath(
MultiPath* newPath,
const MoveDef* moveDef,
const float3& startPos,
const float3& goalPos,
CSolidObject* caller
) const {
CPathFinderDef* pfDef = &newPath->peDef;
// choose the PF or the PE depending on the projected 2D goal-distance
// NOTE: this distance can be far smaller than the actual path length!
// NOTE: take height difference into consideration for "special" cases
// (unit at top of cliff, goal at bottom or vv.)
const float heurGoalDist2D = pfDef->Heuristic(startPos.x / SQUARE_SIZE, startPos.z / SQUARE_SIZE, 1) + math::fabs(goalPos.y - startPos.y) / SQUARE_SIZE;
const float searchDistances[] = {std::numeric_limits<float>::max(), MEDRES_SEARCH_DISTANCE, MAXRES_SEARCH_DISTANCE};
// MAX_SEARCHED_NODES_PF is 65536, MAXRES_SEARCH_DISTANCE is 50 squares
// the circular-constraint area therefore is PI*50*50 squares (i.e. 7854
// rounded up to nearest integer) which means MAX_SEARCHED_NODES_*>>3 is
// only slightly larger (8192) so the constraint has no purpose even for
// max-res queries (!)
assert(MAX_SEARCHED_NODES_PF <= 65536u);
assert(MAXRES_SEARCH_DISTANCE <= 50.0f);
constexpr unsigned int nodeLimits[] = {MAX_SEARCHED_NODES_PE >> 3, MAX_SEARCHED_NODES_PE >> 3, MAX_SEARCHED_NODES_PF >> 3};
constexpr bool useConstraints[] = {false, false, false};
constexpr bool allowRawSearch[] = {false, false, false};
IPathFinder* pathFinders[] = {lowResPE, medResPE, maxResPF};
IPath::Path* pathObjects[] = {&newPath->lowResPath, &newPath->medResPath, &newPath->maxResPath};
IPath::SearchResult bestResult = IPath::Error;
#if 1
unsigned int bestSearch = -1u; // index
enum {
PATH_LOW_RES = 0,
PATH_MED_RES = 1,
PATH_MAX_RES = 2,
};
{
if (heurGoalDist2D <= (MAXRES_SEARCH_DISTANCE * modInfo.pfRawDistMult)) {
pfDef->AllowRawPathSearch( true);
pfDef->AllowDefPathSearch(false); // block default search
// only the max-res CPathFinder implements DoRawSearch
bestResult = pathFinders[PATH_MAX_RES]->GetPath(*moveDef, *pfDef, caller, startPos, *pathObjects[PATH_MAX_RES], nodeLimits[PATH_MAX_RES]);
bestSearch = PATH_MAX_RES;
pfDef->AllowRawPathSearch(false);
pfDef->AllowDefPathSearch( true);
}
if (bestResult != IPath::Ok) {
// try each pathfinder in order from MAX to LOW limited by distance,
// with constraints disabled for all three since these break search
// completeness (CPU usage is still limited by MAX_SEARCHED_NODES_*)
for (int n = PATH_MAX_RES; n >= PATH_LOW_RES; n--) {
// distance-limits are in ascending order
if (heurGoalDist2D > searchDistances[n])
continue;
pfDef->DisableConstraint(!useConstraints[n]);
pfDef->AllowRawPathSearch(allowRawSearch[n]);
const IPath::SearchResult currResult = pathFinders[n]->GetPath(*moveDef, *pfDef, caller, startPos, *pathObjects[n], nodeLimits[n]);
// note: GEQ s.t. MED-OK will be preferred over LOW-OK, etc
if (currResult >= bestResult)
continue;
bestResult = currResult;
bestSearch = n;
if (currResult == IPath::Ok)
break;
}
}
}
for (unsigned int n = PATH_LOW_RES; n <= PATH_MAX_RES; n++) {
if (n != bestSearch) {
pathObjects[n]->path.clear();
pathObjects[n]->squares.clear();
}
}
if (bestResult == IPath::Ok)
return bestResult;
// if we did not get a complete path with distance/search
// constraints enabled, run a final unconstrained fallback
// MED search (unconstrained MAX search is not useful with
// current node limits and could kill performance without)
if (heurGoalDist2D > searchDistances[PATH_MED_RES]) {
pfDef->DisableConstraint(true);
// we can only have a low-res result at this point
pathObjects[PATH_LOW_RES]->path.clear();
pathObjects[PATH_LOW_RES]->squares.clear();
bestResult = std::min(bestResult, pathFinders[PATH_MED_RES]->GetPath(*moveDef, *pfDef, caller, startPos, *pathObjects[PATH_MED_RES], nodeLimits[PATH_MED_RES]));
}
return bestResult;
#else
enum {
PATH_MAX_RES = 0,
PATH_MED_RES = 1,
PATH_LOW_RES = 3
};
int origPathRes = PATH_LOW_RES;
// first attempt - use ideal pathfinder (performance-wise)
{
if (heurGoalDist2D < MAXRES_SEARCH_DISTANCE) {
origPathRes = PATH_MAX_RES;
} else if (heurGoalDist2D < MEDRES_SEARCH_DISTANCE) {
origPathRes = PATH_MED_RES;
//} else {
// origPathRes = PATH_LOW_RES;
}
switch (origPathRes) {
case PATH_MAX_RES: bestResult = maxResPF->GetPath(*moveDef, *pfDef, caller, startPos, newPath->maxResPath, nodeLimits[2]); break;
case PATH_MED_RES: bestResult = medResPE->GetPath(*moveDef, *pfDef, caller, startPos, newPath->medResPath, nodeLimits[1]); break;
case PATH_LOW_RES: bestResult = lowResPE->GetPath(*moveDef, *pfDef, caller, startPos, newPath->lowResPath, nodeLimits[0]); break;
}
if (bestResult == IPath::Ok) {
return bestResult;
}
}
// second attempt - try to reverse path
/*{
CCircularSearchConstraint reversedPfDef(goalPos, startPos, pfDef->sqGoalRadius, 7.0f, 8000);
switch (pathres) {
case PATH_MAX_RES: bestResult = maxResPF->GetPath(*moveDef, reversedPfDef, caller, goalPos, newPath->maxResPath, nodeLimits[2]); break;
case PATH_MED_RES: bestResult = medResPE->GetPath(*moveDef, reversedPfDef, caller, goalPos, newPath->medResPath, nodeLimits[1]); break;
case PATH_LOW_RES: bestResult = lowResPE->GetPath(*moveDef, reversedPfDef, caller, goalPos, newPath->lowResPath, nodeLimits[0]); break;
}
if (bestResult == IPath::Ok) {
assert(false);
float3 midPos;
switch (pathres) {
case PATH_MAX_RES: midPos = newPath->maxResPath.path.back(); break;
case PATH_MED_RES: midPos = newPath->medResPath.path.back(); break;
case PATH_LOW_RES: midPos = newPath->lowResPath.path.back(); break;
}
CCircularSearchConstraint midPfDef(startPos, midPos, pfDef->sqGoalRadius, 3.0f, 8000);
bestResult = maxResPF->GetPath(*moveDef, midPfDef, caller, startPos, newPath->maxResPath, MAX_SEARCHED_NODES_PF >> 3);
CCircularSearchConstraint restPfDef(midPos, goalPos, pfDef->sqGoalRadius, 7.0f, 8000);
switch (pathres) {
case PATH_MAX_RES:
case PATH_MED_RES: bestResult = medResPE->GetPath(*moveDef, restPfDef, caller, startPos, newPath->medResPath, nodeLimits[1]); break;
case PATH_LOW_RES: bestResult = lowResPE->GetPath(*moveDef, restPfDef, caller, startPos, newPath->lowResPath, nodeLimits[0]); break;
}
return bestResult;
}
}*/
// third attempt - use better pathfinder
{
int advPathRes = origPathRes;
int maxRes = (heurGoalDist2D < (MAXRES_SEARCH_DISTANCE * 2.0f)) ? PATH_MAX_RES : PATH_MED_RES;
while (--advPathRes >= maxRes) {
switch (advPathRes) {
case PATH_MAX_RES: bestResult = maxResPF->GetPath(*moveDef, *pfDef, caller, startPos, newPath->maxResPath, nodeLimits[2]); break;
case PATH_MED_RES: bestResult = medResPE->GetPath(*moveDef, *pfDef, caller, startPos, newPath->medResPath, nodeLimits[1]); break;
case PATH_LOW_RES: bestResult = lowResPE->GetPath(*moveDef, *pfDef, caller, startPos, newPath->lowResPath, nodeLimits[0]); break;
}
if (bestResult == IPath::Ok) {
return bestResult;
}
}
}
// fourth attempt - unconstrained search radius (performance heavy, esp. on max_res)
pfDef->DisableConstraint(true);
if (origPathRes > PATH_MAX_RES) {
int advPathRes = origPathRes;
int maxRes = PATH_MED_RES;
while (--advPathRes >= maxRes) {
switch (advPathRes) {
case PATH_MAX_RES: bestResult = maxResPF->GetPath(*moveDef, *pfDef, caller, startPos, newPath->maxResPath, nodeLimits[2]); break;
case PATH_MED_RES: bestResult = medResPE->GetPath(*moveDef, *pfDef, caller, startPos, newPath->medResPath, nodeLimits[1]); break;
case PATH_LOW_RES: bestResult = lowResPE->GetPath(*moveDef, *pfDef, caller, startPos, newPath->lowResPath, nodeLimits[0]); break;
}
if (bestResult == IPath::Ok) {
return bestResult;
}
}
}
LOG_L(L_DEBUG, "PathManager: no path found");
return bestResult;
#endif
}
/*
Test the accessability of a block and it's value
and possibly add it to the open-blocks-queue.
*/
void CPathEstimator::TestBlock(const MoveData& moveData, const CPathFinderDef &peDef, OpenBlock& parentOpenBlock, unsigned int direction) {
testedBlocks++;
//Initial calculations of the new block.
int2 block;
block.x = parentOpenBlock.block.x + directionVector[direction].x;
block.y = parentOpenBlock.block.y + directionVector[direction].y;
int vertexNbr = moveData.pathType * nbrOfBlocks * PATH_DIRECTION_VERTICES + parentOpenBlock.blocknr * PATH_DIRECTION_VERTICES + directionVertex[direction];
//Outside map?
if(/*block.x < 0 || block.x >= nbrOfBlocksX //the blocks should never be able to become wrong due to the infinite vertices at the edges
|| block.y < 0 || block.y >= nbrOfBlocksZ
||*/ vertexNbr < 0 || vertexNbr >= nbrOfVertices)
return;
int blocknr = block.y * nbrOfBlocksX + block.x;
float blockCost = vertex[vertexNbr];
if(blockCost >= PATHCOST_INFINITY)
return;
//Check if the block is unavailable.
if(blockState[blocknr].options & (PATHOPT_FORBIDDEN | PATHOPT_BLOCKED | PATHOPT_CLOSED))
return;
int xSquare = blockState[blocknr].sqrCenter[moveData.pathType].x;
int zSquare = blockState[blocknr].sqrCenter[moveData.pathType].y;
//Check if the block is blocked or out of constraints.
if(!peDef.WithinConstraints(xSquare, zSquare)) {
blockState[blocknr].options |= PATHOPT_BLOCKED;
dirtyBlocks.push_back(blocknr);
return;
}
//Evaluate this node.
float heuristicCost = peDef.Heuristic(xSquare, zSquare);
float currentCost = parentOpenBlock.currentCost + blockCost;
float cost = currentCost + heuristicCost;
//Check if the block is already in queue and better, then keep it.
if(blockState[blocknr].options & PATHOPT_OPEN) {
if(blockState[blocknr].cost <= cost)
return;
blockState[blocknr].options &= 255-7;
}
//Looking for improvements.
if(heuristicCost < goalHeuristic) {
goalBlock = block;
goalHeuristic = heuristicCost;
}
//Store this block as open.
OpenBlock* ob = ++openBlockBufferPointer;
ob->block = block;
ob->blocknr = blocknr;
ob->cost = cost;
ob->currentCost = currentCost;
openBlocks.push(ob);
//Mark the block as open, and it's parent.
blockState[blocknr].cost = cost;
blockState[blocknr].options |= (direction | PATHOPT_OPEN);
blockState[blocknr].parentBlock = parentOpenBlock.block;
dirtyBlocks.push_back(blocknr);
}
// set up the starting point of the search
IPath::SearchResult IPathFinder::InitSearch(const MoveDef& moveDef, const CPathFinderDef& pfDef, const CSolidObject* owner)
{
int2 square = mStartBlock;
if (BLOCK_SIZE != 1)
square = blockStates.peNodeOffsets[moveDef.pathType][mStartBlockIdx];
const bool isStartGoal = pfDef.IsGoal(square.x, square.y);
const bool startInGoal = pfDef.startInGoalRadius;
const bool allowRawPath = pfDef.allowRawPath;
const bool allowDefPath = pfDef.allowDefPath;
assert(allowRawPath || allowDefPath);
// cleanup after the last search
ResetSearch();
IPath::SearchResult results[] = {IPath::CantGetCloser, IPath::Ok, IPath::CantGetCloser};
// although our starting square may be inside the goal radius, the starting coordinate may be outside.
// in this case we do not want to return CantGetCloser, but instead a path to our starting square.
if (isStartGoal && startInGoal)
return results[allowRawPath];
// mark and store the start-block; clear all bits except PATHOPT_OBSOLETE
blockStates.nodeMask[mStartBlockIdx] &= PATHOPT_OBSOLETE;
blockStates.nodeMask[mStartBlockIdx] |= PATHOPT_OPEN;
blockStates.fCost[mStartBlockIdx] = 0.0f;
blockStates.gCost[mStartBlockIdx] = 0.0f;
blockStates.SetMaxCost(NODE_COST_F, 0.0f);
blockStates.SetMaxCost(NODE_COST_G, 0.0f);
dirtyBlocks.push_back(mStartBlockIdx);
// start a new search and add the starting block to the open-blocks-queue
openBlockBuffer.SetSize(0);
PathNode* ob = openBlockBuffer.GetNode(openBlockBuffer.GetSize());
ob->fCost = 0.0f;
ob->gCost = 0.0f;
ob->nodePos = mStartBlock;
ob->nodeNum = mStartBlockIdx;
openBlocks.push(ob);
// mark starting point as best found position
mGoalHeuristic = pfDef.Heuristic(square.x, square.y, BLOCK_SIZE);
enum {
RAW = 0,
IPF = 1,
};
// perform the search
results[RAW] = (allowRawPath )? DoRawSearch(moveDef, pfDef, owner): IPath::Error;
results[IPF] = (allowDefPath && results[RAW] == IPath::Error)? DoSearch(moveDef, pfDef, owner): results[RAW];
if (results[IPF] == IPath::Ok)
return IPath::Ok;
if (mGoalBlockIdx != mStartBlockIdx)
return results[IPF];
// if start and goal are within the same block but distinct squares (or
// considered a single point for search purposes), then we probably can
// not get closer and should return CGC *unless* the caller requested a
// raw search only
return results[IPF + ((!allowRawPath || allowDefPath) && (!isStartGoal || startInGoal))];
}
bool CPathFinder::TestSquare(
const MoveDef& moveDef,
const CPathFinderDef& pfDef,
const PathNode* parentOpenSquare,
unsigned int enterDirection,
int ownerId,
bool synced
) {
testedNodes++;
const int2& dirVec2D = dirVectors2D[enterDirection];
const float3& dirVec3D = dirVectors3D[enterDirection];
// Calculate the new square.
int2 square;
square.x = parentOpenSquare->nodePos.x + dirVec2D.x;
square.y = parentOpenSquare->nodePos.y + dirVec2D.y;
// Inside map?
if (square.x < 0 || square.y < 0 || square.x >= gs->mapx || square.y >= gs->mapy) {
return false;
}
const int sqrIdx = square.x + square.y * gs->mapx;
const int sqrStatus = squareStates.nodeMask[sqrIdx];
// Check if the square is unaccessable or used.
if (sqrStatus & (PATHOPT_CLOSED | PATHOPT_FORBIDDEN | PATHOPT_BLOCKED)) {
return false;
}
const CMoveMath::BlockType blockStatus = moveDef.moveMath->IsBlocked(moveDef, square.x, square.y);
// Check if square are out of constraints or blocked by something.
// Doesn't need to be done on open squares, as those are already tested.
if (!(sqrStatus & PATHOPT_OPEN) &&
((blockStatus & CMoveMath::BLOCK_STRUCTURE) || !pfDef.WithinConstraints(square.x, square.y))
) {
squareStates.nodeMask[sqrIdx] |= PATHOPT_BLOCKED;
dirtySquares.push_back(sqrIdx);
return false;
}
// Evaluate this square.
float squareSpeedMod = moveDef.moveMath->GetPosSpeedMod(moveDef, square.x, square.y, dirVec3D);
float heatCostMod = 1.0f;
if (squareSpeedMod == 0.0f) {
squareStates.nodeMask[sqrIdx] |= PATHOPT_FORBIDDEN;
dirtySquares.push_back(sqrIdx);
return false;
}
if (testMobile && moveDef.avoidMobilesOnPath && (blockStatus & squareMobileBlockBits)) {
if (blockStatus & CMoveMath::BLOCK_MOBILE_BUSY) {
squareSpeedMod *= moveDef.speedModMults[MoveDef::SPEEDMOD_MOBILE_BUSY_MULT];
} else if (blockStatus & CMoveMath::BLOCK_MOBILE) {
squareSpeedMod *= moveDef.speedModMults[MoveDef::SPEEDMOD_MOBILE_IDLE_MULT];
} else { // (blockStatus & CMoveMath::BLOCK_MOVING)
squareSpeedMod *= moveDef.speedModMults[MoveDef::SPEEDMOD_MOBILE_MOVE_MULT];
}
}
// Include heatmap cost adjustment.
if (heatMapping && moveDef.heatMapping && GetHeatOwner(square.x, square.y) != ownerId) {
heatCostMod += (moveDef.heatMod * GetHeatValue(square.x, square.y));
}
const float dirMoveCost = (heatCostMod * moveCost[enterDirection]);
const float extraCost = squareStates.GetNodeExtraCost(square.x, square.y, synced);
const float nodeCost = (dirMoveCost / squareSpeedMod) + extraCost;
const float gCost = parentOpenSquare->gCost + nodeCost; // g
const float hCost = pfDef.Heuristic(square.x, square.y); // h
const float fCost = gCost + hCost; // f
if (squareStates.nodeMask[sqrIdx] & PATHOPT_OPEN) {
// already in the open set
if (squareStates.fCost[sqrIdx] <= fCost)
return true;
squareStates.nodeMask[sqrIdx] &= ~PATHOPT_DIRECTION;
}
// Look for improvements.
if (!exactPath && hCost < goalHeuristic) {
goalSquare = sqrIdx;
goalHeuristic = hCost;
}
// Store this square as open.
openSquareBuffer.SetSize(openSquareBuffer.GetSize() + 1);
assert(openSquareBuffer.GetSize() < MAX_SEARCHED_NODES_PF);
PathNode* os = openSquareBuffer.GetNode(openSquareBuffer.GetSize());
os->fCost = fCost;
os->gCost = gCost;
os->nodePos = square;
os->nodeNum = sqrIdx;
openSquares.push(os);
squareStates.SetMaxFCost(std::max(squareStates.GetMaxFCost(), fCost));
squareStates.SetMaxGCost(std::max(squareStates.GetMaxGCost(), gCost));
// mark this square as open
squareStates.fCost[sqrIdx] = os->fCost;
squareStates.gCost[sqrIdx] = os->gCost;
squareStates.nodeMask[sqrIdx] |= (PATHOPT_OPEN | enterDirection);
dirtySquares.push_back(sqrIdx);
return true;
}
/**
* Test the accessability of a block and its value,
* possibly also add it to the open-blocks pqueue.
*/
void CPathEstimator::TestBlock(const MoveData& moveData, const CPathFinderDef &peDef, OpenBlock& parentOpenBlock, unsigned int direction) {
testedBlocks++;
// initial calculations of the new block
int2 block;
block.x = parentOpenBlock.block.x + directionVector[direction].x;
block.y = parentOpenBlock.block.y + directionVector[direction].y;
int vertexNbr = moveData.pathType * nbrOfBlocks * PATH_DIRECTION_VERTICES + parentOpenBlock.blocknr * PATH_DIRECTION_VERTICES + directionVertex[direction];
/*
if (block.x < 0 || block.x >= nbrOfBlocksX || block.y < 0 || block.y >= nbrOfBlocksZ) {
// blocks should never be able to lie outside map to the infinite vertices at the edges
return;
}
*/
if (vertexNbr < 0 || (unsigned int)vertexNbr >= nbrOfVertices)
return;
int blocknr = block.y * nbrOfBlocksX + block.x;
float blockCost = vertex[vertexNbr];
if (blockCost >= PATHCOST_INFINITY)
return;
// check if the block is unavailable
if (blockState[blocknr].options & (PATHOPT_FORBIDDEN | PATHOPT_BLOCKED | PATHOPT_CLOSED))
return;
int xSquare = blockState[blocknr].sqrCenter[moveData.pathType].x;
int zSquare = blockState[blocknr].sqrCenter[moveData.pathType].y;
// check if the block is blocked or out of constraints
if (!peDef.WithinConstraints(xSquare, zSquare)) {
blockState[blocknr].options |= PATHOPT_BLOCKED;
dirtyBlocks.push_back(blocknr);
return;
}
// evaluate this node
float heuristicCost = peDef.Heuristic(xSquare, zSquare);
float currentCost = parentOpenBlock.currentCost + blockCost;
float cost = currentCost + heuristicCost;
// check if the block is already in queue and keep it if it's better
if (blockState[blocknr].options & PATHOPT_OPEN) {
if (blockState[blocknr].cost <= cost)
return;
blockState[blocknr].options &= 255 - 7;
}
// look for improvements
if (heuristicCost < goalHeuristic) {
goalBlock = block;
goalHeuristic = heuristicCost;
}
// store this block as open.
++openBlockBufferIndex;
assert(openBlockBufferIndex < MAX_SEARCHED_BLOCKS);
OpenBlock* ob = &openBlockBuffer[openBlockBufferIndex];
ob->block = block;
ob->blocknr = blocknr;
ob->cost = cost;
ob->currentCost = currentCost;
openBlocks.push(ob);
// Mark the block as open, and its parent.
blockState[blocknr].cost = cost;
blockState[blocknr].options |= (direction | PATHOPT_OPEN);
blockState[blocknr].parentBlock = parentOpenBlock.block;
dirtyBlocks.push_back(blocknr);
}
/*
Test the availability and value of a square,
and possibly add it to the queue of open squares.
*/
bool CPathFinder::TestSquare(const MoveData& moveData, const CPathFinderDef& pfDef, OpenSquare* parentOpenSquare, unsigned int enterDirection) {
testedNodes++;
//Calculate the new square.
int2 square;
square.x = parentOpenSquare->square.x + directionVector[enterDirection].x;
square.y = parentOpenSquare->square.y + directionVector[enterDirection].y;
//Inside map?
if(square.x < 0 || square.y < 0 || square.x >= gs->mapx || square.y >= gs->mapy)
return false;
int sqr = square.x + square.y * gs->mapx;
//Check if the square is unaccessable or used.
if(squareState[sqr].status & (PATHOPT_CLOSED | PATHOPT_FORBIDDEN | PATHOPT_BLOCKED)) {
if(squareState[sqr].status & PATHOPT_BLOCKED)
return false;
else
return false;
}
int blockStatus=moveData.moveMath->IsBlocked2(moveData, square.x, square.y);
//Check if square are out of constraints or blocked by something.
//Don't need to be done on open squares, as whose are already tested.
if(!(squareState[sqr].status & PATHOPT_OPEN) && (!pfDef.WithinConstraints(square.x, square.y) || (blockStatus & (CMoveMath::BLOCK_STRUCTURE | CMoveMath::BLOCK_TERRAIN)))) {
squareState[sqr].status |= PATHOPT_BLOCKED;
dirtySquares.push_back(sqr);
return false;
}
//Evaluate this node.
float squareSpeedMod = moveData.moveMath->SpeedMod(moveData, square.x, square.y);
if(squareSpeedMod==0) {
squareState[sqr].status |= PATHOPT_FORBIDDEN;
dirtySquares.push_back(sqr);
return false;
}
if(testMobile && (blockStatus & (CMoveMath::BLOCK_MOBILE | CMoveMath::BLOCK_MOVING | CMoveMath::BLOCK_MOBILE_BUSY))) {
if(blockStatus & CMoveMath::BLOCK_MOVING)
squareSpeedMod*=0.65;
else if(blockStatus & CMoveMath::BLOCK_MOBILE)
squareSpeedMod*=0.35;
else
squareSpeedMod*=0.10;
}
float squareCost = moveCost[enterDirection] / squareSpeedMod;
float heuristicCost = pfDef.Heuristic(square.x, square.y);
//Summarize cost.
float currentCost = parentOpenSquare->currentCost + squareCost;
float cost = currentCost + heuristicCost;
//Checks if this square are in que already.
//If the old one is better then keep it, else change it.
if(squareState[sqr].status & PATHOPT_OPEN) {
if(squareState[sqr].cost <= cost)
return true;
squareState[sqr].status &= ~PATHOPT_DIRECTION;
}
//Looking for improvements.
if(!exactPath && heuristicCost < goalHeuristic) {
goalSquare = sqr;
goalHeuristic = heuristicCost;
}
//Store this square as open.
OpenSquare *os = ++openSquareBufferPointer; //Take the next OpenSquare in buffer.
os->square = square;
os->sqr = sqr;
os->currentCost = currentCost;
os->cost = cost;
openSquares.push(os);
//Set this one as open and the direction from which it was reached.
squareState[sqr].cost = os->cost;
squareState[sqr].status |= (PATHOPT_OPEN | enterDirection);
dirtySquares.push_back(sqr);
return true;
}
bool CPathFinder::TestBlock(
const MoveDef& moveDef,
const CPathFinderDef& pfDef,
const PathNode* parentSquare,
const CSolidObject* owner,
const unsigned int pathOptDir,
const unsigned int blockStatus,
float speedMod
) {
testedBlocks++;
// initial calculations of the new block
const int2 square = parentSquare->nodePos + PF_DIRECTION_VECTORS_2D[pathOptDir];
const unsigned int sqrIdx = BlockPosToIdx(square);
// bounds-check
assert((unsigned)square.x < nbrOfBlocks.x);
assert((unsigned)square.y < nbrOfBlocks.y);
assert((blockStates.nodeMask[sqrIdx] & (PATHOPT_CLOSED | PATHOPT_BLOCKED)) == 0);
assert((blockStatus & CMoveMath::BLOCK_STRUCTURE) == 0);
assert(speedMod != 0.0f);
if (pfDef.testMobile && moveDef.avoidMobilesOnPath && (blockStatus & squareMobileBlockBits)) {
if (blockStatus & CMoveMath::BLOCK_MOBILE_BUSY) {
speedMod *= moveDef.speedModMults[MoveDef::SPEEDMOD_MOBILE_BUSY_MULT];
} else if (blockStatus & CMoveMath::BLOCK_MOBILE) {
speedMod *= moveDef.speedModMults[MoveDef::SPEEDMOD_MOBILE_IDLE_MULT];
} else { // (blockStatus & CMoveMath::BLOCK_MOVING)
speedMod *= moveDef.speedModMults[MoveDef::SPEEDMOD_MOBILE_MOVE_MULT];
}
}
const float heatCost = (pfDef.testMobile) ? (PathHeatMap::GetInstance())->GetHeatCost(square.x, square.y, moveDef, ((owner != NULL)? owner->id: -1U)) : 0.0f;
//const float flowCost = (pfDef.testMobile) ? (PathFlowMap::GetInstance())->GetFlowCost(square.x, square.y, moveDef, pathOptDir) : 0.0f;
const float extraCost = blockStates.GetNodeExtraCost(square.x, square.y, pfDef.synced);
const float dirMoveCost = (1.0f + heatCost) * PF_DIRECTION_COSTS[pathOptDir];
const float nodeCost = (dirMoveCost / speedMod) + extraCost;
const float gCost = parentSquare->gCost + nodeCost; // g
const float hCost = pfDef.Heuristic(square.x, square.y); // h
const float fCost = gCost + hCost; // f
if (blockStates.nodeMask[sqrIdx] & PATHOPT_OPEN) {
// already in the open set, look for a cost-improvement
if (blockStates.fCost[sqrIdx] <= fCost)
return true;
blockStates.nodeMask[sqrIdx] &= ~PATHOPT_CARDINALS;
}
// if heuristic says this node is closer to goal than previous h-estimate, keep it
if (!pfDef.exactPath && hCost < mGoalHeuristic) {
mGoalBlockIdx = sqrIdx;
mGoalHeuristic = hCost;
}
// store and mark this square as open (expanded, but not yet pulled from pqueue)
openBlockBuffer.SetSize(openBlockBuffer.GetSize() + 1);
assert(openBlockBuffer.GetSize() < MAX_SEARCHED_NODES_PF);
PathNode* os = openBlockBuffer.GetNode(openBlockBuffer.GetSize());
os->fCost = fCost;
os->gCost = gCost;
os->nodePos = square;
os->nodeNum = sqrIdx;
openBlocks.push(os);
blockStates.SetMaxCost(NODE_COST_F, std::max(blockStates.GetMaxCost(NODE_COST_F), fCost));
blockStates.SetMaxCost(NODE_COST_G, std::max(blockStates.GetMaxCost(NODE_COST_G), gCost));
blockStates.fCost[sqrIdx] = os->fCost;
blockStates.gCost[sqrIdx] = os->gCost;
blockStates.nodeMask[sqrIdx] |= (PATHOPT_OPEN | pathOptDir);
dirtyBlocks.push_back(sqrIdx);
return true;
}
bool CPathFinder::TestSquare(
const MoveDef& moveDef,
const CPathFinderDef& pfDef,
const PathNode* parentOpenSquare,
const CSolidObject* owner,
unsigned int pathOptDir,
bool synced
) {
testedNodes++;
const int2& dirVec2D = directionVectors2D[pathOptDir];
const float3& dirVec3D = directionVectors3D[pathOptDir];
// Calculate the new square.
int2 square;
square.x = parentOpenSquare->nodePos.x + dirVec2D.x;
square.y = parentOpenSquare->nodePos.y + dirVec2D.y;
// Inside map?
if (square.x < 0 || square.y < 0 || square.x >= gs->mapx || square.y >= gs->mapy) {
return false;
}
const unsigned int sqrIdx = square.x + square.y * gs->mapx;
const unsigned int sqrStatus = squareStates.nodeMask[sqrIdx];
// Check if the square is unaccessable or used.
if (sqrStatus & (PATHOPT_CLOSED | PATHOPT_FORBIDDEN | PATHOPT_BLOCKED)) {
return false;
}
const CMoveMath::BlockType blockStatus = CMoveMath::IsBlocked(moveDef, square.x, square.y, owner);
// Check if square are out of constraints or blocked by something.
// Doesn't need to be done on open squares, as those are already tested.
if (!(sqrStatus & PATHOPT_OPEN) &&
((blockStatus & CMoveMath::BLOCK_STRUCTURE) || !pfDef.WithinConstraints(square.x, square.y))
) {
squareStates.nodeMask[sqrIdx] |= PATHOPT_BLOCKED;
dirtySquares.push_back(sqrIdx);
return false;
}
// Evaluate this square.
float squareSpeedMod = CMoveMath::GetPosSpeedMod(moveDef, square.x, square.y, dirVec3D);
if (squareSpeedMod == 0.0f) {
squareStates.nodeMask[sqrIdx] |= PATHOPT_FORBIDDEN;
dirtySquares.push_back(sqrIdx);
return false;
}
if (testMobile && moveDef.avoidMobilesOnPath && (blockStatus & squareMobileBlockBits)) {
if (blockStatus & CMoveMath::BLOCK_MOBILE_BUSY) {
squareSpeedMod *= moveDef.speedModMults[MoveDef::SPEEDMOD_MOBILE_BUSY_MULT];
} else if (blockStatus & CMoveMath::BLOCK_MOBILE) {
squareSpeedMod *= moveDef.speedModMults[MoveDef::SPEEDMOD_MOBILE_IDLE_MULT];
} else { // (blockStatus & CMoveMath::BLOCK_MOVING)
squareSpeedMod *= moveDef.speedModMults[MoveDef::SPEEDMOD_MOBILE_MOVE_MULT];
}
}
const float heatCost = (PathHeatMap::GetInstance())->GetHeatCost(square.x, square.y, moveDef, ((owner != NULL)? owner->id: -1U));
const float flowCost = (PathFlowMap::GetInstance())->GetFlowCost(square.x, square.y, moveDef, pathOptDir);
const float dirMoveCost = (1.0f + heatCost + flowCost) * directionCosts[pathOptDir];
const float extraCost = squareStates.GetNodeExtraCost(square.x, square.y, synced);
const float nodeCost = (dirMoveCost / squareSpeedMod) + extraCost;
const float gCost = parentOpenSquare->gCost + nodeCost; // g
const float hCost = pfDef.Heuristic(square.x, square.y); // h
const float fCost = gCost + hCost; // f
if (squareStates.nodeMask[sqrIdx] & PATHOPT_OPEN) {
// already in the open set
if (squareStates.fCost[sqrIdx] <= fCost)
return true;
squareStates.nodeMask[sqrIdx] &= ~PATHOPT_AXIS_DIRS;
}
// Look for improvements.
if (!exactPath && hCost < mGoalHeuristic) {
mGoalSquareIdx = sqrIdx;
mGoalHeuristic = hCost;
}
// Store this square as open.
openSquareBuffer.SetSize(openSquareBuffer.GetSize() + 1);
assert(openSquareBuffer.GetSize() < MAX_SEARCHED_NODES_PF);
PathNode* os = openSquareBuffer.GetNode(openSquareBuffer.GetSize());
os->fCost = fCost;
os->gCost = gCost;
os->nodePos = square;
os->nodeNum = sqrIdx;
openSquares.push(os);
squareStates.SetMaxCost(NODE_COST_F, std::max(squareStates.GetMaxCost(NODE_COST_F), fCost));
squareStates.SetMaxCost(NODE_COST_G, std::max(squareStates.GetMaxCost(NODE_COST_G), gCost));
// mark this square as open
squareStates.fCost[sqrIdx] = os->fCost;
squareStates.gCost[sqrIdx] = os->gCost;
squareStates.nodeMask[sqrIdx] |= (PATHOPT_OPEN | pathOptDir);
dirtySquares.push_back(sqrIdx);
return true;
}
/**
* Test the availability and value of a square,
* and possibly add it to the queue of open squares.
*/
bool CPathFinder::TestSquare(
const MoveData& moveData,
const CPathFinderDef& pfDef,
const PathNode* parentOpenSquare,
unsigned int enterDirection,
int ownerId,
bool synced
) {
testedNodes++;
// Calculate the new square.
int2 square;
square.x = parentOpenSquare->nodePos.x + directionVector[enterDirection].x;
square.y = parentOpenSquare->nodePos.y + directionVector[enterDirection].y;
// Inside map?
if (square.x < 0 || square.y < 0 || square.x >= gs->mapx || square.y >= gs->mapy) {
return false;
}
const int sqrIdx = square.x + square.y * gs->mapx;
const int sqrStatus = squareStates[sqrIdx].nodeMask;
// Check if the square is unaccessable or used.
if (sqrStatus & (PATHOPT_CLOSED | PATHOPT_FORBIDDEN | PATHOPT_BLOCKED)) {
return false;
}
const int blockStatus = moveData.moveMath->IsBlocked2(moveData, square.x, square.y);
int blockBits = (CMoveMath::BLOCK_STRUCTURE | CMoveMath::BLOCK_TERRAIN);
// Check if square are out of constraints or blocked by something.
// Doesn't need to be done on open squares, as those are already tested.
if ((!pfDef.WithinConstraints(square.x, square.y) || (blockStatus & blockBits)) &&
!(sqrStatus & PATHOPT_OPEN)) {
squareStates[sqrIdx].nodeMask |= PATHOPT_BLOCKED;
dirtySquares.push_back(sqrIdx);
return false;
}
// Evaluate this square.
float squareSpeedMod = moveData.moveMath->SpeedMod(moveData, square.x, square.y);
blockBits = (CMoveMath::BLOCK_MOBILE | CMoveMath::BLOCK_MOVING | CMoveMath::BLOCK_MOBILE_BUSY);
if (squareSpeedMod == 0) {
squareStates[sqrIdx].nodeMask |= PATHOPT_FORBIDDEN;
dirtySquares.push_back(sqrIdx);
return false;
}
if (testMobile && (blockStatus & blockBits)) {
if (blockStatus & CMoveMath::BLOCK_MOVING)
squareSpeedMod *= 0.65f;
else if (blockStatus & CMoveMath::BLOCK_MOBILE)
squareSpeedMod *= 0.35f;
else
squareSpeedMod *= 0.10f;
}
// Include heatmap cost adjustment.
float heatCostMod = 1.0f;
if (heatMapping && moveData.heatMapping && GetHeatOwner(square.x, square.y) != ownerId) {
heatCostMod += (moveData.heatMod * GetHeatValue(square.x,square.y));
}
const float dirMoveCost = (heatCostMod * moveCost[enterDirection]);
const float extraCost = squareStates.GetNodeExtraCost(square.x, square.y, synced);
const float nodeCost = (dirMoveCost / squareSpeedMod) + extraCost;
const float gCost = parentOpenSquare->gCost + nodeCost; // g
const float hCost = pfDef.Heuristic(square.x, square.y); // h
const float fCost = gCost + hCost; // f
if (squareStates[sqrIdx].nodeMask & PATHOPT_OPEN) {
// already in the open set
if (squareStates[sqrIdx].fCost <= fCost)
return true;
squareStates[sqrIdx].nodeMask &= ~PATHOPT_DIRECTION;
}
// Look for improvements.
if (!exactPath && hCost < goalHeuristic) {
goalSquare = sqrIdx;
goalHeuristic = hCost;
}
// Store this square as open.
openSquareBuffer.SetSize(openSquareBuffer.GetSize() + 1);
assert(openSquareBuffer.GetSize() < MAX_SEARCHED_NODES_PF);
PathNode* os = openSquareBuffer.GetNode(openSquareBuffer.GetSize());
os->fCost = fCost;
os->gCost = gCost;
os->nodePos = square;
os->nodeNum = sqrIdx;
openSquares.push(os);
squareStates.SetMaxFCost(std::max(squareStates.GetMaxFCost(), fCost));
squareStates.SetMaxGCost(std::max(squareStates.GetMaxGCost(), gCost));
// mark this square as open
squareStates[sqrIdx].fCost = os->fCost;
squareStates[sqrIdx].gCost = os->gCost;
squareStates[sqrIdx].nodeMask |= (PATHOPT_OPEN | enterDirection);
dirtySquares.push_back(sqrIdx);
return true;
}
