void DynamicEDTOctomap::initializeOcTree(octomap::point3d bbxMin, octomap::point3d bbxMax){
boundingBoxMinKey = octree->coordToKey(bbxMin);
boundingBoxMaxKey = octree->coordToKey(bbxMax);
offsetX = -boundingBoxMinKey[0];
offsetY = -boundingBoxMinKey[1];
offsetZ = -boundingBoxMinKey[2];
int _sizeX = boundingBoxMaxKey[0] - boundingBoxMinKey[0] + 1;
int _sizeY = boundingBoxMaxKey[1] - boundingBoxMinKey[1] + 1;
int _sizeZ = boundingBoxMaxKey[2] - boundingBoxMinKey[2] + 1;
initializeEmpty(_sizeX, _sizeY, _sizeZ, false);
if(unknownOccupied == false){
for(octomap::OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(bbxMin,bbxMax), end=octree->end_leafs_bbx(); it!= end; ++it){
if(octree->isNodeOccupied(*it)){
int nodeDepth = it.getDepth();
if( nodeDepth == treeDepth){
insertMaxDepthLeafAtInitialize(it.getKey());
} else {
int cubeSize = 1 << (treeDepth - nodeDepth);
octomap::OcTreeKey key=it.getIndexKey();
for(int dx = 0; dx < cubeSize; dx++)
for(int dy = 0; dy < cubeSize; dy++)
for(int dz = 0; dz < cubeSize; dz++){
unsigned short int tmpx = key[0]+dx;
unsigned short int tmpy = key[1]+dy;
unsigned short int tmpz = key[2]+dz;
if(boundingBoxMinKey[0] > tmpx || boundingBoxMinKey[1] > tmpy || boundingBoxMinKey[2] > tmpz)
continue;
if(boundingBoxMaxKey[0] < tmpx || boundingBoxMaxKey[1] < tmpy || boundingBoxMaxKey[2] < tmpz)
continue;
insertMaxDepthLeafAtInitialize(octomap::OcTreeKey(tmpx, tmpy, tmpz));
}
}
}
}
} else {
octomap::OcTreeKey key;
for(int dx=0; dx<sizeX; dx++){
key[0] = boundingBoxMinKey[0] + dx;
for(int dy=0; dy<sizeY; dy++){
key[1] = boundingBoxMinKey[1] + dy;
for(int dz=0; dz<sizeZ; dz++){
key[2] = boundingBoxMinKey[2] + dz;
octomap::OcTreeNode* node = octree->search(key);
if(!node || octree->isNodeOccupied(node)){
insertMaxDepthLeafAtInitialize(key);
}
}
}
}
}
}
void DynamicEDT3D::initializeMap(int _sizeX, int _sizeY, int _sizeZ, bool*** _gridMap) {
gridMap = _gridMap;
initializeEmpty(_sizeX, _sizeY, _sizeZ, false);
for (int x=0; x<sizeX; x++) {
for (int y=0; y<sizeY; y++) {
for (int z=0; z<sizeZ; z++) {
if (gridMap[x][y][z]) {
dataCell c = data[x][y][z];
if (!isOccupied(x,y,z,c)) {
bool isSurrounded = true;
for (int dx=-1; dx<=1; dx++) {
int nx = x+dx;
if (nx<0 || nx>sizeX-1) continue;
for (int dy=-1; dy<=1; dy++) {
int ny = y+dy;
if (ny<0 || ny>sizeY-1) continue;
for (int dz=-1; dz<=1; dz++) {
if (dx==0 && dy==0 && dz==0) continue;
int nz = z+dz;
if (nz<0 || nz>sizeZ-1) continue;
if (!gridMap[nx][ny][nz]) {
isSurrounded = false;
break;
}
}
}
}
if (isSurrounded) {
c.obstX = x;
c.obstY = y;
c.obstZ = z;
c.sqdist = 0;
c.dist = 0;
c.queueing = fwProcessed;
data[x][y][z] = c;
} else setObstacle(x,y,z);
}
}
}
}
}
}
void DynamicVoronoi::initializeMap(int _sizeX, int _sizeY, bool** _gridMap) {
gridMap = _gridMap;
initializeEmpty(_sizeX, _sizeY, false);
for (int x=0; x<sizeX; x++) {
for (int y=0; y<sizeY; y++) {
if (gridMap[x][y]) {
dataCell c = data[x][y];
if (!isOccupied(x,y,c)) {
bool isSurrounded = true;
for (int dx=-1; dx<=1; dx++) {
int nx = x+dx;
if (nx<=0 || nx>=sizeX-1) continue;
for (int dy=-1; dy<=1; dy++) {
if (dx==0 && dy==0) continue;
int ny = y+dy;
if (ny<=0 || ny>=sizeY-1) continue;
if (!gridMap[nx][ny]) {
isSurrounded = false;
break;
}
}
}
if (isSurrounded) {
c.obstX = x;
c.obstY = y;
c.sqdist = 0;
c.dist=0;
c.voronoi=occupied;
c.queueing = fwProcessed;
data[x][y] = c;
} else setObstacle(x,y);
}
}
}
}
}
