void OpenQueue::Update( PathNode* pNode )
{
#ifdef DEBUG_PATH_DEEP
printf( "Open Update: " );
graph->PrintStateInfo( pNode->state );
printf( " total=%.1f\n", pNode->totalCost );
#endif
MPASSERT( pNode->inOpen );
// If the node now cost less than the one before it,
// move it to the front of the list.
if ( pNode->prev != sentinel && pNode->totalCost < pNode->prev->totalCost ) {
pNode->Unlink();
sentinel->next->AddBefore( pNode );
}
// If the node is too high, move to the right.
if ( pNode->totalCost > pNode->next->totalCost ) {
PathNode* it = pNode->next;
pNode->Unlink();
while ( pNode->totalCost > it->totalCost )
it = it->next;
it->AddBefore( pNode );
#ifdef DEBUG
sentinel->CheckList();
#endif
}
}
void OpenQueue::Push( PathNode* pNode )
{
MPASSERT( pNode->inOpen == 0 );
MPASSERT( pNode->inClosed == 0 );
#ifdef DEBUG_PATH_DEEP
printf( "Open Push: " );
graph->PrintStateInfo( pNode->state );
printf( " total=%.1f\n", pNode->totalCost );
#endif
// Add sorted. Lowest to highest cost path. Note that the sentinel has
// a value of FLT_MAX, so it should always be sorted in.
MPASSERT( pNode->totalCost < FLT_MAX );
PathNode* iter = sentinel->next;
while ( true )
{
if ( pNode->totalCost < iter->totalCost ) {
iter->AddBefore( pNode );
pNode->inOpen = 1;
break;
}
iter = iter->next;
}
MPASSERT( pNode->inOpen ); // make sure this was actually added.
#ifdef DEBUG
sentinel->CheckList();
#endif
}
void Follower::resetStartNode()
{
mStartNode->setPosition(mPosition);
//find closest existing node and use the same links as it has
PathNode* close;
gCurrentLevel->getPaths()->getNodeNearPosition(close, mPosition);
mStartNode->setLinks(close->getLinks());
}
PathNode* PathGraph::findGlobalNode(int globalNodeID) {
for (int i = 0; i < pathNodes.size(); ++i) {
PathNode* pathNode = pathNodes.get(i);
if (pathNode->getGlobalGraphNodeID() == globalNodeID)
return pathNode;
}
return NULL;
}
void Pathfinder::BuildPath(PathNode * lastNode) {
m_finalPath.clear();
m_edgesPath.clear();
//iterate over all node parents to get the full path
PathNode * node = lastNode;
USVec2D edgeDir;
USVec2D edgePos;
float edgeLength;
while (node->GetParent()) {
m_finalPath.push_back(node);
for (std::vector<Polygon::Edge>::iterator itr = node->GetPolygon()->m_edges.begin();
itr != node->GetPolygon()->m_edges.end(); ++itr) {
if (itr->m_neighbour == node->GetParent()->GetPolygon()) {
edgeDir = node->GetPolygon()->m_vertices[itr->m_verticesID[1]]
- node->GetPolygon()->m_vertices[itr->m_verticesID[0]];
edgeLength = edgeDir.Length();
edgePos = node->GetPolygon()->m_vertices[itr->m_verticesID[0]]
+ (edgeDir.NormVector() * (edgeLength / 2));
m_edgesPath.push_back(edgePos);
}
}
node = node->GetParent();
}
node = node;
}
void NavigationHandle::ClearGraphPathInfo()
{
for(unsigned int i = 0; i < Graph->count(); i++)
{
PathNode* node = dynamic_cast<PathNode*>(Graph->objectAtIndex(i));
node->ClearPathInfo();
}
Path->removeAllObjects();
PathIndex = 0;
}
std::vector<PathNode*> Pathfinding::FindPath(PathNode* start, PathNode* goal)
{
std::vector<PathNode*> retPath;
PathNode* currentNode = new PathNode(*start);
currentNode->combineNode(currentNode, start);
while (!ArrivedAtEnd(currentNode, goal))
{
PathNode tempChildNode(*currentNode);
//Get adjacent walkable tiles
//Move the child node one node to the right to get the node to the right of currentNode
tempChildNode.xPos++;
AddChild(tempChildNode, currentNode, goal, Direction::DIRECTION::EAST);
//Move the child node to the left to get the node to the left of currentNode
tempChildNode.xPos -= 2;
AddChild(tempChildNode, currentNode, goal, Direction::DIRECTION::WEST);
//Move the child node up one row to get the node above currentNode
tempChildNode.xPos++;
tempChildNode.zPos++;
AddChild(tempChildNode, currentNode, goal, Direction::DIRECTION::NORTH);
//Finally, move the child node to the bottom, to get the node one below currentNode
tempChildNode.zPos -= 2;
AddChild(tempChildNode, currentNode, goal, Direction::DIRECTION::SOUTH);
mClosedSet.insert(currentNode);
mOpenList.sort(PathNode::FCostSort());
if (mOpenList.size() > 0)
{
currentNode = mOpenList.back();
mOpenList.remove(currentNode);
}
else
{
break;
}
}
//Populate and create the path vector
while (currentNode->parent != NULL && currentNode != start)
{
retPath.push_back(currentNode);
currentNode = currentNode->getParent();
}
std::reverse(retPath.begin(), retPath.end());
mOpenList.clear();
mClosedSet.clear();
return retPath;
}
PathNode* MicroPather::NewPathNode( void* state, float costFromStart, float estToEnd, PathNode* parent )
{
// Try to find an existing node for this state.
unsigned key = Hash( state ); //(HASH_SIZE-1) & ( (unsigned)state + (((unsigned)state)>>8) + (((unsigned)state)>>16) + (((unsigned)state)>>24) );
if ( !hashTable[key] ) {
// There isn't even a hashtable yet - create and initialize the PathNode.
hashTable[key] = AllocatePathNode();
hashTable[key]->Init( frame, state, costFromStart, estToEnd, parent );
return hashTable[key];
}
PathNode* root = hashTable[key];
PathNode* up = 0;
while ( root ) {
up = root;
if ( root->state == state ) {
root->Reuse( frame, costFromStart, estToEnd, parent );
assert( root->state == state );
return root;
}
#ifdef USE_BINARY_HASH
else if ( state > root->state ) {
root = root->right;
}
#endif
else {
#ifdef USE_BINARY_HASH
assert( state < root->state );
#endif
root = root->left;
}
}
assert( up );
PathNode* pNode = AllocatePathNode();
pNode->Init( frame, state, costFromStart, estToEnd, parent );
#ifdef USE_BINARY_HASH
if ( state > up->state ) {
assert( up->right == 0 );
up->right = pNode;
}
else {
assert( up->left == 0 );
up->left = pNode;
}
#else
up->left = pNode;
#endif
return pNode;
}
void GopathBrowser::pathIndexChanged(const QModelIndex & index)
{
PathNode *node = m_model->nodeFromIndex(index);
if (node) {
QFileInfo info = node->fileInfo();
QModelIndex newIndex = index;
if (info.isDir()) {
newIndex = index;
} else {
newIndex = index.parent();
}
this->setStartIndex(newIndex);
}
}
PathNode* PathNode::create(TilePoint pos)
{
PathNode* node = new PathNode();
if(node && node->init(pos))
{
node->autorelease();
return node;
}
else
{
CC_SAFE_DELETE(node);
return NULL;
}
}
void GopathBrowser::openPathIndex(const QModelIndex &index)
{
PathNode *node = m_model->nodeFromIndex(index);
if (!node) {
return;
}
if (node->isDir()) {
this->setStartIndex(index);
} else if (node->isFile()) {
if (m_syncProject->isChecked()) {
this->setStartIndex(index.parent());
}
m_liteApp->fileManager()->openEditor(node->path(),true);
}
}
void GopathBrowser::setStartIndex(const QModelIndex &index)
{
QModelIndex oldIndex = m_model->startIndex();
if (oldIndex != index) {
m_model->setStartIndex(index);
m_pathTree->update(oldIndex);
m_pathTree->update(index);
emit startPathChanged(m_model->filePath(index));
PathNode *node = m_model->nodeFromIndex(index);
if (node) {
m_startPathLabel->setText(QString("<p><a href file://%1>%2</p>").arg(node->path()).arg(node->text()));
m_startPathLabel->setToolTip(node->path());
}
}
}
bool KBlocksAIPlannerExtend::getPath(int index, AIPlanner_PieceInfo_Sequence *pseq)
{
if ((mPathList == 0) || (index >= (int)mPathList->size())) {
return false;
}
PathNode *node = (*mPathList)[index];
while (node != 0) {
KBlocksPiece piece = node->getContent();
pseq->push_back(piece);
node = node->getParent();
}
return true;
}
float CalculateGhostEffect(const PathNode& in_node, const std::vector<const Pawn* const> in_ghosts)
{
float score = 0;
for(auto ghost : in_ghosts)
{
score += (ghost->GetPosition() - in_node.GetPosition()).MagnitudeSqr() / 1.0f;
if (ghost->GetClosestNode() == in_node.GetId())
score += 1000;
//When blue boost our want to go towards the ghost
if (ghost->GetState() == 2)
score = -score;
}
return score;
}
CCArray* NavigationHandle::InsertOpenList(cocos2d::CCArray* openList, PathNode* node)
{
for(unsigned int i = 0; i < openList->count(); i++)
{
PathNode* kNode = dynamic_cast<PathNode*>(openList->objectAtIndex(i));
if(node->fScore() <= kNode->fScore())
{
openList->insertObject(node, i);
return openList;
}
}
openList->addObject(node);
return openList;
}
bool KBlocksAIPlannerExtend::getNextPieceState(int index, KBlocksPiece *piece)
{
if (index >= (int)mPathList->size()) {
return false;
}
PathNode *node = (*mPathList)[index];
PathNode *last = 0;
while (node != 0) {
last = node;
node = node->getParent();
}
if (last == 0) {
return false;
}
*piece = last->getContent();
return true;
}
float CalculatePointBonus(const PathNode& in_node)
{
const PointObj* const pointObj = in_node.GetObject();
if (!pointObj)
return 1;
return -pointObj->GetWorth();
}
void PortalLayout::connectFloorMeshGraphs() {
for (int i = 0; i < cellProperties.size(); ++i) {
FloorMesh* floorMesh = getFloorMesh(i);
if (floorMesh == NULL)
continue;
PathGraph* pathGraph = floorMesh->getPathGraph();
if (pathGraph == NULL)
continue;
Vector<PathNode*> globalNodes = pathGraph->getGlobalNodes();
for (int j = 0; j < globalNodes.size(); ++j) {
PathNode* node = globalNodes.get(j);
int globalID = node->getGlobalGraphNodeID();
for (int k = 0; k < cellProperties.size(); ++k) {
if (i != k) {
FloorMesh* newMesh = getFloorMesh(k);
if (newMesh != NULL) {
PathGraph* newPathGraph = newMesh->getPathGraph();
if (newPathGraph != NULL) {
Vector<PathNode*> newGlobalNodes = newPathGraph->getGlobalNodes();
for (int l = 0; l < newGlobalNodes.size(); ++l) {
PathNode* newNode = newGlobalNodes.get(l);
int newGlobalID = newNode->getGlobalGraphNodeID();
if (globalID == newGlobalID)
node->addChild(newNode);
}
}
}
}
}
}
}
}
PathNode* PathNodePool::Alloc()
{
if ( freeMemSentinel.next == &freeMemSentinel ) {
MPASSERT( nAvailable == 0 );
Block* b = NewBlock();
b->nextBlock = blocks;
blocks = b;
MPASSERT( freeMemSentinel.next != &freeMemSentinel );
}
PathNode* pathNode = freeMemSentinel.next;
pathNode->Unlink();
++nAllocated;
MPASSERT( nAvailable > 0 );
--nAvailable;
return pathNode;
}
PathNode* PathFinder::getPathNodeFromOpenList(Tile* aTile)
{
//Get the tile's tile index.
int tileIndex = m_Level->getTileIndexForTile(aTile);
//Cycle through the open list and compare the tile indexes.
for(int i = 0; i < m_OpenList.size(); i++)
{
PathNode* pathNode = m_OpenList.at(i);
if(m_Level->getTileIndexForTile(pathNode->getTile()) == tileIndex)
{
return pathNode;
}
}
//If we got here then the tile is not in the open list.
return false;
}
bool PathFinder::doesTileExistInClosedList(Tile* aTile)
{
//Get the tile index from the level for the tile pointer
int tileIndex = m_Level->getTileIndexForTile(aTile);
//Cycle through the closed list and compare the tile indexes
for(int i = 0; i < m_PathNodeClosed.size(); i++)
{
PathNode* pathNode = m_PathNodeClosed.at(i);
if(m_Level->getTileIndexForTile(pathNode->getTile()) == tileIndex)
{
return true;
}
}
//The tile doesn't exist in the closed list
return false;
}
bool PathFinder::isTileInClosedList(Tile* aTile)
{
//Get the tile's tile index.
int tileIndex = m_Level->getTileIndexForTile(aTile);
//Cycle through the closed list and compare the tile indexes.
for(int i = 0; i < m_ClosedList.size(); i++)
{
PathNode* pathNode = m_ClosedList.at(i);
if(m_Level->getTileIndexForTile(pathNode->getTile()) == tileIndex)
{
return true;
}
}
//If we got here then the tile is not in the closed list.
return false;
}
PathNode* PathFinder::getOpenPathNodeForTile(Tile* aTile)
{
//Get the tile index from the level for the tile pointer
int tileIndex = m_Level->getTileIndexForTile(aTile);
//Cycle through the open list and compare the tile indexes
for(int i = 0; i < m_PathNodeOpen.size(); i++)
{
PathNode* pathNode = m_PathNodeOpen.at(i);
if(m_Level->getTileIndexForTile(pathNode->getTile()) == tileIndex)
{
return pathNode;
}
}
//The tile doesn't exist in the open list, return NULL
return NULL;
}
PathNode* PathGraph::findNearestNode(const Vector3& pointAlfa) {
float minDistance = 160000000.f;
PathNode* node = NULL;
for (int i = 0; i < pathNodes.size(); ++i) {
PathNode* pathNode = pathNodes.get(i);
Vector3 point(pathNode->getX(), pathNode->getY(), pathNode->getZ());
float sqrDistance = pointAlfa.squaredDistanceTo(point);
if (sqrDistance < minDistance) {
minDistance = sqrDistance;
node = pathNode;
}
}
return node;
}
//-----------------------------------------------------------------------------------
bool Path::FindPathStep()
{
if (m_openList.empty() && m_hasBegun)
{
return false;
}
PathNode* activeNode = FindLowestFNodeOnOpenListAndRemoveIt();
m_closedList.push_back(activeNode);
if (activeNode->position == m_currentGoal)
{
RecursivelyBuildPathToStartFromNode(activeNode);
m_hasBegun = false;
m_openList.clear();
m_closedList.clear();
m_currentGoal = -Vector2Int::ONE;
return true;
}
std::vector<PathNode*> validPositions;
FindValidAdjacentPositions(validPositions, activeNode, m_currentGoal);
for (PathNode* positionNode : validPositions)
{
MapPosition position = positionNode->position;
if (IsPositionOnClosedList(positionNode->position))
continue;
float localG = positionNode->g; //The computed local G.
float parentG = activeNode->g; //The parent's actual G, which includes the entire cost to get here.
float h = positionNode->h; //The computed H for the proposed posiion
PathNode* existingNode = FindOpenNodeWithPosInOpenList(position);
if (existingNode)
{
if (localG + parentG < existingNode->g)
{
existingNode->UpdateVariables(activeNode, localG + parentG, h);
}
continue;
}
PathNode* newNode = new PathNode(position, activeNode, localG + parentG, h);
m_openList.push_back(newNode);
}
return false;
}
static inline bool addAliasImpl(PathNode &node, std::string const &source,
AliasPriority priority) {
if (!aliasNeedsUpdate(node, source, priority)) {
return false;
}
elements::AliasElement elt;
elt.setSource(source);
elt.priority() = priority;
node.value() = elt;
return true;
}
PathNode* OpenQueue::Pop()
{
MPASSERT( sentinel->next != sentinel );
PathNode* pNode = sentinel->next;
pNode->Unlink();
#ifdef DEBUG
sentinel->CheckList();
#endif
MPASSERT( pNode->inClosed == 0 );
MPASSERT( pNode->inOpen == 1 );
pNode->inOpen = 0;
#ifdef DEBUG_PATH_DEEP
printf( "Open Pop: " );
graph->PrintStateInfo( pNode->state );
printf( " total=%.1f\n", pNode->totalCost );
#endif
return pNode;
}
void PathGraph::connectNodes(Vector<PathEdge>& pathEdges) {
for (int i = 0; i < pathEdges.size(); ++i) {
PathEdge* pathEdge = &pathEdges.get(i);
int from = pathEdge->getFromConnection();
int to = pathEdge->getToConnection();
PathNode* fromNode = pathNodes.get(from);
PathNode* toNode = pathNodes.get(to);
/*Vector<PathNode*>* path = AStarAlgorithm<PathGraph, PathNode>::search<uint32>(this, fromNode, toNode);
if (path != NULL) {
System::out << "found path\n";
delete path;
} else {
System::out << "didint find path\n";
}*/
fromNode->addChild(toNode);
}
}
void GopathBrowser::treeViewContextMenuRequested(const QPoint &pos)
{
QModelIndex index = m_pathTree->indexAt(pos);
if (!index.isValid()) {
return;
}
PathNode *node = m_model->nodeFromIndex(index);
if (!node) {
return;
}
m_contextInfo = node->fileInfo();
m_contextIndex = index;
QMenu *contextMenu = 0;
if (node->isDir()) {
contextMenu = m_folderMenu;
} else {
contextMenu = m_fileMenu;
}
if (contextMenu && contextMenu->actions().count() > 0) {
contextMenu->popup(m_pathTree->mapToGlobal(pos));
}
}
PathNode* Pop()
{
#ifdef USE_LIST
assert( sentinel->next != sentinel );
PathNode* pNode = sentinel->next;
pNode->Unlink();
#ifdef DEBUG
sentinel->CheckList();
#endif
#else
PathNode* pNode = heapVector[0];
const int size = heapVector.size();
int found = 0;
for( int i=1; i<size; ++i ) {
if ( heapVector[i]->totalCost < pNode->totalCost ) {
pNode = heapVector[i];
found = i;
}
}
if ( found < size-1 )
memcpy( &heapVector[found], &heapVector[found+1], sizeof( PathNode* ) * (size-found-1) );
heapVector.pop_back();
#endif
assert( pNode->inClosed == 0 );
assert( pNode->inOpen == 1 );
pNode->inOpen = 0;
#ifdef DEBUG_PATH_DEEP
printf( "Open Pop: " );
graph->PrintStateInfo( pNode->state );
printf( " total=%.1f\n", pNode->totalCost );
#endif
return pNode;
}