void setAllInfoAStarNode(aStarNode* f_inputAStarNode_pst, node* f_inputInfoNode_pst,
aStarNode* f_inputCameFromNode_pst, int32 f_inputGscore_i32,
int32 f_inputHscore_i32)
{
setInformationPtr(f_inputAStarNode_pst, f_inputInfoNode_pst);
setCameFromPtr(f_inputAStarNode_pst, f_inputCameFromNode_pst);
setGScore(f_inputAStarNode_pst, f_inputGscore_i32);
setHScore(f_inputAStarNode_pst, f_inputHscore_i32);
setFScore(f_inputAStarNode_pst, f_inputGscore_i32, f_inputHscore_i32);
}
void ComputePath::getPath( Point from,Point to )
{
insertInOpenSteps(ShortestPathStep::createFromPos(from));
do
{
auto currentStep = spOpenSteps.at(0);
spClosedSteps.pushBack(currentStep);
spOpenSteps.erase(0);
if (currentStep->getPos() == to)
{
constructPath(currentStep);
spOpenSteps.clear();
spClosedSteps.clear();
break;
}
auto points = _layer->getValidStep(currentStep->getPos());
for (int i = 0 ; i < points->count(); i++)
{
auto v = points->getControlPointAtIndex(i);
auto step = ShortestPathStep::createFromPos(v);
int closeindex = -1;
for (int i = 0 ; i < spClosedSteps.size();i++ )
{
if (spClosedSteps.at(i)->isEqual(step))
{
closeindex = i;
break;
}
}
if (closeindex != -1)
{
continue;
}
auto moveCost = this->costToMoveFromStep(currentStep,step);
int index = -1;
for (int i = 0; i < spOpenSteps.size();i++)
{
auto item = spOpenSteps.at(i);
if (step->isEqual(item))
{
index = i;
break;
}
}
if (index == -1)
{
step->setParent(currentStep);
step->setGScore(currentStep->getGScore() + moveCost);
step->setHScore(this->computeHScoreFromCoord(step->getPos(),to));
this->insertInOpenSteps(step);
}
else
{
step = this->spOpenSteps.at(index);
if (currentStep->getGScore() + moveCost < step->getGScore())
{
step->setGScore(currentStep->getGScore() + moveCost);
step->retain();
spOpenSteps.erase(index);
this->insertInOpenSteps(step);
step->release();
}
}
}
} while (spOpenSteps.size() > 0);
}
neighbourList* aStarPathFinder(node* f_startNode_pst, node* f_endNode_pst)
{
int32 unitGscore_i32 = 1;
int32 gScoreStart_i32 = 0;
int32 hScoreStart_i32;
aStarNode* startAStarNode = createAStarNode(f_startNode_pst);
aStarNode* startCameFrom_pst = NULL;
aStarList* openAStarList_pst = createAStarList();
aStarList* closedAStarList_pst = createAStarList();
if(startAStarNode && openAStarList_pst && closedAStarList_pst)
{
hScoreStart_i32 = heuristicEstimate(f_startNode_pst, f_startNode_pst, f_endNode_pst);
setAllInfoAStarNode(startAStarNode, f_startNode_pst, startCameFrom_pst, gScoreStart_i32, hScoreStart_i32);
addToEndAStarList(openAStarList_pst, startAStarNode);
while(!isAStarListEmpty(openAStarList_pst))
{
aStarNode* LowestFscoreNode_pst = getLowest(openAStarList_pst);
if (f_endNode_pst == LowestFscoreNode_pst ->informationNode_pst)
{
// Reconstruct path, destroy the AStarlists created above
// and return the node list to process
neighbourList* AstarPath_pst = reconstructPath(startAStarNode, LowestFscoreNode_pst);
destroyAStarList(openAStarList_pst);
destroyAStarList(closedAStarList_pst);
return AstarPath_pst;
}
removeFromAStarList(openAStarList_pst, LowestFscoreNode_pst);
addToEndAStarList(closedAStarList_pst, LowestFscoreNode_pst);
node* currentNode_pst = LowestFscoreNode_pst ->informationNode_pst;
neighbourList* NeighbourList_pst = getNeighbourList(currentNode_pst);
if(!isNeighbourListEmpty(NeighbourList_pst))
{
neighbourListNode* nextNeighbour_pst = NULL;
neighbourListNode* neighbour_pst = NeighbourList_pst->head;
for (; neighbour_pst != NULL; neighbour_pst = nextNeighbour_pst)
{
nextNeighbour_pst = neighbour_pst->next;
node* neighbourNode_pst = neighbour_pst ->neighbourNode_pst;
if(searchNeighbourInAStarList(closedAStarList_pst, neighbourNode_pst))
{
continue;
}
int32 neighbourGscore_i32 = LowestFscoreNode_pst ->gScore_i32 + unitGscore_i32;
aStarListNode* searchResult_pst = searchNeighbourInAStarList(openAStarList_pst, neighbourNode_pst);
if (!searchResult_pst)
{
aStarNode* neighbourAStarNode = createAStarNode(neighbourNode_pst);
int32 neighbourHscore_i32 = heuristicEstimate(f_startNode_pst, neighbourNode_pst, f_endNode_pst);
setAllInfoAStarNode(neighbourAStarNode, neighbourNode_pst, LowestFscoreNode_pst,
neighbourGscore_i32, neighbourHscore_i32);
addToEndAStarList(openAStarList_pst, neighbourAStarNode);
}
else if (neighbourGscore_i32 < searchResult_pst ->aStarNode_pst ->gScore_i32)
{
aStarNode* neighbourAStarNode = searchResult_pst ->aStarNode_pst;
setCameFromPtr(neighbourAStarNode, LowestFscoreNode_pst);
setGScore(neighbourAStarNode, neighbourGscore_i32);
setFScore(neighbourAStarNode, neighbourGscore_i32, neighbourAStarNode ->hScore_i32);
}
}
}
destroyNeighbourList(NeighbourList_pst);
}
return NULL;
}
return NULL;
}
