Skip to content

Commit

Permalink
Add rotatable 2d window
Browse files Browse the repository at this point in the history
  • Loading branch information
@griswaldbrooks
griswaldbrooks committed Sep 5, 2024
1 parent 7c4f475 commit d1f48de
Showing 1 changed file with 113 additions and 15 deletions.
128 changes: 113 additions & 15 deletions src/map.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,16 +160,21 @@ namespace geometry {
return out;
}

std::vector<Cell> convexFill(const std::vector<Cell>& polygon) {
std::vector<Cell> convexFill(const std::vector<Cell>& polygon, window const& bounds) {
// we need a minimum polygon of a triangle
if (polygon.size() < 3) return polygon;

const auto outline = polygonOutline(polygon, std::numeric_limits<int>::max());
auto outline = polygonOutline(polygon, std::numeric_limits<int>::max());
// Clamp each pixel to the bounds
std::for_each(outline.begin(), outline.end(), [&](auto& pixel) {
pixel.x = std::clamp(pixel.x, 0, static_cast<int>(bounds.width));
pixel.y = std::clamp(pixel.y, 0, static_cast<int>(bounds.height));
});

std::unordered_map<int,std::pair<int,int>> extrema;

// Find all the x values and extrema
for (const auto pixel:outline) {
for (const auto pixel: outline) {
// Check if the x value for the pixel is in extrema
if (extrema.find(pixel.x) == extrema.end()) {
// if it isnt in extrema push the x in and with the y value as both the min and the max
Expand Down Expand Up @@ -230,6 +235,55 @@ struct bounds_checked_layout_policy {
// 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)
: 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 {
auto tx = static_cast<double>(x);
auto ty = static_cast<double>(y);
double const alpha = -std::tan(theta_/2.);
double const beta = std::sin(theta_);
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;
}

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

static constexpr bool is_always_unique() noexcept { return false; }
static constexpr bool is_always_exhaustive() noexcept { return true; }
static constexpr bool is_always_strided() noexcept { return false; }

constexpr bool is_unique() const noexcept { return false; }
constexpr bool is_exhaustive() const noexcept { return true; }
constexpr bool is_strided() const noexcept { return false; }

private:
window size_;
coordinate translation_;
double theta_;
extents_type extents_;
};
};
template <std::size_t Height, std::size_t Width>
struct layout_polygonal {
template <typename Extents>
requires (Extents::rank() == 1)
Expand All @@ -246,16 +300,19 @@ struct layout_polygonal {
v.y = y;
}
// create filled footprint
return geometry::convexFill(vertices);
return geometry::convexFill(vertices, {Height, Width});
}()}, extents_{polygon_.size()}{}

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

size_type operator()(size_type i) const noexcept {
auto coord = polygon_[i];
auto offset = static_cast<size_type>(coord.x * 1 + coord.y * 30);
auto const& coord = polygon_[i];
auto const x = static_cast<size_type>(coord.x);
auto const y = static_cast<size_type>(coord.y);

auto const offset = x + y * Width;
return offset;
}

Expand All @@ -279,8 +336,7 @@ struct layout_polygonal {

using subgrid = std::mdspan<int,
std::dextents<size_t, 2>,
std::layout_stride,
std::default_accessor<int>
std::layout_stride
>;

template <std::size_t Height, std::size_t Width>
Expand Down Expand Up @@ -317,7 +373,7 @@ struct occupancy_grid {
return {first, submap_layout};
}

subgrid window_from_layout_right(typename::window size, coordinate corner) {
subgrid window_from_layout_right(window size, coordinate corner) {
// Constrain the window to the grid map
corner.x = std::clamp(corner.x, std::size_t{0}, Width);
corner.y = std::clamp(corner.y, std::size_t{0}, Height);
Expand All @@ -339,7 +395,7 @@ struct occupancy_grid {
return {first, std::layout_right::mapping{shape}};
}

subgrid window_submdspan(typename::window size, coordinate corner /* transform t*/) {
subgrid window_submdspan(window size, coordinate corner /* transform t*/) {
// Constrain the window to the grid map
corner.x = std::clamp(corner.x, std::size_t{0}, Width);
corner.y = std::clamp(corner.y, std::size_t{0}, Height);
Expand All @@ -353,9 +409,14 @@ struct occupancy_grid {
return stdex::submdspan(grid_, rows, cols);
}

auto window_rotatable(window size, coordinate corner, double theta) {
typename layout_rotatable<Height, Width>::mapping<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);
}

auto window_polygonal(std::vector<geometry::Cell> const& vertices, geometry::Cell const& p, double theta) {
auto footprint = layout_polygonal::mapping<std::dextents<std::size_t, 1>>{vertices, p, theta};
return std::mdspan<int, std::dextents<std::size_t, 1>, layout_polygonal, std::default_accessor<int>>(data_.begin(), footprint);
typename layout_polygonal<Height, Width>::mapping<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);
}

private:
Expand Down Expand Up @@ -408,19 +469,28 @@ int main (int /* argc */, char** /* argv[] */) {
}
}

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},{5,0},{5,5},{0,5}};
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}};
double const theta = geometry::degrees_to_radians(angle);
geometry::Cell p = {3, 5};
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) = 5;
footprint(i) = c;
}
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;
}
}
++c;
// What's interesting is that if you try to use
// std::fill(window.data_handle(), window.data_handle() + offset, 1);
// then instead of it filling the submap, it just fills the linear range.
Expand All @@ -435,6 +505,34 @@ int main (int /* argc */, char** /* argv[] */) {
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";
// 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();
}

Expand Down

0 comments on commit d1f48de

Please sign in to comment.