GridMap GridMap::getSubmap(const Eigen::Vector2d& position, const Eigen::Array2d& length, Eigen::Array2i& indexInSubmap, bool& isSuccess)
{
// Submap the generate.
GridMap submap(types_);
submap.setClearTypes(clearTypes_);
submap.setTimestamp(timestamp_);
submap.setFrameId(frameId_);
// Get submap geometric information.
Array2i topLeftIndex;
Array2i submapBufferSize;
Eigen::Vector2d submapPosition;
Array2d submapLength;
if (!getSubmapInformation(topLeftIndex, submapBufferSize, submapPosition, submapLength, indexInSubmap, position,
length, length_, position_, resolution_, bufferSize_, bufferStartIndex_)) {
isSuccess = false;
return GridMap(types_);
}
submap.setGeometry(submapLength, resolution_, submapPosition);
submap.bufferStartIndex_.setZero(); // Because of the way we copy the data below.
// Copy data.
std::vector<Eigen::Array2i> submapIndeces;
std::vector<Eigen::Array2i> submapSizes;
if (!getBufferRegionsForSubmap(submapIndeces, submapSizes, topLeftIndex, submap.bufferSize_, bufferSize_, bufferStartIndex_)) {
cout << "Cannot access submap of this size." << endl;
isSuccess = false;
return GridMap(types_);
}
for (auto& data : data_) {
submap.data_[data.first].topLeftCorner (submapSizes[0](0), submapSizes[0](1)) = data.second.block(submapIndeces[0](0), submapIndeces[0](1), submapSizes[0](0), submapSizes[0](1));
submap.data_[data.first].topRightCorner (submapSizes[1](0), submapSizes[1](1)) = data.second.block(submapIndeces[1](0), submapIndeces[1](1), submapSizes[1](0), submapSizes[1](1));
submap.data_[data.first].bottomLeftCorner (submapSizes[2](0), submapSizes[2](1)) = data.second.block(submapIndeces[2](0), submapIndeces[2](1), submapSizes[2](0), submapSizes[2](1));
submap.data_[data.first].bottomRightCorner(submapSizes[3](0), submapSizes[3](1)) = data.second.block(submapIndeces[3](0), submapIndeces[3](1), submapSizes[3](0), submapSizes[3](1));
}
isSuccess = true;
return submap;
}
void ModelContainerView::generateHeightMap(int pMapId, int x, int y)
{
//TODO: Fix this in general.
//Here we need to load the GridMap from .map file. then generate vertex map from heigh points.
GridMap mapArray[64][64]; //TODO: make smaller array and recalculate matching gridmap.
for (int x1 = x-1; x1 <= x+1; ++x1)
for (int y1 = y-1; y1 <= y+1; ++y1) {
mapArray[x1][y1] = GridMap();
char tmp[12];
sprintf(tmp, "%03u%02u%02u.map",pMapId,x1,y1);
std::string gmap = gMapDataDir + "/" + tmp;
if (mapArray[x1][y1].loadData(gmap.c_str()))
printf("Loaded %s\n", gmap.c_str());
}
float x_min,y_min,x_max, y_max;
x_max = (32-x)*SIZE_OF_GRIDS + 50;
y_max = (32-y)*SIZE_OF_GRIDS + 50;
x_min = (32-x)*SIZE_OF_GRIDS - 533 - 50;
y_min = (32-y)*SIZE_OF_GRIDS - 533 - 50;
for (float x = x_min; x < x_max-2;x += 2)
for (float y = y_min; y < y_max-2;y += 2) {
int gx,gy;
// Here we need to add vertexes. so 3 Vector3 for each triangle.
// FIXME: This is overly efficient since we visit each vector3 multipletimes during loop.
gx = (int)(32 - x / SIZE_OF_GRIDS);
gy = (int)(32 - y / SIZE_OF_GRIDS);
float heightxy = mapArray[gx][gy].getHeight(x,y);
gx = (int)(32 - (x+2) / SIZE_OF_GRIDS);
float heightx1y = mapArray[gx][gy].getHeight(x+2,y);
gx = (int)(32 - x / SIZE_OF_GRIDS);
gy = (int)(32 - (y+2) / SIZE_OF_GRIDS);
float heightxy1 = mapArray[gx][gy].getHeight(x,y+2);
gx = (int)(32 - (x+2) / SIZE_OF_GRIDS);
float heightx1y1 = mapArray[gx][gy].getHeight(x+2,y+2);
Vector3 vector1(x,y,heightxy);
Vector3 vector2(x+2,y,heightx1y);
Vector3 vector3(x,y+2,heightxy1);
Vector3 vector4(x+2,y+2,heightx1y1);
/*
* vector1 ------ vector2
* | \ / |
* | \ / |
* | \ / |
* | X |
* | / \ |
* | / \ |
* | / \ |
* vector3 -------- vector4
*/
Triangle t1 = Triangle(vector1,vector4,vector3);
Triangle t2 = Triangle(vector1,vector2,vector4);
// Check if the center of this Triangle is deep under water. (here: 1 meter)
Vector3 center = t1.center();
if (mapArray[(int)(32 - center.x / SIZE_OF_GRIDS)][(int)(32 - center.y / SIZE_OF_GRIDS)].getLiquidLevel(center.x,center.y) < center.z - 1.f)
globalTriangleArray.append(t1);
center = t2.center();
if (mapArray[(int)(32 - center.x / SIZE_OF_GRIDS)][(int)(32 - center.y / SIZE_OF_GRIDS)].getLiquidLevel(center.x,center.y) < center.z - 1.f)
globalTriangleArray.append(t2);
}
}