frank
ca6fed5371
worked on grid-generation added helper library for nav-meshes started working on nav meshes
556 lines
15 KiB
C++
Executable File
556 lines
15 KiB
C++
Executable File
#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();
|
|
}
|
|
|
|
/** 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
|