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; }