void DynamicEDTOctomap::insertMaxDepthLeafAtInitialize(octomap::OcTreeKey key){
bool isSurrounded = true;
for(int dx=-1; dx<=1; dx++)
for(int dy=-1; dy<=1; dy++)
for(int dz=-1; dz<=1; dz++){
if(dx==0 && dy==0 && dz==0)
continue;
octomap::OcTreeNode* node = octree->search(octomap::OcTreeKey(key[0]+dx, key[1]+dy, key[2]+dz));
if((!unknownOccupied && node==NULL) || ((node!=NULL) && (octree->isNodeOccupied(node)==false))){
isSurrounded = false;
break;
}
}
if(isSurrounded){
//obstacles that are surrounded by obstacles do not need to be put in the queues,
//hence this initialization
dataCell c;
int x = key[0]+offsetX;
int y = key[1]+offsetY;
int z = key[2]+offsetZ;
c.obstX = x;
c.obstY = y;
c.obstZ = z;
c.sqdist = 0;
c.dist = 0.0;
c.queueing = fwProcessed;
c.needsRaise = false;
data[x][y][z] = c;
} else {
setObstacle(key[0]+offsetX, key[1]+offsetY, key[2]+offsetZ);
}
}
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 PhysicalObstacle::_update()
{
OgreNewt::Body *body = mPhysicalThing->_getBody();
RlAssert(body, "PhysicalThing has no body yet!");
Vector3 position;
Quaternion orientation;
body->getPositionOrientation(position, orientation);
const OgreNewt::Collision* collision = body->getCollision();
RlAssert(collision, "Body has no collision!");
AxisAlignedBox box = collision->getAABB();
Ogre::Vector3 dims = box.getMaximum() - box.getMinimum();
OpenSteer::BoxObstacle *obstacle = new OpenSteer::BoxObstacle(dims[0], dims[1], dims[2]);
obstacle->setForward(0,0,-1);
obstacle->setPosition(position[0], position[1], position[2]);
//obstacle->setOrientation(orient[0], orient[1], orient[2]);
setObstacle(obstacle);
}
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);
}
}
}
}
}
void DynamicVoronoi::exchangeObstacles(std::vector<INTPOINT> &points) {
for (unsigned int i=0; i<lastObstacles.size(); i++) {
int x = lastObstacles[i].x;
int y = lastObstacles[i].y;
bool v = gridMap[x][y];
if (v) continue;
removeObstacle(x,y);
}
lastObstacles.clear();
for (unsigned int i=0; i<points.size(); i++) {
int x = points[i].x;
int y = points[i].y;
bool v = gridMap[x][y];
if (v) continue;
setObstacle(x,y);
lastObstacles.push_back(points[i]);
}
}
void DynamicEDTOctomap::updateMaxDepthLeaf(octomap::OcTreeKey& key, bool occupied){
if(occupied)
setObstacle(key[0]+offsetX, key[1]+offsetY, key[2]+offsetZ);
else
removeObstacle(key[0]+offsetX, key[1]+offsetY, key[2]+offsetZ);
}
void DynamicEDT3D::occupyCell(int x, int y, int z) {
gridMap[x][y][z] = 1;
setObstacle(x,y,z);
}
void DynamicVoronoi::occupyCell(int x, int y) {
gridMap[x][y] = 1;
setObstacle(x,y);
}
void MutableMap::toggle(unsigned _x, unsigned _y) {
setObstacle(_x,_y,isObstacle(_x,_y)?false:true);
}
