571 lines
16 KiB
C++
Executable File
571 lines
16 KiB
C++
Executable File
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef GRID_H
|
||
#define GRID_H
|
||
|
||
#include <vector>
|
||
#include <iostream>
|
||
#include <unordered_map>
|
||
#include <algorithm>
|
||
|
||
#include "../Assertions.h"
|
||
|
||
#include "../Exception.h"
|
||
#include "GridPoint.h"
|
||
#include "GridNode.h"
|
||
|
||
#include "../Assertions.h"
|
||
#include "../geo/BBox3.h"
|
||
#include "../misc/Debug.h"
|
||
|
||
#define GM_BOX 1
|
||
#define GM_HOBEYCOMB 2
|
||
#define GRID_MODE GM_BOX
|
||
|
||
|
||
/**
|
||
* 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!");
|
||
StaticAssert::AinheritsB<T, GridNode>(); // "T must inherit from GridNode!"
|
||
StaticAssert::AinheritsB<T, GridPoint>(); // "T must inherit from GridPoint!"
|
||
Log::add(name, "empty grid with " + std::to_string(gridSize_cm) + "cm grid-size");
|
||
}
|
||
|
||
/** no-copy */
|
||
Grid(const Grid& o) = delete;
|
||
|
||
/**
|
||
* reset (clear) the grid
|
||
* remove all nodes, hashes, ..
|
||
*/
|
||
void reset() {
|
||
nodes.clear();
|
||
hashes.clear();
|
||
}
|
||
|
||
/** is the grid empty? */
|
||
bool isEmpty() const {
|
||
return nodes.empty();
|
||
}
|
||
|
||
/** 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 bool check=true) {
|
||
const UID uid = getUID(elem); // get the UID for this new element
|
||
if (check) {
|
||
Assert::isTrue(hashes.find(uid) == hashes.end(), "node's UID is already taken!"); // avoid potential errors
|
||
}
|
||
const int idx = nodes.size(); // next free index
|
||
nodes.push_back(elem); // add it to the grid
|
||
nodes.back()._idx = idx; // let the node know his own index
|
||
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) {
|
||
Assert::isFalse(n1.hasNeighbor(n2._idx), "this neighbor is already connected"); // already connected?
|
||
Assert::notEqual(n1.getIdx(), n2.getIdx(), "can not connect node with itself");
|
||
Assert::isFalse(n1.fullyConnected(), "this node has already reached its neighbor-limit");
|
||
n1._neighbors[n1._numNeighbors] = n2._idx;
|
||
++n1._numNeighbors;
|
||
}
|
||
|
||
/**
|
||
* 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 a list of all nodes within the graph */
|
||
const std::vector<T>& getNodes() const {
|
||
return nodes;
|
||
}
|
||
|
||
/** get the center-node the given Point belongs to */
|
||
const T& getNodeFor(const GridPoint& p) const {
|
||
//const UID uid = getUID(p);
|
||
auto it = hashes.find(getUID(p));
|
||
Assert::isTrue(it != hashes.end(), "element not found!");
|
||
return nodes[it->second];
|
||
}
|
||
|
||
/** get the center-node the given Point belongs to. or nullptr if not present */
|
||
const T* getNodePtrFor(const GridPoint& p) const {
|
||
auto it = hashes.find(getUID(p));
|
||
return (it == hashes.end()) ? (nullptr) : (&nodes[it->second]);
|
||
}
|
||
|
||
/** get the node nearest to the given positon */
|
||
const T& getNearestNode(const GridPoint& p) const {
|
||
auto comp = [p] (const T& a, const T& b) { return a.getDistanceInMeter(p) < b.getDistanceInMeter(p); };
|
||
auto it = std::min_element(nodes.begin(), nodes.end(), comp);
|
||
return nodes[it->getIdx()];
|
||
}
|
||
|
||
/** get the node nearest to the given positon, examining only the nodes given by the provided index-array */
|
||
const T& getNearestNode(const GridPoint& p, const std::vector<int>& indices) const {
|
||
auto comp = [&] (const int a, const int b) { return nodes[a].getDistanceInMeter(p) < nodes[b].getDistanceInMeter(p); };
|
||
auto it = std::min_element(indices.begin(), indices.end(), comp);
|
||
return nodes[it->getIdx()];
|
||
}
|
||
|
||
/** 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);
|
||
}
|
||
|
||
/** is this node part of a non-plain stair/escalator */
|
||
bool isPlain(const T& n1) const {
|
||
for (const T& n2 : neighbors(n1)) {
|
||
if (n2.z_cm != n1.z_cm) {return false;}
|
||
}
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
* get an UID for the given point.
|
||
* this works only for aligned points.
|
||
*
|
||
*/
|
||
UID getUID(const GridPoint& p) const {
|
||
|
||
// sanity check (region between -1^19 and +1^19
|
||
const int32_t max = 1 << 19;
|
||
Assert::isBetween((int32_t)p.x_cm, -max, +max, "x out of bounds");
|
||
Assert::isBetween((int32_t)p.y_cm, -max, +max, "y out of bounds");
|
||
Assert::isBetween((int32_t)p.z_cm, -max, +max, "z out of bounds");
|
||
|
||
// shift by half of the allowed width of 20 bit to allow negative regions:
|
||
// -> 19 bit positive and 19 bit negative
|
||
const uint64_t center = 1 << 19;
|
||
|
||
// build
|
||
#if (GRID_MODE == GM_HOBEYCOMB)
|
||
const int xx = ((int)std::round(p.y_cm / gridSize_cm) % 2 == 0) ? (0) : (gridSize_cm/2);
|
||
const uint64_t x = center + (int64_t) idxX(p.x_cm-xx);
|
||
const uint64_t y = center + (int64_t) idxY(p.y_cm);
|
||
const uint64_t z = center + (int64_t) idxZ(p.z_cm);
|
||
#elif (GRID_MODE == GM_BOX)
|
||
const uint64_t x = center + (int64_t) idxX(p.x_cm);
|
||
const uint64_t y = center + (int64_t) idxY(p.y_cm);
|
||
const uint64_t z = center + (int64_t) idxZ(p.z_cm);
|
||
#endif
|
||
|
||
|
||
|
||
return (z << 40) | (y << 20) | (x << 0);
|
||
|
||
}
|
||
|
||
inline int idxX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm);}
|
||
inline int idxY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm);}
|
||
inline int idxZ(const int z_cm) const {return std::round(z_cm / 20.0f);} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
|
||
|
||
inline int snapX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm) * gridSize_cm;}
|
||
inline int snapY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm) * gridSize_cm;}
|
||
inline int snapZ(const int z_cm) const {return std::round(z_cm / 20.0f) * 20;} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
|
||
|
||
|
||
/** array access */
|
||
T& operator [] (const int idx) {
|
||
Assert::isBetween(idx, 0, getNumNodes()-1, "index out of bounds");
|
||
return nodes[idx];
|
||
}
|
||
|
||
/** const array access */
|
||
const T& operator [] (const int idx) const {
|
||
Assert::isBetween(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(const int idx1, const int idx2) {
|
||
disconnectUniDir(nodes[idx1], nodes[idx2]);
|
||
}
|
||
|
||
/** remove the connection from n1 to n2 (not the other way round!) */
|
||
void disconnectUniDir(T& n1, const 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;
|
||
}
|
||
}
|
||
}
|
||
|
||
/** convert to a GridPoint coordinate (in cm) */
|
||
GridPoint toGridPoint(const Point3 pos_m) const {
|
||
return GridPoint( snapX(pos_m.x*100), snapY(pos_m.y*100), snapZ(pos_m.z*100) );
|
||
}
|
||
|
||
/** 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]);
|
||
}
|
||
|
||
|
||
/**
|
||
* mark the given node for deletion
|
||
* see: cleanup()
|
||
*/
|
||
void remove(T& node) {
|
||
|
||
// disconnect from all neighbors
|
||
int cnt = 0;
|
||
while (node._numNeighbors) {
|
||
|
||
// by removing one neighbor, all others are shifted to close the gap within the array
|
||
// its therefor ok to always delete array index [0]
|
||
disconnectBiDir(node._idx, node._neighbors[0]);
|
||
if (++cnt > node.MAX_NEIGHBORS) {
|
||
Log::add(name, "WARNING! found unsolveable neighboring! " + node.asString(), true);
|
||
break;
|
||
}
|
||
}
|
||
|
||
// remove from hash-list
|
||
hashes.erase(getUID(node));
|
||
|
||
// mark for deleteion (see: cleanup())
|
||
node._idx = -1;
|
||
|
||
}
|
||
|
||
/** remove all nodes marked for deletion */
|
||
void cleanup() {
|
||
|
||
debugMemoryUsage();
|
||
|
||
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();
|
||
|
||
debugMemoryUsage();
|
||
|
||
}
|
||
|
||
/** 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();
|
||
|
||
}
|
||
|
||
/** debug-print the grid's current memory usage */
|
||
void debugMemoryUsage() {
|
||
const uint32_t bytes = nodes.size() * sizeof(T);
|
||
const uint32_t numNodes = nodes.size();
|
||
uint32_t numNeighbors = 0;
|
||
//uint32_t numNeighborsUnused = 0;
|
||
for (T& n : nodes) {
|
||
numNeighbors += n._numNeighbors;
|
||
}
|
||
Log::add(name, "memory: " + std::to_string(bytes/1024.0f/1024.0f) + " MB in " + std::to_string(numNodes) + " nodes");
|
||
}
|
||
|
||
|
||
public:
|
||
|
||
/** serialize into the given stream */
|
||
void write(std::ostream& out) {
|
||
|
||
// size (in bytes) one node has. this is a sanity check whether the file matches the code!
|
||
const int nodeSize = sizeof(T);
|
||
out.write((const char*) &nodeSize, sizeof(nodeSize));
|
||
|
||
// number of nodes
|
||
const int numNodes = nodes.size();
|
||
Assert::isTrue(numNodes > 0, "grid says it contains 0 nodes. there must be some error!");
|
||
out.write((const char*) &numNodes, sizeof(numNodes));
|
||
|
||
// serialize
|
||
for (const T& node : nodes) {
|
||
out.write((const char*) &node, sizeof(T));
|
||
}
|
||
|
||
// serialize static parameters
|
||
T::staticSerialize(out);
|
||
|
||
out.flush();
|
||
|
||
}
|
||
|
||
/** deserialize from the given stream */
|
||
void read(std::istream& inp) {
|
||
|
||
Log::add(name, "loading grid from input-stream");
|
||
|
||
// size (in bytes) one node has. this is a sanity check whether the file matches the code!
|
||
int nodeSize;
|
||
inp.read((char*) &nodeSize, sizeof(nodeSize));
|
||
Assert::equal(nodeSize, (int)sizeof(T), "sizeof(node) of the saved grid does not match sizeof(node) for the code!");
|
||
|
||
// number of nodes
|
||
int numNodes;
|
||
inp.read((char*) &numNodes, sizeof(numNodes));
|
||
Assert::isTrue(numNodes > 0, "grid-file says it contains 0 nodes. there must be some error!");
|
||
|
||
// allocate node-space
|
||
nodes.resize(numNodes);
|
||
|
||
// deserialize
|
||
inp.read((char*) nodes.data(), numNodes*sizeof(T));
|
||
Log::add(name, "deserialized " + std::to_string(nodes.size()) + " nodes");
|
||
|
||
// deserialize static parameters
|
||
T::staticDeserialize(inp);
|
||
|
||
// update
|
||
rebuildHashes();
|
||
|
||
}
|
||
|
||
|
||
public:
|
||
|
||
|
||
|
||
NeighborForEach neighbors(const int idx) const {
|
||
return neighbors(nodes[idx]);
|
||
}
|
||
|
||
NeighborForEach neighbors(const T& node) const {
|
||
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 (GRID_MODE == GM_HOBEYCOMB)
|
||
|
||
#elif (GRID_MODE == GM_BOX)
|
||
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
|
||
}
|
||
|
||
};
|
||
|
||
#endif // GRID_H
|