std::vector<Pnt3f> OctreeAStarAlgorithm::search(OctreePtr Tree,
const Pnt3f& Start,
const Pnt3f& Goal,
PathCostHeuristicFunc CostHeuristicFunc)
{
_Tree = Tree;
_StartNode = _Tree->getNodeThatContains(Start);
_GoalNode = _Tree->getNodeThatContains(Goal);
_CostHeuristicFunc = CostHeuristicFunc;
if(!_StartNode ||
!_GoalNode)
{
return _SolutionPath;
}
//clear lists
_SolutionPath.clear();
_OpenNodes.clear();
_ClosedNodes.clear();
//initialize start node
ASNodePtr startNode = ASNodePtr(new ASNode);
startNode->_OctreeNode = _StartNode;
startNode->_CostFromStart = 0;
Pnt3f Center;
_StartNode->getVolume().getCenter(Center);
startNode->_CostToGoal = _CostHeuristicFunc(_Tree, _StartNode, Center);
startNode->_Parent.reset();
_OpenNodes.push_back(startNode);
while(!_OpenNodes.empty())
{
ASNodePtr Node = _OpenNodes.front();
_OpenNodes.erase(_OpenNodes.begin());
if(Node->_OctreeNode == _GoalNode)
{
constructPath(Node);
return _SolutionPath;//success
}
else
{
for(Int8 i = 0; i < Node->_OctreeNode->getNeighbors().size(); ++i)
{
if(!Node->_OctreeNode->getNeighbor(i)->getContainsObstacles())
{//Node->_OctreeNode->getNeighbor(i)->children.size() == 0
Node->_OctreeNode->getNeighbor(i)->getVolume().getCenter(Center);
Real32 NewCost = Node->_CostFromStart + _CostHeuristicFunc(_Tree, _StartNode, Center);
Int32 locInOpen = inOpen(Node->_OctreeNode->getNeighbor(i));
Int32 locInClosed = inClosed(Node->_OctreeNode->getNeighbor(i));
if((locInOpen >= 0 && _OpenNodes[locInOpen]->_CostFromStart <= NewCost) || (locInClosed >= 0 && _ClosedNodes[locInClosed]->_CostFromStart <= NewCost))
{
continue;
}
else
{
//initialize a new node
ASNodePtr NewNode = ASNodePtr(new ASNode());
NewNode->_OctreeNode = Node->_OctreeNode->getNeighbor(i);
NewNode->_OctreeNode->getVolume().getCenter(Center);
NewNode->_CostFromStart = _CostHeuristicFunc(_Tree, _StartNode, Center);
NewNode->_CostToGoal = _CostHeuristicFunc(_Tree, _GoalNode, Center);
NewNode->_Parent = Node;
if(locInClosed >= 0)
{
_ClosedNodes.erase(_ClosedNodes.begin() + locInClosed);
}
/*if(locInOpen >= 0)
* {
_OpenNodes.erase(_OpenNodes.begin() + locInOpen);
}else{
pushOnOpen(NewNode);
}*/
if(locInOpen >= 0)
{
_OpenNodes.erase(_OpenNodes.begin() + locInOpen);
}
pushOnOpen(NewNode);
}//endif
}//endif...
}//end for loop...dont with Node
}//endif
_ClosedNodes.push_back(Node);
}//end while loop
return _SolutionPath;
}
void AStar::createPath(Vec3f startPos, Vec3f endPos, levelGraph *level)
{
//no need to process this situation
if(startPos == endPos)
{
return;
}
/*startPos[0] = (startPos[0] - level->levelMask->startingOffset[0]) / level->levelMask->scaleX;
startPos[1] = 0.0f;
startPos[2] = (startPos[2] - level->levelMask->startingOffset[2]) / level->levelMask->scaleZ;
endPos[0] = (endPos[0] - level->levelMask->startingOffset[0]) / level->levelMask->scaleX;
endPos[1] = 0.0f;
endPos[2] = (endPos[2] - level->levelMask->startingOffset[2]) / level->levelMask->scaleZ;
*/
start = findNodeInGraph(startPos, level);
goal = findNodeInGraph(endPos, level);
//if goal is in collision zone, return so game does not freeze
if(goal->neighbor[0] == NULL &&
goal->neighbor[1] == NULL &&
goal->neighbor[2] == NULL &&
goal->neighbor[3] == NULL)
{
//cout << "ERROR, goal is in collision zone" << endl;
return;
}
open.push_back(start);
//while lowest ranked node's postion in the open list is not equal to the goal's position
//if it is equal we are done finding our path
while(open.front()->pos != goal->pos)
{
current = open.front();
//cout<< current->pos << endl; //##############################################################################################################
removeFromOpen(open.front());
//open.clear();
//put node in closed so we dont reprocess a node that we already processed
closed.push_back(current);
//process current's neighbors to look for the right path
for(int i = 0; i < 4; i++)
{
if(current->neighbor[i] != NULL)
{
//float cost = g(current) + movementCost(current->neighbor[i]);
////if neighbor in open and cost less than g(neighbor)
//if(inOpen(current->neighbor[i]) && cost < g(current->neighbor[i]))
////if(inOpen(current->neighbor[i]) && h(current) < h(current->neighbor[i]))
//{
// //remove neighbor from open, because new path is better
// removeFromOpen(current->neighbor[i]);
// cout << "GGGGG" << endl;
//}
////if neighbor in closed and cost less than g(neighbor)
//else if(inClosed(current->neighbor[i]) && cost < g(current->neighbor[i]))
////else if(inClosed(current->neighbor[i]) && h(current) < h(current->neighbor[i]))
//{
// removeFromClosed(current->neighbor[i]);
// cout << "AAAA" << endl;
//}
//if neighbor not in closed and neighbor not in open
if((!inOpen(current->neighbor[i])) && (!inClosed(current->neighbor[i])))
{
open.push_back(current->neighbor[i]);
current->neighbor[i]->rank = g(current->neighbor[i]) + h(current->neighbor[i]);
current->neighbor[i]->parent = current;
}
}
}
//*******************SORT*********************************************//
//bubble sort
int check = open.size();
//ERROR check
if(check > 1)
{
//sort open list from lowest rank to highest
for(unsigned int counter = 0; counter < check; counter++)
{
list<node*>::iterator k = open.end();
k--;
list<node*>::iterator m = open.end();
m--;
m--;
unsigned int countEnd = check - 1;
while(countEnd > counter)
{
if((*k)->rank < ((*m)->rank))
{
node *temp;
temp = new node();
temp = (*k);
(*k) = (*m);
(*m) = temp;
temp = NULL;
delete temp;
}
if(countEnd-1 != counter)
{
k--;
m--;
}
countEnd--;
}
}
//optimizes pathfinding by shinking open list to fewer nodes to sort
if(open.size() > 20)
{
unsigned int openSize = check / 2;
list<node*>::iterator k = open.end();
k--;
//remove some of the highest ranked nodes to make open list process faster
for(unsigned int counter = 0; counter < openSize; counter++)
{
//put node in closed so we dont reprocess a node that we already processed
closed.push_back((*k));
removeFromOpen((*k));
k = open.end();
k--;
}
}
}
else
{
//cout << "ERROR, open list in AStarPathFinding.h contains zero elements, NPC start point is in collision zone. " << endl;
//clear the open and closed lists so they are ready for the next iteration
open.clear();
closed.clear();
if(!firstTime)
{
//clear the path list from the previous determined path
path.clear();
}
return;
}
//*************************************************************************************************/
}
//reconstruct path from goal to start by following parent pointers
getPath( startPos, endPos, level->levelMask);
//clear the open and closed lists so they are ready for the next iteration
open.clear();
closed.clear();
}
