Program Listing for File Gridder.h
↰ Return to documentation for file (include/apriltag_mit/AprilTags/Gridder.h
)
#ifndef APRILTAGS_GRIDDER_H_
#define APRILTAGS_GRIDDER_H_
#include <algorithm>
#include <iterator>
#include <vector>
#include "apriltag_mit/AprilTags/Segment.h"
namespace AprilTags {
template <class T>
class Gridder {
private:
Gridder(const Gridder&);
Gridder& operator=(const Gridder&);
struct Cell {
T* object;
Cell* next;
Cell() : object(nullptr), next(nullptr) {}
Cell(const Cell& c) : object(c.object), next(c.next) {}
// Destructor
~Cell() { delete next; }
Cell& operator=(const Cell& other) {
if (this == &other) return *this;
object = other.object;
next = other.next;
return *this;
}
};
float pixels_per_cell_;
int width_, height_;
std::vector<std::vector<Cell*>> cells_;
public:
Gridder(float pixel_width, float pixel_height, float pixels_per_cell)
: pixels_per_cell_(pixels_per_cell) {
width_ = pixel_width / pixels_per_cell + 1;
height_ = pixel_height / pixels_per_cell + 1;
cells_ = std::vector<std::vector<Cell*>>(
height_, std::vector<Cell*>(width_, (Cell*)nullptr));
}
// Destructor
~Gridder() {
for (unsigned int i = 0; i < cells_.size(); i++) {
for (unsigned int j = 0; j < cells_[i].size(); j++) {
delete cells_[i][j];
}
}
}
void Add(float x, float y, T* object) {
int xc = x / pixels_per_cell_;
int yc = y / pixels_per_cell_;
if (xc >= 0 && yc >= 0 && xc < width_ && yc < height_) {
Cell* c = new Cell;
c->object = object;
c->next = cells_[yc][xc];
cells_[yc][xc] = c;
}
}
// iterator begin();
// iterator end();
class Iterator {
public:
Iterator(Gridder* grid, float x, float y, float range)
: outer(grid), xc0(), xc1(), yc0(), yc1(), xc(), yc(), c(nullptr) {
Init(x, y, range);
}
void Init(float x, float y, float range) {
const auto ppc = outer->pixels_per_cell_;
const auto w = outer->width_;
const auto h = outer->height_;
xc0 = (x - range) / ppc;
yc0 = (y - range) / ppc;
xc1 = (x + range) / ppc;
yc1 = (y + range) / ppc;
xc0 = std::max(0, xc0);
xc0 = std::min(w - 1, xc0);
xc1 = std::max(0, xc1);
xc1 = std::min(w - 1, xc1);
yc0 = std::max(0, yc0);
yc0 = std::min(h - 1, yc0);
yc1 = std::max(0, yc1);
yc1 = std::min(h - 1, yc1);
xc = xc0;
yc = yc0;
c = outer->cells_[yc][xc];
}
Iterator(const Iterator& it)
: outer(it.outer),
xc0(it.xc0),
xc1(it.xc1),
yc0(it.yc0),
yc1(it.yc1),
xc(it.xc),
yc(it.yc),
c(it.c) {}
Iterator& operator=(const Iterator& it) {
outer = it.outer;
xc0 = it.xc0;
xc1 = it.xc1;
yc0 = it.yc0;
yc1 = it.yc1;
xc = it.xc;
yc = it.yc;
c = it.c;
}
bool hasNext() {
if (c == nullptr) findNext();
return (c != nullptr);
}
T& next() {
T* thisObj = c->object;
findNext();
return *thisObj; // return Segment
}
private:
void findNext() {
if (c != nullptr) c = c->next;
if (c != nullptr) return;
xc++;
while (true) {
if (xc > xc1) {
yc++;
xc = xc0;
}
if (yc > yc1) break;
c = outer->cells_[yc][xc];
if (c != nullptr) break;
xc++;
}
}
Gridder* outer;
int xc0, xc1, yc0, yc1;
int xc, yc;
Cell* c;
};
typedef Iterator iterator;
iterator find(float x, float y, float range) {
return Iterator(this, x, y, range);
}
};
} // namespace AprilTags
#endif // APRILTAGS_GRIDDER_H_