// Helper method which attempts to add a SearchNode to the output vector if it is traversable.
void AStarPlanner::_tryToAdd(unsigned int x, unsigned int z, const SearchNodePtr& from, float cost, Util::Point goal, std::vector<SearchNodePtr>& out) {
int index = gSpatialDatabase->getCellIndexFromGridCoords(x, z);
if (!canBeTraversed(index)) return;
Util::Point p;
gSpatialDatabase->getLocationFromIndex(index, p);
// CHANGE weight for the weighted A*
float weight = 1;
// CHANGE to distanceBetween for Euclidean distance and manhattan_distance for Manhattan distance
float h = distanceBetween(p, goal) * weight;
out.push_back(SearchNodePtr(new SearchNode(index, from->g() + cost, h)));
}
std::vector<AStarPlannerNode> AStarPlanner::findNeighbor(Util::Point p1, SteerLib::GridDatabase2D* gSpatialDatabase){
int index;
std::vector<AStarPlannerNode> result;
Util::Point p;
unsigned x, z;
index = gSpatialDatabase->getCellIndexFromLocation(p1);
gSpatialDatabase->getGridCoordinatesFromIndex(index, x, z);
index = gSpatialDatabase->getCellIndexFromGridCoords(x, z - 1);
if(canBeTraversed(index)){
gSpatialDatabase->getLocationFromIndex(index, p);
AStarPlannerNode temp = AStarPlannerNode(p, DBL_MAX, DBL_MAX, NULL);
result.push_back(temp);
}
index = gSpatialDatabase->getCellIndexFromGridCoords(x+1, z);
if(canBeTraversed(index)){
gSpatialDatabase->getLocationFromIndex(index, p);
AStarPlannerNode temp = AStarPlannerNode(p, DBL_MAX, DBL_MAX, NULL);
result.push_back(temp);
}
index = gSpatialDatabase->getCellIndexFromGridCoords(x, z+1);
if(canBeTraversed(index)){
gSpatialDatabase->getLocationFromIndex(index, p);
AStarPlannerNode temp = AStarPlannerNode(p, DBL_MAX, DBL_MAX, NULL);
result.push_back(temp);
}
index = gSpatialDatabase->getCellIndexFromGridCoords(x-1, z);
if(canBeTraversed(index)){
gSpatialDatabase->getLocationFromIndex(index, p);
AStarPlannerNode temp = AStarPlannerNode(p, DBL_MAX, DBL_MAX, NULL);
result.push_back(temp);
}
index = gSpatialDatabase->getCellIndexFromGridCoords(x-1, z-1);
if(canBeTraversed(index)){
gSpatialDatabase->getLocationFromIndex(index, p);
AStarPlannerNode temp = AStarPlannerNode(p, DBL_MAX, DBL_MAX, NULL);
result.push_back(temp);
}
index = gSpatialDatabase->getCellIndexFromGridCoords(x+1, z-1);
if(canBeTraversed(index)){
gSpatialDatabase->getLocationFromIndex(index, p);
AStarPlannerNode temp = AStarPlannerNode(p, DBL_MAX, DBL_MAX, NULL);
result.push_back(temp);
}
index = gSpatialDatabase->getCellIndexFromGridCoords(x+1, z+1);
if(canBeTraversed(index)){
gSpatialDatabase->getLocationFromIndex(index, p);
AStarPlannerNode temp = AStarPlannerNode(p, DBL_MAX, DBL_MAX, NULL);
result.push_back(temp);
}
index = gSpatialDatabase->getCellIndexFromGridCoords(x-1, z+1);
if(canBeTraversed(index)){
gSpatialDatabase->getLocationFromIndex(index, p);
AStarPlannerNode temp = AStarPlannerNode(p, DBL_MAX, DBL_MAX, NULL);
result.push_back(temp);
}
return result;
}
bool AStarPlanner::computePath(std::vector<Util::Point>& agent_path, Util::Point start, Util::Point goal, SteerLib::GridDatabase2D * _gSpatialDatabase, bool append_to_path)
{
gSpatialDatabase = _gSpatialDatabase;
std::deque<Node> openNodes; //List of nodes to visit and their priority
std::vector<Node> visitedNodes; //List of nodes that have already been visited
Node currNode;
std::vector<Node> neighbors;
std::map<int, int> parents; //ID of node, value = parent of that node. -1 means no parent
//Add start node to queue
Node startNode;
Util::Point startPos;
unsigned int startX, startZ, startIndex;
startIndex = gSpatialDatabase->getCellIndexFromLocation(start);
gSpatialDatabase->getGridCoordinatesFromIndex(startIndex, startX, startZ);
gSpatialDatabase->getLocationFromIndex(startIndex, startPos);
startNode.position = startPos;
startNode.gCost = 0;
startNode.hCost = computeHCost(HEURISTIC, startNode, goal);
startNode.fCost = startNode.gCost + startNode.fCost;
startNode.isDiag = false;
startNode.id = gSpatialDatabase->getCellIndexFromLocation(startNode.position);
openNodes.push_back(startNode);
agent_path.push_back(startNode.position);
parents[startNode.id] = -1; //start node has no parent
//Set goal node
Node goalNode;
Util::Point goalPos;
unsigned int goalX, goalZ, goalIndex;
goalIndex = gSpatialDatabase->getCellIndexFromLocation(goal);
gSpatialDatabase->getGridCoordinatesFromIndex(goalIndex, goalX, goalZ);
gSpatialDatabase->getLocationFromIndex(goalIndex, goalPos);
goalNode.id = goalIndex;
goalNode.position = goalPos;
//Loop until there are no more nodes to visit
while (openNodes.size() > 0)
{
//Select node with lowest fCost
int lowest = 9999;
int index = 0;
bool breakTie = false;
std::vector<Node> equalFNodes;
for (int b = 0; b < openNodes.size(); b++)
{
if (openNodes[b].fCost < lowest) //For later part, add equality check and compare g/h costs
{
lowest = openNodes[b].fCost;
currNode = openNodes[b];
index = b;
}
else if (openNodes[b].fCost == lowest && TIEBREAKER != -1)
{
breakTie = true;
equalFNodes.push_back(openNodes[b]);
}
}
if (breakTie == false)
{
currNode.id = gSpatialDatabase->getCellIndexFromLocation(currNode.position);
visitedNodes.push_back(openNodes[index]);
openNodes.erase(openNodes.begin() + index);
}
else
{
int lowestG = 9999;
int lowestIndex = 0;
int highestG = -1;
int highestIndex = 0;
for (int i = 0; i < equalFNodes.size()-1; i++)
{
if (equalFNodes[i].gCost < lowestG)
{
lowestG = equalFNodes[i].gCost;
lowestIndex = i;
}
if (equalFNodes[i].gCost > highestG)
{
highestG = equalFNodes[i].gCost;
highestIndex = i;
}
}
if (TIEBREAKER == 0) //Select lowest gCost
{
currNode = equalFNodes[lowestIndex];
currNode.id = gSpatialDatabase->getCellIndexFromLocation(currNode.position);
visitedNodes.push_back(openNodes[lowestIndex]);
openNodes.erase(openNodes.begin() + lowestIndex);
}
else if (TIEBREAKER > 0) //Select highest gCost
{
currNode = equalFNodes[highestIndex];
currNode.id = gSpatialDatabase->getCellIndexFromLocation(currNode.position);
visitedNodes.push_back(openNodes[highestIndex]);
openNodes.erase(openNodes.begin() + highestIndex);
}
}
//Found goal
if (currNode.position.operator==(goalPos))
{
//std::cout << "\n GOAL REACHED!";
break;
}
//Get the neighbors of the current node
unsigned int currIndex = gSpatialDatabase->getCellIndexFromLocation(currNode.position);
unsigned int currXCoord = 0;
unsigned int currZCoord = 0;
gSpatialDatabase->getGridCoordinatesFromIndex(currIndex, currXCoord, currZCoord);
unsigned int neighborXCoord = currXCoord;
unsigned int neighborZCoord = currZCoord;
float travCost = 0;
Node neighborNode;
Util::Point neighborPos;
neighborXCoord--; //Check cell at (x-1, z)
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
//Check if neighboring cell is a node. Obstacles have a traversal cost of 1000
gSpatialDatabase->getLocationFromIndex(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord), neighborPos); //(x-1, z)
if (travCost >= 0 && travCost < COLLISION_COST && canBeTraversed(gSpatialDatabase->getCellIndexFromLocation(neighborPos)))
{
//Add neighbor to list
neighborNode.position = neighborPos;
neighborNode.isDiag = false;
neighborNode.gCost = travCost + currNode.gCost;
neighborNode.hCost = computeHCost(HEURISTIC, neighborNode, goal);
neighborNode.fCost = neighborNode.gCost + neighborNode.hCost;
//neighborNode.id = currNode.id + 1;
neighborNode.id = gSpatialDatabase->getCellIndexFromLocation(neighborNode.position);
neighbors.push_back(neighborNode);
//Reset values for the next check
neighborXCoord = currXCoord; //Check cell at (x+1, z)
neighborZCoord = currZCoord;
neighborXCoord += 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
else //Not a traversible node
{
//Reset values for the next check
neighborXCoord = currXCoord; //Check cell at (x+1, z)
neighborZCoord = currZCoord;
neighborXCoord += 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
gSpatialDatabase->getLocationFromIndex(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord), neighborPos); //(x+1, z)
if (travCost >= 0 && travCost < COLLISION_COST && canBeTraversed(gSpatialDatabase->getCellIndexFromLocation(neighborPos)))
{
//Add neighbor to list
neighborNode.position = neighborPos;
neighborNode.isDiag = false;
neighborNode.gCost = travCost + currNode.gCost;
neighborNode.hCost = computeHCost(HEURISTIC, neighborNode, goal);
neighborNode.fCost = neighborNode.gCost + neighborNode.hCost;
//neighborNode.id = currNode.id + 2;
neighborNode.id = gSpatialDatabase->getCellIndexFromLocation(neighborNode.position);
neighbors.push_back(neighborNode);
//Reset values for the next check
neighborXCoord = currXCoord; //Check cell at (x, z-1)
neighborZCoord = currZCoord;
neighborZCoord -= 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
else //Not a traversible node
{
//Reset values for the next check
neighborXCoord = currXCoord;//Check cell at (x, z-1)
neighborZCoord = currZCoord;
neighborZCoord -= 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
gSpatialDatabase->getLocationFromIndex(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord), neighborPos); //(x, z-1)
if (travCost >= 0 && travCost < COLLISION_COST && canBeTraversed(gSpatialDatabase->getCellIndexFromLocation(neighborPos)))
{
//Add neighbor to list
neighborNode.position = neighborPos;
neighborNode.isDiag = false;
neighborNode.gCost = travCost + currNode.gCost;
neighborNode.hCost = computeHCost(HEURISTIC, neighborNode, goal);
neighborNode.fCost = neighborNode.gCost + neighborNode.hCost;
//neighborNode.id = currNode.id + 3;
neighborNode.id = gSpatialDatabase->getCellIndexFromLocation(neighborNode.position);
neighbors.push_back(neighborNode);
//Reset values for the next check
neighborXCoord = currXCoord; //Check cell at (x, z+1)
neighborZCoord = currZCoord;
neighborZCoord += 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
else //Not a traversible node
{
//Reset values for the next check
neighborXCoord = currXCoord; //Check cell at (x, z+1)
neighborZCoord = currZCoord;
neighborZCoord += 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
gSpatialDatabase->getLocationFromIndex(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord), neighborPos); //(x, z+1)
if (travCost >= 0 && travCost < COLLISION_COST && canBeTraversed(gSpatialDatabase->getCellIndexFromLocation(neighborPos)))
{
//Add neighbor to list
neighborNode.position = neighborPos;
neighborNode.isDiag = false;
neighborNode.gCost = travCost + currNode.gCost;
neighborNode.hCost = computeHCost(HEURISTIC, neighborNode, goal);
neighborNode.fCost = neighborNode.gCost + neighborNode.hCost;
//neighborNode.id = currNode.id + 4;
neighborNode.id = gSpatialDatabase->getCellIndexFromLocation(neighborNode.position);
neighbors.push_back(neighborNode);
//Reset values for the next check
neighborXCoord = currXCoord; //Check cell at (x-1, z-1)
neighborZCoord = currZCoord;
neighborXCoord -= 1;
neighborZCoord -= 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
else //Not a traversible node
{
//Reset values for the next check
neighborXCoord = currXCoord; //Check cell at (x-1, z-1)
neighborZCoord = currZCoord;
neighborXCoord -= 1;
neighborZCoord -= 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
gSpatialDatabase->getLocationFromIndex(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord), neighborPos); //(x-1, z-1)
if (travCost >= 0 && travCost < COLLISION_COST && canBeTraversed(gSpatialDatabase->getCellIndexFromLocation(neighborPos)))
{
//Add neighbor to list
neighborNode.position = neighborPos;
neighborNode.isDiag = true;
neighborNode.gCost = travCost + currNode.gCost;
neighborNode.hCost = computeHCost(HEURISTIC, neighborNode, goal);
neighborNode.fCost = neighborNode.gCost + neighborNode.hCost;
//neighborNode.id = currNode.id + 5;
neighborNode.id = gSpatialDatabase->getCellIndexFromLocation(neighborNode.position);
neighbors.push_back(neighborNode);
//Reset values for the next check
neighborXCoord = currXCoord;
neighborZCoord = currZCoord;
neighborZCoord += 1; //Check cell at (x-1, z+1)
neighborXCoord -= 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
else //Not a traversible node
{
//Reset values for the next check
neighborXCoord = currXCoord;
neighborZCoord = currZCoord;
neighborZCoord += 1; //Check cell at (x-1, z+1)
neighborXCoord -= 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
gSpatialDatabase->getLocationFromIndex(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord), neighborPos); //(x-1, z+1)
if (travCost >= 0 && travCost < COLLISION_COST && canBeTraversed(gSpatialDatabase->getCellIndexFromLocation(neighborPos)))
{
//Add neighbor to list
neighborNode.position = neighborPos;
neighborNode.isDiag = true;
neighborNode.gCost = travCost + currNode.gCost;
neighborNode.hCost = computeHCost(HEURISTIC, neighborNode, goal);
neighborNode.fCost = neighborNode.gCost + neighborNode.hCost;
//neighborNode.id = currNode.id + 6;
neighborNode.id = gSpatialDatabase->getCellIndexFromLocation(neighborNode.position);
neighbors.push_back(neighborNode);
//Reset values for the next check
neighborXCoord = currXCoord;
neighborZCoord = currZCoord;
neighborZCoord -= 1; //Check cell at (x+1, z-1)
neighborXCoord += 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
else //Not a traversible node
{
//Reset values for the next check
neighborXCoord = currXCoord;
neighborZCoord = currZCoord;
neighborZCoord -= 1; //Check cell at (x+1, z-1)
neighborXCoord += 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
gSpatialDatabase->getLocationFromIndex(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord), neighborPos); //(x+1, z-1)
if (travCost >= 0 && travCost < COLLISION_COST && canBeTraversed(gSpatialDatabase->getCellIndexFromLocation(neighborPos)))
{
//Add neighbor to list
neighborNode.position = neighborPos;
neighborNode.isDiag = true;
neighborNode.gCost = travCost + currNode.gCost;
neighborNode.hCost = computeHCost(HEURISTIC, neighborNode, goal);
neighborNode.fCost = neighborNode.gCost + neighborNode.hCost;
//neighborNode.id = currNode.id + 7;
neighborNode.id = gSpatialDatabase->getCellIndexFromLocation(neighborNode.position);
neighbors.push_back(neighborNode);
//Reset values for the next check
neighborXCoord = currXCoord;
neighborZCoord = currZCoord;
neighborZCoord += 1; //Check cell at (x+1, z+1)
neighborXCoord += 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
else //Not a traversible node
{
//Reset values for the next check
neighborXCoord = currXCoord;
neighborZCoord = currZCoord;
neighborZCoord += 1; //Check cell at (x+1, z+1)
neighborXCoord += 1;
travCost = gSpatialDatabase->getTraversalCost(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord));
}
gSpatialDatabase->getLocationFromIndex(gSpatialDatabase->getCellIndexFromGridCoords(neighborXCoord, neighborZCoord), neighborPos); //(x+1, z+1)
if (travCost >= 0 && travCost < COLLISION_COST && canBeTraversed(gSpatialDatabase->getCellIndexFromLocation(neighborPos)))
{
//Add neighbor to list
neighborNode.position = neighborPos;
neighborNode.isDiag = true;
neighborNode.gCost = travCost + currNode.gCost;
neighborNode.hCost = computeHCost(HEURISTIC, neighborNode, goal);
neighborNode.fCost = neighborNode.gCost + neighborNode.hCost;
//neighborNode.id = currNode.id + 8;
neighborNode.id = gSpatialDatabase->getCellIndexFromLocation(neighborNode.position);
neighbors.push_back(neighborNode);
}
//Loop through neighbors and see if they are usable nodes
for (int i = 0; i < neighbors.size(); i++)
{
int newCost = 0;
if (neighbors[i].isDiag == false)
{
newCost = currNode.gCost + 1;
}
else
{
newCost = currNode.gCost + DIAG_COST;
}
bool nodeVisited = false;
for (int q = 0; q < visitedNodes.size()-1; q++)
{
if (visitedNodes[q].position.operator==(neighbors[i].position))
{
nodeVisited = true;
}
}
//Check if neighbor was previously visited
if (nodeVisited == true)
{
continue;
}
else //Neighbor was not visited before
{
if (newCost <= neighbors[i].gCost)
{
//Add node to agent_path
neighbors[i].gCost = newCost;
neighbors[i].hCost = computeHCost(HEURISTIC, neighbors[i], goal);
neighbors[i].fCost = neighbors[i].gCost + neighbors[i].hCost;
parents[neighbors[i].id] = currNode.id; //Set parent node
openNodes.push_back(neighbors[i]);
visitedNodes.push_back(neighbors[i]);
}
}
}
neighbors.clear(); //Clear list of neighbors for the next node
}
int pathIndex = parents[goalNode.id];
std::vector<Util::Point> finalPath;
finalPath.push_back(goalNode.position);
int numNodes = 0;
while (pathIndex != startNode.id || pathIndex != 0)
{
for (int i = 0; i < visitedNodes.size()-1; i++)
{
if (visitedNodes[i].id == pathIndex && pathIndex != -1)
{
numNodes++;
finalPath.push_back(visitedNodes[i].position);
pathIndex = parents[visitedNodes[i].id];
break;
}
}
//std::cout << "\n path: " << pathIndex;
if (pathIndex == -1 || pathIndex == 0)
{
break;
}
}
finalPath.push_back(startNode.position);
while (numNodes > 0)
{
if (numNodes - 1 >= 0)
{
agent_path.push_back(finalPath[numNodes - 1]);
}
numNodes--;
}
std::cout << "\nNumber of expanded nodes: " << visitedNodes.size();
std::cout << "\nLength of solution path: " << agent_path.size() << "\n";
return true;
}
bool AStarPlanner::computePath(std::vector<Util::Point>& agent_path, Util::Point start, Util::Point goal, SteerLib::GridDatabase2D * _gSpatialDatabase, bool append_to_path)
{
std::cout << goal << std::endl;
gSpatialDatabase = _gSpatialDatabase;
//Build node trees/sets
priorityQueue openNodes = priorityQueue();
std::vector<SteerLib::AStarPlannerNode> closedNodes;
double max = 10000;
//Build start node
SteerLib::AStarPlannerNode startNode = SteerLib::AStarPlannerNode(start, 0, max-(0+HEURISTICWEIGHT*calcHeuristic(start, goal, false)), -1);
startNode.gridIndex = gSpatialDatabase->getCellIndexFromLocation(start);
openNodes.insert(startNode);
int expandedCount = 0;
while(!openNodes.heap.empty()) {
SteerLib::AStarPlannerNode currNode = openNodes.pop();
expandedCount++;
closedNodes.push_back(currNode);
//Found goal, trace back for path
if(currNode.gridIndex == gSpatialDatabase->getCellIndexFromLocation(goal)) {
std::cout << std::endl << "PATH LENGTH: " << currNode.g << std::endl << "NODES EXPANDED: " << expandedCount << std::endl;
currNode.point = goal;
int currIndex = currNode.gridIndex;
while(currIndex != gSpatialDatabase->getCellIndexFromLocation(start)) {
Util::Point currPoint;
gSpatialDatabase->getLocationFromIndex(currIndex, currPoint);
agent_path.insert(agent_path.begin(), currPoint);
currIndex = closedNodes[searchClosed(closedNodes, currIndex, 0, 0, gSpatialDatabase)].parentIndex;
}
agent_path.insert(agent_path.begin(), start);
return true;
}
//TODO
//Update neighbors
unsigned int xIndex, zIndex;
gSpatialDatabase->getGridCoordinatesFromIndex(currNode.gridIndex, xIndex, zIndex);
//aligned neighbors
//check if neighbor is valid (out of bounds of grid)
//left
if(xIndex-1 >= 0 && canBeTraversed(gSpatialDatabase->getCellIndexFromGridCoords(xIndex-1, zIndex))) {
//Is in closed set?
if(searchClosed(closedNodes, currNode.gridIndex, -1, 0, gSpatialDatabase)==-1) {
//Is in open set?
openNodes.update(currNode, -1, 0, gSpatialDatabase, goal);
}
}
//right
if(xIndex+1 < gSpatialDatabase->getNumCellsX() && canBeTraversed(gSpatialDatabase->getCellIndexFromGridCoords(xIndex+1, zIndex))) {
if(searchClosed(closedNodes, currNode.gridIndex, 1, 0, gSpatialDatabase)==-1) {
openNodes.update(currNode, 1, 0, gSpatialDatabase, goal);
}
}
//top
if(zIndex-1 >= 0 && canBeTraversed(gSpatialDatabase->getCellIndexFromGridCoords(xIndex, zIndex-1))) {
if(searchClosed(closedNodes, currNode.gridIndex, 0, -1, gSpatialDatabase)==-1) {
openNodes.update(currNode, 0, -1, gSpatialDatabase, goal);
}
}
//bottom
if(zIndex+1 < gSpatialDatabase->getNumCellsZ() && canBeTraversed(gSpatialDatabase->getCellIndexFromGridCoords(xIndex, zIndex+1))) {
if(searchClosed(closedNodes, currNode.gridIndex, 0, 1, gSpatialDatabase)==-1) {
openNodes.update(currNode, 0, 1, gSpatialDatabase, goal);
}
}
//diagonal neighbors
//top-left
if(zIndex-1>=0 && xIndex-1>=0 && canBeTraversed(gSpatialDatabase->getCellIndexFromGridCoords(xIndex-1, zIndex-1))) {
if(searchClosed(closedNodes, currNode.gridIndex, -1, -1, gSpatialDatabase)==-1) {
openNodes.update(currNode, -1, -1, gSpatialDatabase, goal);
}
}
//top-right
if(zIndex+-1>=0 && xIndex+1<gSpatialDatabase->getNumCellsX() && canBeTraversed(gSpatialDatabase->getCellIndexFromGridCoords(xIndex+1, zIndex-1))) {
if(searchClosed(closedNodes, currNode.gridIndex, 1, -1, gSpatialDatabase)==-1) {
openNodes.update(currNode, 1, -1, gSpatialDatabase, goal);
}
}
//bottom-left
if(zIndex+1<gSpatialDatabase->getNumCellsZ() && xIndex-1>=0 && canBeTraversed(gSpatialDatabase->getCellIndexFromGridCoords(xIndex-1, zIndex+1))) {
if(searchClosed(closedNodes, currNode.gridIndex, -1, 1, gSpatialDatabase)==-1) {
openNodes.update(currNode, -1, 1, gSpatialDatabase, goal);
}
}
//bottom-right
if(zIndex+1<gSpatialDatabase->getNumCellsZ() && xIndex+1<gSpatialDatabase->getNumCellsX() && canBeTraversed(gSpatialDatabase->getCellIndexFromGridCoords(xIndex+1, zIndex+1))) {
if(searchClosed(closedNodes, currNode.gridIndex, 1, 1, gSpatialDatabase)==-1) {
openNodes.update(currNode, 1, 1, gSpatialDatabase, goal);
}
}
}
//TODO
std::cout<<"\nIn A*";
return false;
}
