Map CreateMap(ConfigurationManager config)
{
std::vector<unsigned char> image;
unsigned width, height;
unsigned int x, y;
const char* path = config.getMapPath();
unsigned error = lodepng::decode(image, width, height, path);
if (error)
{
std::cout << "decoder error " << error << ": "
<< lodepng_error_text(error) << std::endl;
}
// Get parameters
double gridResoultion = config.getGridResolutionCM();
double mapResoultion = config.getMapResolutionCM();
int divider = ceil(gridResoultion/mapResoultion); // Numbers of pixels for 1 cell in matrix
// Create map as png pixels
char** map = initializeMatrix(height + (divider - 1) * 2,width + (divider - 1) * 2, FREE_CELL);
unsigned char color;
for (y = 0; y < height; y++)
{
for (x = 0; x < width; x++)
{
if (image[y * width * 4 + x * 4 + 0]
|| image[y * width * 4 + x * 4 + 1]
|| image[y * width * 4 + x * 4 + 2])
{
// There is an obstacle in the map
color = FREE_CELL;
}
else
{
// There is no obstacle
color = BLOCK_CELL;
}
map[y + divider - 1][x + divider - 1] = color;
}
}
// Create map as config cm size
int newMapHieght = height/divider;
int newMapWidth = width/divider;
char** mapBlow = initializeMatrix(newMapHieght, newMapWidth, FREE_CELL);
unsigned i,j;
int mapBlowY = 0, mapBlowX = 0;
bool hasObstacle = false;
for (y = divider - 1; y < height; y += divider)
{
for (x = divider - 1; x < width; x += divider)
{
for (i = 0; i < divider && !hasObstacle; i++)
{
for (j = 0; j < divider && !hasObstacle; j++)
{
if (map[y + i][x + j] == BLOCK_CELL)
{
hasObstacle = true;
}
}
}
if (hasObstacle)
{
mapBlow[mapBlowY][mapBlowX] = BLOCK_CELL;
}
hasObstacle = false;
mapBlowX++;
}
mapBlowX = 0;
mapBlowY++;
}
// ReSize obstacle
double* robotSize = config.getRobotSize();
int sizeH = ceil(((robotSize[0] / gridResoultion)- 1) / 2);
int sizeW = ceil(((robotSize[1] / gridResoultion)- 1) / 2);
char** newMap = initializeMatrix(newMapHieght + sizeH*2, newMapWidth + sizeW*2, FREE_CELL);
for (int i = 0; i < newMapHieght; i++)
{
for (int j = 0; j < newMapWidth; j++)
{
if (mapBlow[i][j] == BLOCK_CELL)
{
for (int iNewMap = i; iNewMap <= i + sizeH + 1; iNewMap++)
{
for (int jNewMap = j; jNewMap <= j + sizeW + 1; jNewMap++)
{
newMap[iNewMap][jNewMap] = BLOCK_CELL;
}
}
}
}
}
// Initialize final map with wall block of size 1.
char** finalMap = initializeMatrix(newMapHieght + 2, newMapWidth + 2, BLOCK_CELL);
for (int i=1; i < newMapHieght + 1; i++)
{
for (int j=1; j< newMapWidth + 1; j++)
{
finalMap[i][j] = newMap[sizeH + i - 1][sizeW + j -1];
}
}
printMap(map, height + 6, width + 6);
printMap(mapBlow, newMapHieght, newMapWidth);
printMap(newMap, newMapHieght + sizeH, newMapWidth + sizeW);
printMap(finalMap, newMapHieght + 2, newMapWidth + 2);
return Map(finalMap, newMapHieght + 2, newMapWidth + 2, gridResoultion / 100);
}
