std::list<Node*> Dlite::GetSurroundings(Node *current, LocalMap &map) {
std::list<Node> result1;
int x = current->point.x;
int y = current->point.y;
if(opt.allowdiagonal) {
for (int k = y - 1; k <= y + 1; ++k) {
for (int l = x - 1; l <= x + 1; ++l) {
if (!(k == y && l == x) && map.CellOnGrid(Cell(l, k)) && map.CellIsTraversable(Cell(l, k))) {
result1.push_front(Node(Cell(l, k)));
}
}
}
} else {
for (int k = x - 1; k <= x + 1; ++k)
if (k != x && map.CellOnGrid(Cell(k, y)) && map.CellIsTraversable(Cell(k, y)))
result1.push_front(Node(Cell(k, y)));
for (int l = y - 1; l <= y + 1; ++l)
if (l != y && map.CellOnGrid(Cell(x, l)) && map.CellIsTraversable(Cell(x, l)))
result1.push_front(Node(Cell(x, l)));
}
std::list<Node*> result;
for(auto elem : result1) {
if(!NODES.count(vertex(elem.point, map.height))) { //if vertex wasn't previously examined
continue;
} else {
result.push_back(&(NODES.find(vertex(elem.point, map.height))->second));
}
}
return result;
}
std::list<Node> Dlite::FindNeighbors(Node* n, LocalMap &map) const {
Node newNode;
Cell curNode = n->point;
std::list<Node> successors;
for (int i = -1; i <= +1; i++)
for (int j = -1; j <= +1; j++)
if ((i != 0 || j != 0) && map.CellOnGrid(Cell(curNode.x + j, curNode.y + i)) &&
(map.CellIsTraversable(Cell(curNode.x + j, curNode.y + i)))) {
if (i != 0 && j != 0) {
if (!opt.allowdiagonal)
continue;
else if (!opt.cutcorners) {
if (map.CellIsObstacle(Cell(curNode.x + j, curNode.y)) ||
map.CellIsObstacle(Cell(curNode.x, curNode.y + i)))
continue;
}
else if (!opt.allowsqueeze) {
if (map.CellIsObstacle(Cell( curNode.x + j, curNode.y)) &&
map.CellIsObstacle(Cell( curNode.x, curNode.y + i)))
continue;
}
}
newNode = Node(Cell(curNode.x + j, curNode.y + i), n);
successors.push_front(newNode);
}
return successors;
}