142 lines
3.6 KiB
C++
Executable File
142 lines
3.6 KiB
C++
Executable File
#ifndef GRID_H
|
|
#define GRID_H
|
|
|
|
#include <vector>
|
|
#include <unordered_map>
|
|
|
|
#include "../Exception.h"
|
|
#include "GridPoint.h"
|
|
#include "GridNode.h"
|
|
#include <iostream>
|
|
|
|
/**
|
|
* grid of the given grid-size, storing some value which
|
|
* extends GridPoint
|
|
*/
|
|
template <int gridSize_cm, typename T> class Grid {
|
|
|
|
typedef uint64_t UID;
|
|
|
|
private:
|
|
|
|
/** all elements (nodes) within the grid */
|
|
std::vector<T> nodes;
|
|
|
|
/** UID -> index mapping */
|
|
std::unordered_map<UID, int> hashes;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
Grid() {
|
|
static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
|
|
static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
|
|
}
|
|
|
|
/** no-copy */
|
|
Grid(const Grid& o) = delete;
|
|
|
|
/** no-assign */
|
|
void operator = (const Grid& o) = delete;
|
|
|
|
|
|
/**
|
|
* add the given element to the grid.
|
|
* returns the index of the element within the internal data-structure
|
|
* @param elem the element to add
|
|
*/
|
|
int add(const T& elem) {
|
|
assertAligned(elem); // assert that the to-be-added element is aligned to the grid
|
|
const int idx = nodes.size(); // next free index
|
|
const UID uid = getUID(elem); // get the UID for this new element
|
|
nodes.push_back(elem); // add it to the grid
|
|
nodes.back()._idx = idx;
|
|
hashes[uid] = idx; // add an UID->index lookup
|
|
return idx; // done
|
|
}
|
|
|
|
/**
|
|
* connect (bi-directional) the two provided nodes
|
|
* @param idx1 index of the first element
|
|
* @param idx2 index of the second element
|
|
*/
|
|
void connect(const int idx1, const int idx2) {
|
|
T& n1 = nodes[idx1]; // get the first element
|
|
T& n2 = nodes[idx2]; // get the second element
|
|
connect(n1, n2);
|
|
}
|
|
|
|
/**
|
|
* connect (bi-directional) the two provided nodes
|
|
* @param n1 the first node
|
|
* @param n2 the second node
|
|
*/
|
|
void connect(T& n1, T& n2) {
|
|
n1._neighbors[n1._numNeighbors] = n2._idx; // add them both as neighbors
|
|
n2._neighbors[n2._numNeighbors] = n1._idx;
|
|
++n1._numNeighbors; // increment the neighbor-counter
|
|
++n2._numNeighbors;
|
|
}
|
|
|
|
/** get the number of contained nodes */
|
|
int getNumNodes() const {
|
|
return nodes.size();
|
|
}
|
|
|
|
/** get the number of neighbors for the given element */
|
|
int getNumNeighbors(const int idx) const {
|
|
return getNumNeighbors(nodes[idx]);
|
|
}
|
|
|
|
/** get the number of neighbors for the given element */
|
|
int getNumNeighbors(const T& e) const {
|
|
return e._numNeighbors;
|
|
}
|
|
|
|
/** get the center-node the given Point belongs to */
|
|
const T& getNodeFor(const GridPoint& p) {
|
|
const UID uid = getUID(p);
|
|
if (hashes.find(uid) == hashes.end()) {throw Exception("element not found!");}
|
|
return nodes[hashes[uid]];
|
|
}
|
|
|
|
/** get the BBox for the given node */
|
|
GridNodeBBox getBBox(const int idx) const {
|
|
return getBBox(nodes[idx]);
|
|
}
|
|
|
|
/** get the BBox for the given node */
|
|
GridNodeBBox getBBox(const T& node) const {
|
|
return GridNodeBBox(node, gridSize_cm);
|
|
}
|
|
|
|
/**
|
|
* get an UID for the given point.
|
|
* this works only for aligned points.
|
|
*
|
|
*/
|
|
UID getUID(const GridPoint& p) const {
|
|
const uint64_t x = std::round(p.x_cm / gridSize_cm);
|
|
const uint64_t y = std::round(p.y_cm / gridSize_cm);
|
|
const uint64_t z = std::round(p.z_cm / gridSize_cm);
|
|
return (z << 40) | (y << 20) | (x << 0);
|
|
}
|
|
|
|
/** array access */
|
|
const T& operator [] (const int idx) const {
|
|
return nodes[idx];
|
|
}
|
|
|
|
private:
|
|
|
|
/** asssert that the given element is aligned to the grid */
|
|
void assertAligned(const T& elem) {
|
|
if (((int)elem.x_cm % gridSize_cm) != 0) {throw Exception("element's x is not aligned!");}
|
|
if (((int)elem.y_cm % gridSize_cm) != 0) {throw Exception("element's y is not aligned!");}
|
|
if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");}
|
|
}
|
|
|
|
};
|
|
|
|
#endif // GRID_H
|