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; }