void
CustomBuilding::draw(View& view)
{
view.get_sc().color().draw(sprite,
x_pos * 40 + get_map_width()*20,
y_pos * 40 + get_map_height()*20); // FIXME: Hardcoded tilesize
}
void map_random_stone(void)
{
static size_t i = 0;
set_box_addstone(rand() % get_map_width(),
rand() % get_map_height(),
stonetab[i++],
rand() % map_nb_stones);
i = (i < (sizeof(stonetab) / sizeof(*stonetab)) ? i : 0);
}
// -------------------------------------------------------------------------------------------------
void SensorMap::update(SensorModel & model, float data) {
// -------------------------------------------------------------------------------------------------
//std::cout << "Update sensor" << std::endl;
model.insert(data);
std::vector<Vector2> passed = model.getPassed(get_cell_size());
std::vector<Vector2> blocked = model.getBlocked(get_cell_size());
for (std::vector<Vector2>::iterator el = passed.begin(); el != passed.end(); ++el) {
size_t row = get_map_center_row() - el->get_y();
size_t col = get_map_center_col() + el->get_x();
if (row >= 0 && row < get_map_height() && col >= 0 && col < get_map_width()) map_array_[row][col]->update(1);
}
for (std::vector<Vector2>::iterator el = blocked.begin(); el != blocked.end(); ++el) {
size_t row = get_map_center_row() - el->get_y();
size_t col = get_map_center_col() + el->get_x();
if (row >= 0 && row < get_map_height() && col >= 0 && col < get_map_width()) map_array_[row][col]->update(0);
}
}
// -------------------------------------------------------------------------------------------------
void SensorMap::draw(cv::Mat & dst) const {
// -------------------------------------------------------------------------------------------------
// Iterate each pixel and add the occupancy value in relation to the scale from 0 to 255
for (size_t row = 0; row < get_map_height(); row++) {
for (size_t col = 0; col < get_map_width(); col++) {
dst.at<cv::Vec3b>(row, col) = cv::Vec3b(255, 255, 255);
dst.at<cv::Vec3b>(row, col) *= (map_array_[row][col]->occupancy());
}
}
}
// -------------------------------------------------------------------------------------------------
void Map::transform(Vector2** transformation_matrix) {
// -------------------------------------------------------------------------------------------------
Cell*** new_map_array = new Cell**[height_];
for (size_t row = 0; row < height_; row++) {
new_map_array[row] = new Cell*[width_];
for (size_t col = 0; col < width_; col++) {
new_map_array[row][col] = new Cell(HISTORY_SIZE, DISCOUNT_FACTOR);
}
}
for (size_t row = 0; row < height_; row++) {
for (size_t col = 0; col < width_; col++) {
int transformed_x = transformation_matrix[row][col].get_x();
int transformed_y = transformation_matrix[row][col].get_y();
if (transformed_x != Vector2::ERROR_VALUE && transformed_y != Vector2::ERROR_VALUE) {
Cell* tmp = new_map_array[transformed_y][transformed_x];
new_map_array[transformed_y][transformed_x] = map_array_[row][col];
map_array_[row][col] = tmp;
}
}
}
float updated_occupancies[height_][width_];
for (size_t row = 0; row < height_; row++) {
for (size_t col = 0; col < width_; col++) {
if (new_map_array[row][col]->occupancy() != INITIAL_OCCUPANCY) continue;
double updated_occupancy = 0.0;
int blur_counter = 0;
for (int x_offset = -1; x_offset <= 1; x_offset++) {
for (int y_offset = -1; y_offset <= 1; y_offset++) {
if (x_offset == 0 && y_offset == 0) continue;
int new_row = row + y_offset;
int new_col = col + x_offset;
if (new_row < 0) new_row = -new_row;
if (new_col < 0) new_col = -new_col;
if (new_row >= (int)get_map_height()) new_row = new_row - (int)get_map_height();
if (new_col >= (int)get_map_width()) new_col = new_col - (int)get_map_width();
updated_occupancy += new_map_array[new_row][new_col]->occupancy();
blur_counter++;
}
}
updated_occupancy /= blur_counter;
updated_occupancies[row][col] = updated_occupancy;
}
}
for (size_t row = 0; row < height_; row++) {
for (size_t col = 0; col < width_; col++) {
if (new_map_array[row][col]->occupancy() != 0.5) continue;
for (size_t i = 0; i < new_map_array[row][col]->history_size(); i++) new_map_array[row][col]->update(updated_occupancies[row][col]);
}
}
Cell*** tmp = map_array_;
map_array_ = new_map_array;
for (size_t row = 0; row < height_; row++) {
for (size_t col = 0; col < width_; col++) {
delete tmp[row][col];
}
delete[] tmp[row];
}
delete[] tmp;
}
