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 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 CPathFinder::DoSearch(const MoveDef& moveDef, const CPathFinderDef& pfDef, const CSolidObject* owner, bool synced) {
bool foundGoal = false;
while (!openSquares.empty() && (openSquareBuffer.GetSize() < maxOpenNodes)) {
// Get the open square with lowest expected path-cost.
PathNode* os = const_cast<PathNode*>(openSquares.top());
openSquares.pop();
// check if this PathNode has become obsolete
if (squareStates.fCost[os->nodeNum] != os->fCost)
continue;
// Check if the goal is reached.
if (pfDef.IsGoal(os->nodePos.x, os->nodePos.y)) {
mGoalSquareIdx = os->nodeNum;
mGoalHeuristic = 0.0f;
foundGoal = true;
break;
}
// Test the 8 surrounding squares.
const bool right = TestSquare(moveDef, pfDef, os, owner, PATHOPT_RIGHT, synced);
const bool left = TestSquare(moveDef, pfDef, os, owner, PATHOPT_LEFT, synced);
const bool up = TestSquare(moveDef, pfDef, os, owner, PATHOPT_UP, synced);
const bool down = TestSquare(moveDef, pfDef, os, owner, PATHOPT_DOWN, synced);
if (up) {
// we dont want to search diagonally if there is a blocking object
// (not blocking terrain) in one of the two side squares
if (right) {
TestSquare(moveDef, pfDef, os, owner, (PATHOPT_RIGHT | PATHOPT_UP), synced);
}
if (left) {
TestSquare(moveDef, pfDef, os, owner, (PATHOPT_LEFT | PATHOPT_UP), synced);
}
}
if (down) {
if (right) {
TestSquare(moveDef, pfDef, os, owner, (PATHOPT_RIGHT | PATHOPT_DOWN), synced);
}
if (left) {
TestSquare(moveDef, pfDef, os, owner, (PATHOPT_LEFT | PATHOPT_DOWN), synced);
}
}
// Mark this square as closed.
squareStates.nodeMask[os->nodeNum] |= PATHOPT_CLOSED;
}
if (foundGoal)
return IPath::Ok;
// Could not reach the goal.
if (openSquareBuffer.GetSize() >= maxOpenNodes)
return IPath::GoalOutOfRange;
// Search could not reach the goal, due to the unit being locked in.
if (openSquares.empty())
return IPath::GoalOutOfRange;
// should be unreachable
// LogObject() << "ERROR: CPathFinder::DoSearch() - Unhandled end of search!\n";
return IPath::Error;
}
/*
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);
}
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 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;
}
/**
* Search with several start positions
*/
IPath::SearchResult CPathFinder::GetPath(
const MoveData& moveData,
const std::vector<float3>& startPos,
const CPathFinderDef& pfDef,
IPath::Path& path,
int ownerId,
bool synced
) {
// Clear the given path.
path.path.clear();
path.squares.clear();
path.pathCost = PATHCOST_INFINITY;
// Store som basic data.
maxSquaresToBeSearched = MAX_SEARCHED_NODES_PF - 8U;
testMobile = false;
exactPath = true;
needPath = true;
// 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;
// If the starting position is a goal position, then no search need to be performed.
if (pfDef.IsGoal(startxSqr, startzSqr))
return IPath::CantGetCloser;
// Clearing the system from last search.
ResetSearch();
openSquareBuffer.SetSize(0);
for (std::vector<float3>::const_iterator si = startPos.begin(); si != startPos.end(); ++si) {
start = *si;
startxSqr = (int(start.x) / SQUARE_SIZE) | 1;
startzSqr = (int(start.z) / SQUARE_SIZE) | 1;
startSquare = startxSqr + startzSqr * gs->mapx;
goalSquare = startSquare;
squareStates[startSquare].nodeMask = (PATHOPT_START | PATHOPT_OPEN);
squareStates[startSquare].fCost = 0.0f;
squareStates[startSquare].gCost = 0.0f;
dirtySquares.push_back(startSquare);
if (openSquareBuffer.GetSize() >= MAX_SEARCHED_NODES_PF) {
continue;
}
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;
openSquareBuffer.SetSize(openSquareBuffer.GetSize() + 1);
openSquares.push(os);
}
// note: DoSearch, not InitSearch
IPath::SearchResult result = DoSearch(moveData, pfDef, ownerId, synced);
// Respond to the success of the search.
if (result == IPath::Ok) {
FinishSearch(moveData, path);
if (PATHDEBUG) {
LogObject() << "Path found.\n";
LogObject() << "Nodes tested: " << testedNodes << "\n";
LogObject() << "Open squares: " << openSquareBuffer.GetSize() << "\n";
LogObject() << "Path nodes: " << path.path.size() << "\n";
LogObject() << "Path cost: " << path.pathCost << "\n";
}
} else {
if (PATHDEBUG) {
LogObject() << "No path found!\n";
LogObject() << "Nodes tested: " << testedNodes << "\n";
LogObject() << "Open squares: " << openSquareBuffer.GetSize() << "\n";
}
}
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, 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);
}
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 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;
}
/*
Search with several start positions
*/
IPath::SearchResult CPathFinder::GetPath(const MoveData& moveData, std::vector<float3> startPos, const CPathFinderDef& pfDef, Path& path) {
//Clear the given path.
path.path.clear();
path.pathCost = PATHCOST_INFINITY;
//Store som basic data.
maxNodesToBeSearched = MAX_SEARCHED_SQARES;
testMobile=false;
exactPath = true;
needPath=true;
//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;
//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();
openSquareBufferPointer = &openSquareBuffer[0];
for(std::vector<float3>::iterator si=startPos.begin(); si!=startPos.end(); ++si) {
start = *si;
startxSqr = (int(start.x) / SQUARE_SIZE)|1;
startzSqr = (int(start.z) / SQUARE_SIZE)|1;
startSquare = startxSqr + startzSqr * gs->mapx;
squareState[startSquare].status = (PATHOPT_START | PATHOPT_OPEN);
squareState[startSquare].cost = 0;
dirtySquares.push_back(startSquare);
goalSquare = startSquare;
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);
//Respond to the success of the search.
if(result == Ok) {
FinishSearch(moveData, path);
if(PATHDEBUG) {
*info << "Path found.\n";
*info << "Nodes tested: " << (int)testedNodes << "\n";
*info << "Open squares: " << (openSquareBufferPointer - openSquareBuffer) << "\n";
*info << "Path steps: " << (int)(path.path.size()) << "\n";
*info << "Path cost: " << path.pathCost << "\n";
}
} else {
if(PATHDEBUG) {
*info << "Path not found!\n";
*info << "Nodes tested: " << (int)testedNodes << "\n";
*info << "Open squares: " << (openSquareBufferPointer - openSquareBuffer) << "\n";
}
}
return result;
}
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;
}
void CPathFinder::TestNeighborSquares(
const MoveDef& moveDef,
const CPathFinderDef& pfDef,
const PathNode* square,
const CSolidObject* owner
) {
// early out
if (!pfDef.WithinConstraints(square->nodePos.x, square->nodePos.y)) {
blockStates.nodeMask[square->nodeNum] |= PATHOPT_CLOSED;
dirtyBlocks.push_back(square->nodeNum);
return;
}
struct SqState {
SqState() : blockedState(CMoveMath::BLOCK_NONE), speedMod(0.0f), inSearch(false) {}
CMoveMath::BlockType blockedState;
float speedMod;
bool inSearch;
};
SqState ngbStates[PATH_DIRECTIONS];
// precompute structure-blocked for all neighbors
for (unsigned int dir = 0; dir < PATH_DIRECTIONS; dir++) {
const int2 ngbSquareCoors = square->nodePos + PF_DIRECTION_VECTORS_2D[ PathDir2PathOpt(dir) ];
const int ngbSquareIdx = BlockPosToIdx(ngbSquareCoors);
if ((unsigned)ngbSquareCoors.x >= nbrOfBlocks.x || (unsigned)ngbSquareCoors.y >= nbrOfBlocks.y)
continue;
if (blockStates.nodeMask[ngbSquareIdx] & (PATHOPT_CLOSED | PATHOPT_BLOCKED)) //FIXME
continue;
// very time expensive call
SqState& sqState = ngbStates[dir];
sqState.blockedState = CMoveMath::IsBlockedNoSpeedModCheck(moveDef, ngbSquareCoors.x, ngbSquareCoors.y, owner);
if (sqState.blockedState & CMoveMath::BLOCK_STRUCTURE) {
blockStates.nodeMask[ngbSquareIdx] |= PATHOPT_CLOSED;
dirtyBlocks.push_back(ngbSquareIdx);
continue; // early-out (20% chance)
}
// hint: use posSpeedMod for PE! cause it assumes path costs are bidirectional and so it only saves one `cost` for left & right movement
// hint2: not worth to put in front of the above code, it only has a ~2% chance (heavily depending on the map) to early-out
if (!pfDef.dirIndependent) {
sqState.speedMod = CMoveMath::GetPosSpeedMod(moveDef, ngbSquareCoors.x, ngbSquareCoors.y, PF_DIRECTION_VECTORS_3D[ PathDir2PathOpt(dir) ]);
} else {
sqState.speedMod = CMoveMath::GetPosSpeedMod(moveDef, ngbSquareCoors.x, ngbSquareCoors.y);
// only close node if we're dirIndependent
// otherwise, it's possible we'll be able to enter it from another direction.
if (sqState.speedMod == 0.0f) {
blockStates.nodeMask[ngbSquareIdx] |= PATHOPT_CLOSED;
dirtyBlocks.push_back(ngbSquareIdx);
}
}
if (sqState.speedMod != 0.0f) {
sqState.inSearch = pfDef.WithinConstraints(ngbSquareCoors.x, ngbSquareCoors.y);
}
}
const auto CAN_TEST_SQUARE_SM = [&](const int dir) { return (ngbStates[dir].speedMod != 0.0f); };
const auto CAN_TEST_SQUARE_IS = [&](const int dir) { return (ngbStates[dir].inSearch); };
// first test squares along the cardinal directions
for (unsigned int dir: PATHDIR_CARDINALS) {
if (!CAN_TEST_SQUARE_SM(dir))
continue;
const SqState& sqState = ngbStates[dir];
const unsigned int opt = PathDir2PathOpt(dir);
TestBlock(moveDef, pfDef, square, owner, opt, sqState.blockedState, sqState.speedMod);
}
// next test the diagonal squares
//
// don't search diagonally if there is a blocking object
// (or blocking terrain!) in one of the two side squares
// e.g. do not consider the edge (p, q) passable if X is
// impassable in this situation:
// +---+---+
// | X | q |
// +---+---+
// | p | X |
// +---+---+
//
// *** IMPORTANT ***
//
// if either side-square is merely outside the constrained
// area but the diagonal square is not, we do consider the
// edge passable since we still need to be able to jump to
// diagonally adjacent PE-blocks!
//
const auto TEST_DIAG_SQUARE = [&](const int BASE_DIR_X, const int BASE_DIR_Y, const int BASE_DIR_XY) {
if (CAN_TEST_SQUARE_SM(BASE_DIR_XY) && (CAN_TEST_SQUARE_SM(BASE_DIR_X) && CAN_TEST_SQUARE_SM(BASE_DIR_Y))) {
if (CAN_TEST_SQUARE_IS(BASE_DIR_XY) || (CAN_TEST_SQUARE_IS(BASE_DIR_X) && CAN_TEST_SQUARE_IS(BASE_DIR_Y))) {
const unsigned int ngbOpt = PathDir2PathOpt(BASE_DIR_XY);
const SqState& sqState = ngbStates[BASE_DIR_XY];
TestBlock(moveDef, pfDef, square, owner, ngbOpt, sqState.blockedState, sqState.speedMod);
}
}
};
TEST_DIAG_SQUARE(PATHDIR_LEFT, PATHDIR_UP, PATHDIR_LEFT_UP );
TEST_DIAG_SQUARE(PATHDIR_RIGHT, PATHDIR_UP, PATHDIR_RIGHT_UP );
TEST_DIAG_SQUARE(PATHDIR_LEFT, PATHDIR_DOWN, PATHDIR_LEFT_DOWN );
TEST_DIAG_SQUARE(PATHDIR_RIGHT, PATHDIR_DOWN, PATHDIR_RIGHT_DOWN);
// mark this square as closed
blockStates.nodeMask[square->nodeNum] |= PATHOPT_CLOSED;
dirtyBlocks.push_back(square->nodeNum);
}
// 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))];
}
