FrankE
e6329e1db4
deleting from the grid is now bleaching fast added new helper methods many new test-cases many new methods for geo classes and others added a bunch of new grid-walkers
427 lines
11 KiB
C++
Executable File
427 lines
11 KiB
C++
Executable File
#ifndef GRID_H
|
|
#define GRID_H
|
|
|
|
#include <vector>
|
|
#include <iostream>
|
|
#include <unordered_map>
|
|
|
|
#include "../Exception.h"
|
|
#include "GridPoint.h"
|
|
#include "GridNode.h"
|
|
|
|
#include <KLib/Assertions.h>
|
|
#include "../geo/BBox3.h"
|
|
#include "../misc/Debug.h"
|
|
|
|
/**
|
|
* grid of a given-size, storing some user-data-value which
|
|
* - extends GridPoint and GridNode
|
|
*
|
|
* Usage:
|
|
* for (Node& n : grid) {...}
|
|
* for (Node& n2 : grid.neighbors(n)) {...}
|
|
*
|
|
*/
|
|
template <typename T> class Grid {
|
|
|
|
static constexpr const char* name = "Grid";
|
|
|
|
#include "GridNeighborIterator.h"
|
|
|
|
/** UID for nodes */
|
|
typedef uint64_t UID;
|
|
|
|
private:
|
|
|
|
/** all elements (nodes) within the grid */
|
|
std::vector<T> nodes;
|
|
|
|
/** UID -> index mapping */
|
|
std::unordered_map<UID, int> hashes;
|
|
|
|
/** the user-given grid-size */
|
|
const int gridSize_cm;
|
|
|
|
public:
|
|
|
|
/** ctor with the grid's size (in cm) */
|
|
Grid(const int gridSize_cm) : gridSize_cm(gridSize_cm) {
|
|
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;
|
|
|
|
|
|
/** allows for-each iteration over all included nodes */
|
|
decltype(nodes.begin()) begin() {return nodes.begin();}
|
|
|
|
/** allows for-each iteration over all included nodes */
|
|
decltype(nodes.end()) end() {return nodes.end();}
|
|
|
|
/** get the grid's size */
|
|
int getGridSize_cm() const {return gridSize_cm;}
|
|
|
|
/**
|
|
* 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
|
|
return addUnaligned(elem);
|
|
}
|
|
|
|
/** add the given (not necessarly aligned) element to the grid */
|
|
int addUnaligned(const T& elem) {
|
|
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 (uni-dir) i1 -> i2 */
|
|
void connectUniDir(const int idx1, const int idx2) {
|
|
connectUniDir(nodes[idx1], nodes[idx2]);
|
|
}
|
|
|
|
/** connect (uni-dir) i1 -> i2 */
|
|
void connectUniDir(T& n1, const T& n2) {
|
|
n1._neighbors[n1._numNeighbors] = n2._idx;
|
|
++n1._numNeighbors;
|
|
_assertBetween(n1._numNeighbors, 0, 10, "number of neighbors out of bounds!");
|
|
}
|
|
|
|
/**
|
|
* connect (bi-directional) the two provided nodes
|
|
* @param idx1 index of the first element
|
|
* @param idx2 index of the second element
|
|
*/
|
|
void connectBiDir(const int idx1, const int idx2) {
|
|
connectBiDir(nodes[idx1], nodes[idx2]);
|
|
}
|
|
|
|
|
|
/**
|
|
* connect (bi-directional) the two provided nodes
|
|
* @param n1 the first node
|
|
* @param n2 the second node
|
|
*/
|
|
void connectBiDir(T& n1, T& n2) {
|
|
connectUniDir(n1, n2);
|
|
connectUniDir(n2, n1);
|
|
}
|
|
|
|
/** 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 n-th neighbor for the given node */
|
|
T& getNeighbor(const int nodeIdx, const int nth) const {
|
|
const T& node = nodes[nodeIdx];
|
|
return getNeighbor(node, nth);
|
|
}
|
|
|
|
/** get the n-th neighbor for the given node */
|
|
T& getNeighbor(const T& node, const int nth) const {
|
|
const T& neighbor = nodes[node._neighbors[nth]];
|
|
return (T&) neighbor;
|
|
}
|
|
|
|
/** do we have a center-point the given point belongs to? */
|
|
bool hasNodeFor(const GridPoint& p) const {
|
|
const UID uid = getUID(p);
|
|
return (hashes.find(uid) != hashes.end());
|
|
}
|
|
|
|
/** 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 center-node the given Point belongs to. or nullptr if not present */
|
|
const T* getNodePtrFor(const GridPoint& p) {
|
|
auto it = hashes.find(getUID(p));
|
|
return (it == hashes.end()) ? (nullptr) : (&nodes[it->second]);
|
|
}
|
|
|
|
/** 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 / (float)gridSize_cm);
|
|
const uint64_t y = std::round(p.y_cm / (float)gridSize_cm);
|
|
const uint64_t z = std::round(p.z_cm / (float)gridSize_cm);
|
|
return (z << 40) | (y << 20) | (x << 0);
|
|
}
|
|
|
|
/** array access */
|
|
T& operator [] (const int idx) {
|
|
_assertBetween(idx, 0, getNumNodes()-1, "index out of bounds");
|
|
return nodes[idx];
|
|
}
|
|
|
|
/** const array access */
|
|
const T& operator [] (const int idx) const {
|
|
_assertBetween(idx, 0, getNumNodes()-1, "index out of bounds");
|
|
return nodes[idx];
|
|
}
|
|
|
|
/** disconnect the two nodes (bidirectional) */
|
|
void disconnectBiDir(const int idx1, const int idx2) {
|
|
disconnectBiDir(nodes[idx1], nodes[idx2]);
|
|
}
|
|
|
|
/** disconnect the two nodes (bidirectional) */
|
|
void disconnectBiDir(T& n1, T& n2) {
|
|
disconnectUniDir(n1, n2);
|
|
disconnectUniDir(n2, n1);
|
|
}
|
|
|
|
/** remove the connection from n1 to n2 (not the other way round!) */
|
|
void disconnectUniDir(T& n1, T& n2) {
|
|
for (int n = 0; n < n1._numNeighbors; ++n) {
|
|
if (n1._neighbors[n] == n2._idx) {
|
|
arrayRemove(n1._neighbors, n, n1._numNeighbors);
|
|
--n1._numNeighbors;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
/** remove the given array-index by moving all follwing elements down by one */
|
|
template <typename X> void arrayRemove(X* arr, const int idxToRemove, const int arrayLen) {
|
|
for (int i = idxToRemove+1; i < arrayLen; ++i) {
|
|
arr[i-1] = arr[i];
|
|
}
|
|
}
|
|
|
|
/**
|
|
* mark the given node for deletion
|
|
* see: cleanup()
|
|
*/
|
|
void remove(const int idx) {
|
|
remove(nodes[idx]);
|
|
}
|
|
|
|
void remove(T& node) {
|
|
|
|
// disconnect from all neighbors
|
|
while (node._numNeighbors) {
|
|
disconnectBiDir(node._idx, node._neighbors[0]);
|
|
}
|
|
|
|
// remove from hash-list
|
|
hashes.erase(getUID(node));
|
|
|
|
// mark for deleteion (see: cleanup())
|
|
node._idx = -1;
|
|
|
|
}
|
|
|
|
void cleanup() {
|
|
|
|
Log::add(name, "running grid cleanup", false);
|
|
Log::tick();
|
|
|
|
// generate a look-up-table for oldIndex (before deletion) -> newIndex (after deletion)
|
|
std::vector<int> oldToNew; oldToNew.resize(nodes.size());
|
|
int newIdx = 0;
|
|
for (size_t oldIdx = 0; oldIdx < nodes.size(); ++oldIdx) {
|
|
if (nodes[oldIdx].getIdx() != -1) {
|
|
oldToNew[oldIdx] = newIdx;
|
|
++newIdx;
|
|
}
|
|
}
|
|
|
|
// adjust all indices from the old to the new mapping
|
|
for (size_t i = 0; i < nodes.size(); ++i) {
|
|
|
|
// skip the nodes actually marked for deletion
|
|
if (nodes[i]._idx == -1) {continue;}
|
|
|
|
// adjust the node's index
|
|
nodes[i]._idx = oldToNew[nodes[i]._idx];
|
|
|
|
// adjust its neighbor's indices
|
|
for (int j = 0; j < nodes[i]._numNeighbors; ++j) {
|
|
nodes[i]._neighbors[j] = oldToNew[nodes[i]._neighbors[j]];
|
|
}
|
|
|
|
}
|
|
|
|
// MUCH(!!!) faster than deleting nodes from the existing node-vector
|
|
// is to build a new one and swap those two
|
|
std::vector<T> newNodes;
|
|
for (size_t i = 0; i < nodes.size(); ++i) {
|
|
if (nodes[i]._idx != -1) {newNodes.push_back(nodes[i]);}
|
|
}
|
|
std::swap(nodes, newNodes);
|
|
|
|
Log::tock();
|
|
|
|
rebuildHashes();
|
|
|
|
}
|
|
|
|
/** rebuild the UID-hash-list */
|
|
void rebuildHashes() {
|
|
|
|
Log::add(name, "rebuilding UID hashes", false);
|
|
Log::tick();
|
|
|
|
hashes.clear();
|
|
for (size_t idx = 0; idx < nodes.size(); ++idx) {
|
|
hashes[getUID(nodes[idx])] = idx;
|
|
}
|
|
|
|
Log::tock();
|
|
|
|
}
|
|
|
|
// /**
|
|
// * remove all nodes, marked for deletion.
|
|
// * BEWARE: this will invalidate all indices used externally!
|
|
// */
|
|
// void cleanupOld() {
|
|
|
|
// Log::add(name, "running grid cleanup");
|
|
|
|
// // check every single node
|
|
// for (size_t i = 0; i < nodes.size(); ++i) {
|
|
|
|
// // is this node marked as "deleted"? (idx == -1)
|
|
// if (nodes[i]._idx == -1) {
|
|
|
|
// // remove this node
|
|
// deleteNode(i);
|
|
// --i;
|
|
|
|
// }
|
|
// }
|
|
|
|
// // rebuild hashes
|
|
// Log::add(name, "rebuilding UID hashes", false);
|
|
// Log::tick();
|
|
// hashes.clear();
|
|
// for (size_t idx = 0; idx < nodes.size(); ++idx) {
|
|
// hashes[getUID(nodes[idx])] = idx;
|
|
// }
|
|
// Log::tock();
|
|
|
|
// }
|
|
|
|
private:
|
|
|
|
// /** hard-delete the given node */
|
|
// void deleteNode(const int idx) {
|
|
|
|
// _assertBetween(idx, 0, nodes.size()-1, "index out of bounds");
|
|
|
|
// // COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT
|
|
|
|
// // remove him from the node list (reclaim its memory and its index)
|
|
// nodes.erase(nodes.begin()+idx);
|
|
|
|
// // decrement the index for all of the following nodes and adjust neighbor references
|
|
// for (size_t i = 0; i < nodes.size(); ++i) {
|
|
|
|
// // decrement the higher indices (reclaim the free one)
|
|
// if (nodes[i]._idx >= idx) { --nodes[i]._idx;}
|
|
|
|
// // adjust the neighbor references (decrement by one)
|
|
// for (int n = 0; n < nodes[i]._numNeighbors; ++n) {
|
|
// if (nodes[i]._neighbors[n] >= idx) {--nodes[i]._neighbors[n];}
|
|
// }
|
|
|
|
// }
|
|
// }
|
|
|
|
public:
|
|
|
|
|
|
|
|
NeighborForEach neighbors(const int idx) {
|
|
return neighbors(nodes[idx]);
|
|
}
|
|
|
|
NeighborForEach neighbors(const T& node) {
|
|
return NeighborForEach(*this, node._idx);
|
|
}
|
|
|
|
/** get the grid's bounding-box. EXPENSIVE! */
|
|
BBox3 getBBox() const {
|
|
BBox3 bb;
|
|
for (const T& n : nodes) {
|
|
bb.add( Point3(n.x_cm, n.y_cm, n.z_cm) );
|
|
}
|
|
return bb;
|
|
}
|
|
|
|
int kdtree_get_point_count() const {
|
|
return nodes.size();
|
|
}
|
|
|
|
template <class BBOX> bool kdtree_get_bbox(BBOX& bb) const { (void) bb; return false; }
|
|
|
|
inline float kdtree_get_pt(const size_t idx, const int dim) const {
|
|
const T& p = nodes[idx];
|
|
if (dim == 0) {return p.x_cm;}
|
|
if (dim == 1) {return p.y_cm;}
|
|
if (dim == 2) {return p.z_cm;}
|
|
throw 1;
|
|
}
|
|
|
|
inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
|
|
const float d0 = p1[0] - nodes[idx_p2].x_cm;
|
|
const float d1 = p1[1] - nodes[idx_p2].y_cm;
|
|
const float d2 = p1[2] - nodes[idx_p2].z_cm;
|
|
return (d0*d0) + (d1*d1) + (d2*d2);
|
|
}
|
|
|
|
|
|
|
|
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
|