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