many changes :P

grid/Grid.h

@@ -9,6 +9,9 @@
 #include "GridNode.h"
 #include <iostream>
 
+#include <KLib/Assertions.h>
+#include "../geo/BBox3.h"
+
 /**
  * grid of the given grid-size, storing some value which
  * extends GridPoint
@@ -47,6 +50,11 @@ public:
 	 */
 	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
@@ -55,27 +63,39 @@ public:
 		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;
+		if (n1._numNeighbors > 12) {
+			int i = 0;
+		}
+		_assertBetween(n1._numNeighbors, 0, 12, "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 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);
+	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 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;
+	void connectBiDir(T& n1, T& n2) {
+		connectUniDir(n1, n2);
+		connectUniDir(n2, n1);
 	}
 
 	/** get the number of contained nodes */
@@ -93,6 +113,24 @@ public:
 		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);
@@ -123,10 +161,151 @@ public:
 	}
 
 	/** array access */
-	const T& operator [] (const int idx) const {
+	T& operator [] (const int idx) {
 		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;
+
+	}
+
+	/**
+	 * remove all nodes, marked for deletion.
+	 * BEWARE: this will invalidate all indices used externally!
+	 */
+	void cleanup() {
+
+		for (size_t i = 0; i < nodes.size(); ++i) {
+			if (nodes[i]._idx == -1) {
+				nodes.erase(nodes.begin()+i);
+				moveDown(i);
+				--i;
+			}
+		}
+
+	}
+
+	void moveDown(const int idx) {
+		for (size_t i = 0; i < nodes.size(); ++i) {
+			if (nodes[i]._idx >= idx) {--nodes[i]._idx;}
+			for (int n = 0; n < nodes[i]._numNeighbors; ++n) {
+				if (nodes[i]._neighbors[n] >= idx) {--nodes[i]._neighbors[n];}
+			}
+		}
+	}
+
+	class NeighborIter : std::iterator<std::input_iterator_tag, int> {
+	private:
+		Grid<gridSize_cm, T>& grid;
+		int nodeIdx;
+		int nIdx;
+	public:
+		NeighborIter(Grid<gridSize_cm, T>& grid, const int nodeIdx, const int nIdx) : grid(grid), nodeIdx(nodeIdx), nIdx(nIdx) {;}
+		NeighborIter& operator++() {++nIdx; return *this;}
+		NeighborIter operator++(int) {NeighborIter tmp(*this); operator++(); return tmp;}
+		bool operator==(const NeighborIter& rhs) {return nodeIdx == rhs.nodeIdx && nIdx == rhs.nIdx;}
+		bool operator!=(const NeighborIter& rhs) {return nodeIdx != rhs.nodeIdx || nIdx != rhs.nIdx;}
+		T& operator*() {return (T&) grid.nodes[nodeIdx]._neighbors[nIdx];}
+	};
+
+	class NeighborForEach {
+	private:
+		Grid<gridSize_cm, T>& grid;
+		int nodeIdx;
+	public:
+		NeighborForEach(Grid<gridSize_cm, T>& grid, const int nodeIdx) : grid(grid), nodeIdx(nodeIdx) {;}
+		NeighborIter begin() {return NeighborIter(grid, nodeIdx, 0);}
+		NeighborIter end() {return NeighborIter(grid, nodeIdx, grid[nodeIdx]._numNeighbors);}
+	};
+
+	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 { 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 */