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;
}
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;
}
TEST(addDevice, normalConditions) {
PathTree tree;
PathNode *dev = nullptr;
ASSERT_NO_THROW(dev = &addDevice(tree, "/com_osvr_sample/MyDevice"));
// Check Device
ASSERT_EQ(dev->getName(), "MyDevice");
ASSERT_FALSE(dev->hasChildren()) << "Make sure it has no children.";
ASSERT_TRUE(isNodeType<elements::DeviceElement>(*dev)) << "Check type";
ASSERT_NE(dev->getParent(), nullptr) << "Make sure it has a parent.";
// Check com_osvr_sample
auto plugin = dev->getParent();
ASSERT_EQ(plugin->getName(), "com_osvr_sample");
ASSERT_TRUE(isNodeType<elements::PluginElement>(*plugin)) << "Check type";
ASSERT_NE(plugin->getParent(), nullptr) << "Make sure it has a parent.";
auto root = plugin->getParent();
ASSERT_TRUE(root->isRoot());
ASSERT_EQ(tree.getNodeByPath("/"), *root)
<< "Root identity should be preserved";
ASSERT_EQ(tree.getNodeByPath("/com_osvr_sample/MyDevice"), *dev)
<< "Identity should be preserved";
}
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;
}
void PathCmp::search (Eigen::Vector2f targetPos_) {
_level = LogicSystem::getInstance()->getLevel();
for (auto it = _openList.begin(); it != _openList.end(); ++ it) {
if (*it != nullptr)
delete *it;
}
_openList.clear();
for (auto it = _closedList.begin(); it != _closedList.end(); ++ it) {
if (*it != nullptr)
delete *it;
}
_closedList.clear();
_shortestPath.clear();
auto posCmp = getEntity()->GET_CMP(PositionComponent);
_srcIdx = _level->posToTileIndex(posCmp->getPosition());
_tarIdx = _level->posToTileIndex(targetPos_);
if (_srcIdx == _tarIdx)
return;
if (_level->isTileObstacle(_tarIdx.x(), _tarIdx.y()))
return;
PathNode* srcNode = new PathNode(_srcIdx);
calcCostH(srcNode);
_openList.push_back(srcNode);
do {
PathNode* curNode = _openList[0];
_closedList.push_back(curNode);
_openList.erase(_openList.begin());
if (curNode->getIndex() == _tarIdx) {
PathNode* walkNode = curNode;
_shortestPath.clear();
do {
Eigen::Vector2f pos;
pos.x() = _level->tileIndexXToPosX(walkNode->getIndex().x());
pos.y() = _level->tileIndexYToPosY(walkNode->getIndex().y());
_shortestPath.push_front(pos);
walkNode = walkNode->getParent();
} while (walkNode);
for (auto it = _openList.begin(); it != _openList.end(); ++ it) {
if (*it != nullptr)
delete *it;
}
_openList.clear();
for (auto it = _closedList.begin(); it != _closedList.end(); ++ it) {
if (*it != nullptr)
delete *it;
}
_closedList.clear();
break;
}
putAdjacentTiles(curNode);
for (auto it = _adjacentTiles.begin(); it != _adjacentTiles.end(); ++ it) {
bool isInClosedList = false;
for (int i = 0; i < _closedList.size(); ++ i)
if ((*it) == _closedList[i]->getIndex())
isInClosedList = true;
if (isInClosedList) continue;
auto node = new PathNode(*it);
int costGToAdd = calcCostG(node, curNode);
std::vector<PathNode *>::iterator opit = _openList.begin();
while (opit != _openList.end()) {
if ((*opit)->getIndex() == node->getIndex())
break;
++ opit;
}
// already on openlist
if (opit!= _openList.end()) {
delete node;
node = (*opit);
if (curNode->getCostG() + costGToAdd < node->getCostG())
node->setCostG(curNode->getCostG() + costGToAdd);
} else {
node->setParent(curNode);
node->setCostG(curNode->getCostG() + costGToAdd);
calcCostH(node);
addOpenList(node);
}
}
} while (_openList.size() > 0);
}