void NavGrid::ExamineNeighbour(Node* aCurrentNode, int aNeighbourIndex, int aGoalIndex, CU::Heap<Node*, NavGrid_NodeComparer<Node*>>& aHeap) { if (aNeighbourIndex < 0 || aNeighbourIndex > myGrid.Size() - 1) return; Node* neighbour = &myGrid[aNeighbourIndex]; if (neighbour->myListState == IN_CLOSED || !neighbour->myIsWalkable) return; if (neighbour->myListState == IN_OPEN) { if (neighbour->myG > aCurrentNode->myG + myMoveCost) { UpdateNodeValues(aCurrentNode, neighbour, aGoalIndex); aHeap.Heapify(); } } else { neighbour->myListState = IN_OPEN; UpdateNodeValues(aCurrentNode, neighbour, aGoalIndex); aHeap.Enqueue(neighbour); } }
/** * @function FindVarietyPaths2 * @brief Using Free Space */ std::vector< std::vector<Eigen::Vector3i> > LJM2::FindVarietyPaths2( int _x1, int _y1, int _z1, int _x2, int _y2, int _z2, int _times, float _alpha ) { mAlpha = _alpha; std::vector< std::vector<Eigen::Vector3i> > paths; std::vector<Eigen::Vector3i> path; std::vector< std::vector<int> > nodePaths; std::vector<int> allNodePaths; time_t ts; time_t tf; double dt; //-- Find the nearest points int startIndex = ref( _x1, _y1, _z1 ); int targetIndex = ref( _x2, _y2, _z2 ); printf("--> Start: %d Target: %d \n", startIndex, targetIndex ); InitSearch(); for( size_t i = 0; i < _times; ++i ) { path.resize(0); //ts = clock(); path = FindPath( startIndex, targetIndex ); //tf = clock(); printf("--[%d] Found Path of size %d \n", i, path.size() ); //printf(" Time elapsed: %.3f \n", (double) (tf - ts)/CLOCKS_PER_SEC ); paths.push_back( path ); nodePaths.push_back( mPath ); //-- Update the values //ts = clock(); ResetSearch(); //tf = clock(); //printf("--[%d] Search : Time elapsed: %.3f \n", i, (double) (tf - ts)/CLOCKS_PER_SEC ); allNodePaths = JoinPaths( nodePaths ); ts = clock(); UpdateNodeValues( allNodePaths ); //tf = clock(); //printf("--[%d] UpdateNodes : Time elapsed: %.3f \n", i, (double) (tf - ts)/CLOCKS_PER_SEC ); } return paths; }
//--------------------------------------------------------------------------------------------------------------------------- bool PathFinder::RunOneStep(int movementProperties) { if(m_openList.empty()) return false; PathNode* activeNode = FindLowestFNodeOnOpenListAndRemoveIt(); AddToClosedList(activeNode); if (activeNode->m_position == m_goal) { WalkClosedListAndConstructPath(); m_isFinished = true; return true; } std::vector<IntVector2> validPositions = MapProxy::FindValidAdjacentPositions(activeNode->m_position, movementProperties); for (IntVector2& position : validPositions) { if (IsPositionOnClosedList(position)) continue; float gLocal = MapProxy::ComputeLogalG(activeNode->m_position, position, movementProperties); float gParent = activeNode->gLocal + activeNode->gParent; float h = MapProxy::ComputeHueristic(position, m_goal); PathNode* existingNode = FindOpenNodeWithPos(position); if (existingNode) { if ((gLocal + gParent) < (existingNode->gLocal + existingNode->gParent)) { UpdateNodeValues(existingNode, gLocal, gParent, activeNode); } continue; } PathNode* newNode = CreateNode(position, activeNode, gLocal, gParent, h); AddNodeToOpenList(newNode); } return false; }