Skip to content

Commit

Permalink
Add setter and is_occupied
Browse files Browse the repository at this point in the history
  • Loading branch information
@griswaldbrooks
griswaldbrooks committed Sep 6, 2024
1 parent fb7313f commit 36a5cad
Showing 1 changed file with 87 additions and 87 deletions.
174 changes: 87 additions & 87 deletions src/map.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -230,43 +230,38 @@ struct bounds_checked_layout_policy {
};
};

// std::size_t coord_to_offset(coordinate const& coord) {
// int const row = offset / width;
// int const col = offset % width;
// return {row, col};
// }

template <std::size_t Height, std::size_t Width>
struct layout_rotatable {
template <typename Extents>
requires(Extents::rank() == 2) struct mapping {
using extents_type = Extents;
using size_type = typename extents_type::size_type;

mapping(window const& size, coordinate const& translation, double theta)
constexpr mapping(window const& size, coordinate const& translation,
double theta)
: size_{size},
translation_{translation},
theta_{theta},
extents_{size.height, size.width} {}

constexpr const extents_type& extents() const noexcept { return extents_; }

size_type operator()(size_type x, size_type y) const noexcept {
constexpr size_type operator()(size_type x, size_type y) const noexcept {
// Use the three skew algorithm to rotate the elements
auto tx = static_cast<double>(x);
auto ty = static_cast<double>(y);
// because a normal transformation matrix produces holes and off elements
double const alpha = -std::tan(theta_ / 2.);
double const beta = std::sin(theta_);
auto tx = static_cast<double>(x);
auto ty = static_cast<double>(y);
tx += std::round(alpha * ty);
ty += std::round(beta * tx);
tx += std::round(alpha * ty);
x = static_cast<size_type>(std::round(tx)) + translation_.x;
y = static_cast<size_type>(std::round(ty)) + translation_.y;
auto const offset = x + y * Width;
return offset;
return x + y * Width;
}

size_type required_span_size() const noexcept {
constexpr size_type required_span_size() const noexcept {
return size_.height * size_.width;
}

Expand All @@ -285,15 +280,16 @@ struct layout_rotatable {
extents_type extents_;
};
};

template <std::size_t Height, std::size_t Width>
struct layout_polygonal {
template <typename Extents>
requires(Extents::rank() == 1) struct mapping {
using extents_type = Extents;
using size_type = typename extents_type::size_type;

mapping(std::vector<geometry::Cell> vertices, geometry::Cell const& p,
double theta)
constexpr mapping(std::vector<geometry::Cell> vertices,
geometry::Cell const& p, double theta)
: polygon_{[&] {
for (auto& v : vertices) {
auto x = static_cast<int>(v.x * std::cos(theta) -
Expand All @@ -310,7 +306,7 @@ struct layout_polygonal {

constexpr const extents_type& extents() const noexcept { return extents_; }

size_type operator()(size_type i) const noexcept {
constexpr size_type operator()(size_type i) const noexcept {
auto const& coord = polygon_[i];
auto const x = static_cast<size_type>(coord.x);
auto const y = static_cast<size_type>(coord.y);
Expand All @@ -319,7 +315,9 @@ struct layout_polygonal {
return offset;
}

size_type required_span_size() const noexcept { return polygon_.size(); }
constexpr size_type required_span_size() const noexcept {
return polygon_.size();
}

static constexpr bool is_always_unique() noexcept { return false; }
static constexpr bool is_always_exhaustive() noexcept { return true; }
Expand All @@ -340,7 +338,7 @@ using subgrid = std::mdspan<int, std::dextents<size_t, 2>, std::layout_stride>;
template <std::size_t Height, std::size_t Width>
struct occupancy_grid {
explicit occupancy_grid(int value = 0)
: data_{}, grid_(data_.data(), Height, Width) {
: data_{}, grid_{data_.data(), Height, Width} {
std::fill(data_.begin(), data_.end(), value);
}

Expand Down Expand Up @@ -414,8 +412,8 @@ struct occupancy_grid {
std::dextents<std::size_t, 2>>
layout_window{size, corner, theta};
return std::mdspan<int, std::dextents<std::size_t, 2>,
layout_rotatable<Height, Width>>(data_.begin(),
layout_window);
layout_rotatable<Height, Width>>{data_.begin(),
layout_window};
}

auto window_polygonal(std::vector<geometry::Cell> const& vertices,
Expand All @@ -424,8 +422,8 @@ struct occupancy_grid {
std::dextents<std::size_t, 1>>
footprint{vertices, p, theta};
return std::mdspan<int, std::dextents<std::size_t, 1>,
layout_polygonal<Height, Width>>(data_.begin(),
footprint);
layout_polygonal<Height, Width>>{data_.begin(),
footprint};
}

private:
Expand All @@ -444,65 +442,95 @@ struct occupancy_grid {
}
return os;
}

std::array<int, Height * Width> data_;
std::mdspan<int, std::extents<std::size_t, Height, Width>> grid_;
};

template <typename Container>
requires(Container::extents_type::rank() ==
1) bool is_occupied(Container const& footprint) {
for (auto i = 0u; i != footprint.extent(0); i++) {
if (footprint(i) != 0) {
return true;
}
}
return false;
}

template <typename Container>
requires(Container::extents_type::rank() ==
2) bool is_occupied(Container const& footprint) {
for (auto i = 0u; i != footprint.extent(0); i++) {
for (auto j = 0u; j != footprint.extent(1); j++) {
if (footprint(i, j) != 0) {
return true;
}
}
}
return false;
}

template <typename Container>
requires(Container::extents_type::rank() ==
1) void set(Container& footprint,
typename Container::value_type const& value) {
for (auto i = 0u; i != footprint.extent(0); i++) {
footprint(i) = value;
}
}

template <typename Container>
requires(Container::extents_type::rank() ==
2) void set(Container& footprint,
typename Container::value_type const& value) {
for (auto i = 0u; i != footprint.extent(0); i++) {
for (auto j = 0u; j != footprint.extent(1); j++) {
footprint(i, j) = value;
}
}
}

int main(int /* argc */, char** /* argv[] */) {
// height, width
auto map = occupancy_grid<20, 30>{};

// This one doesn't work. Show this as the negative example (both layout_right
// and layout_left)
auto window_bad = map.window_from_layout_right({5, 4}, {10, 15});
for (auto i = 0u; i != window_bad.extent(0); i++) {
for (auto j = 0u; j != window_bad.extent(1); j++) {
window_bad(i, j) = 2;
}
}
set(window_bad, 1);

// This works and uses layout_stride.
// Show how this can also do multistride
// Note the slide from Bryce Lelbach and the data[i * M + j] vs data[i * X + j
// * Y] This doesn't feel quite right and need to explain why layout_left !=
// layout_stride for this simple case
auto window = map.window_from_layout_stride({4, 4}, {5, 10});
for (auto i = 0u; i != window.extent(0); i++) {
for (auto j = 0u; j != window.extent(1); j++) {
// window(i, j) = 1;
}
}
auto window = map.window_from_layout_stride({4, 4}, {20, 1});
set(window, 2);
// But for the local costmap example, using submdspan is even easier and
// simpler to grok not needing to mess with pointer offsets
auto local_map = map.window_submdspan({4, 4}, {10, 5});
std::cout << "local_map.extent = " << local_map.extent(0) << ", "
<< local_map.extent(1) << "\n";
for (auto i = 0u; i != local_map.extent(0); i++) {
for (auto j = 0u; j != local_map.extent(1); j++) {
// local_map(i, j) = 3;
}
}
set(local_map, 3);

// Character to display that changes every loop
int c = 1;
for (auto angle : {0, 12, 22, 30, 45, 58, 67, 79, 90}) {
// create footprint
// rotates around upper left corner
std::vector<geometry::Cell> vertices = {{0, 0}, {3, 0}, {3, 3}, {0, 3}};
// rotates somewhere around center
// std::vector<geometry::Cell> vertices = {{-2,-2},{3,-2},{3,3},{-2,3}};
// create footprint
// rotates around upper left corner
std::vector<geometry::Cell> const vertices = {{0, 0}, {3, 0}, {3, 3}, {0, 3}};
// rotates somewhere around center
// std::vector<geometry::Cell> vertices = {{-2,-2},{3,-2},{3,3},{-2,3}};
geometry::Cell const p = {3, 3};
for (auto const& angle : {0, 12, 22, 30, 45, 58, 67, 79, 90}) {
double const theta = geometry::degrees_to_radians(angle);
geometry::Cell p = {3, 3};

auto footprint = map.window_polygonal(vertices, p, theta);
// // set footprint
for (auto i = 0u; i != footprint.extent(0); i++) {
footprint(i) = c;
}
std::cout << "footprint is_occupied = " << is_occupied(footprint) << "\n";
set(footprint, c);
std::cout << "footprint is_occupied = " << is_occupied(footprint) << "\n";

auto rotatable = map.window_rotatable({4, 4}, {10, 10}, theta);
// // set footprint
for (auto i = 0u; i != rotatable.extent(0); i++) {
for (auto j = 0u; j != rotatable.extent(1); j++) {
rotatable(i, j) = c;
}
}
set(rotatable, c);

++c;
// What's interesting is that if you try to use
// std::fill(window.data_handle(), window.data_handle() + offset, 1);
Expand All @@ -515,37 +543,9 @@ int main(int /* argc */, char** /* argv[] */) {
// print map
std::cout << map << "\n";
// Clear footprint
for (auto i = 0u; i != footprint.extent(0); i++) {
footprint(i) = 0;
}
// Clear footprint
for (auto i = 0u; i != rotatable.extent(0); i++) {
for (auto j = 0u; j != rotatable.extent(1); j++) {
rotatable(i, j) = 0;
}
}
std::cin.get();
}

int d = 1;
for (auto angle : {0, 12, 22, 30, 45, 58, 67, 79, 90}) {
double const theta = geometry::degrees_to_radians(angle);
auto rotatable = map.window_rotatable({4, 4}, {5, 3}, theta);
// // set footprint
for (auto i = 0u; i != rotatable.extent(0); i++) {
for (auto j = 0u; j != rotatable.extent(1); j++) {
rotatable(i, j) = d;
}
}
++d;
// print map
std::cout << map << "\n";
set(footprint, 0);
// Clear footprint
for (auto i = 0u; i != rotatable.extent(0); i++) {
for (auto j = 0u; j != rotatable.extent(1); j++) {
rotatable(i, j) = 0;
}
}
set(rotatable, 0);
std::cin.get();
}

Expand Down

0 comments on commit 36a5cad

Please sign in to comment.