many changes :P

CMakeLists.txt

@@ -8,7 +8,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
 # select build type
 SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" )
 
-PROJECT(graphModel)
+PROJECT(Indoor)
 
 IF(NOT CMAKE_BUILD_TYPE)
 	MESSAGE(STATUS "No build type selected. Default to Debug")
@@ -36,7 +36,7 @@ FILE(GLOB SOURCES
 	./*.cpp
 	./*/*.cpp
 	./*/*/*.cpp
-	/mnt/firma/kunden/HandyGames/KLib/inc/tinyxml/*.cpp
+	../KLib/inc/tinyxml/*.cpp
 )
 
 
@@ -64,6 +64,7 @@ ADD_DEFINITIONS(
 	-O0
 
 	-DWITH_TESTS
+	-DWITH_ASSERTIONS
 
 )
 

floorplan/Floor.h

@@ -2,7 +2,7 @@
 #define FLOOR_H
 
 #include <vector>
-#include "../geo/Line2D.h"
+#include "../geo/Line2.h"
 
 /**
  * represents one floor by describing all contained obstacles
@@ -13,7 +13,7 @@ private:
 
 
 	/** all obstacles within the floor */
-	std::vector<Line2D> lines;
+	std::vector<Line2> lines;
 
 	/** total width of the floor (in meter) */
 	double width_cm;
@@ -28,12 +28,12 @@ public:
 	Floor(const float width_cm, const float depth_cm) : width_cm(width_cm), depth_cm(depth_cm) {;}
 
 	/** get all obstacles */
-	const std::vector<Line2D>& getObstacles() const {
+	const std::vector<Line2>& getObstacles() const {
 		return lines;
 	}
 
 	/** add a new obstacle to this floor */
-	void addObstacle(const Line2D& l ) {
+	void addObstacle(const Line2& l ) {
 		lines.push_back(l);
 	}
 

floorplan/FloorplanFactorySVG.h

@@ -2,6 +2,8 @@
 #define FLOORPLANFACTORYSVG_H
 
 #include "Floor.h"
+#include "Stairs.h"
+
 #include <string>
 #include "../Exception.h"
 
@@ -32,21 +34,21 @@ class FloorplanFactorySVG {
 		}
 
 		/** scale (x, y) into (_x, _y) */
-		void scale(const double x, const double y, double& _x, double& _y) const {
+		void scale(const double x, const double y, float& _x, float& _y) const {
 			_x = x * scalingFactor;
 			_y = y * scalingFactor;
 		}
 
-		/** scale the given point into a new output point */
+//		/** scale the given point into a new output point */
-		K::Point scale(const K::Point p) const {
+//		K::Point scale(const K::Point p) const {
-			K::Point ret;
+//			K::Point ret;
-			scale (p.x, p.y, ret.x, ret.y);
+//			scale (p.x, p.y, ret.x, ret.y);
-			return ret;
+//			return ret;
-		}
+//		}
 
 		/** scale the given line into a new output line */
-		Line2D scale(const K::Line l) const {
+		Line2 scale(const K::Line l) const {
-			Line2D ret;
+			Line2 ret;
 			scale (l.p1.x, l.p1.y, ret.p1.x, ret.p1.y);
 			scale (l.p2.x, l.p2.y, ret.p2.x, ret.p2.y);
 			return ret;
@@ -58,8 +60,8 @@ private:
 
 	K::SVGFile svg;
 	SVGScaler scaler;
-	double width;
+	float width;
-	double depth;
+	float depth;
 
 public:
 
@@ -88,22 +90,38 @@ public:
 
 		// create and parse the new floor
 		Floor floor(width, depth);
-		load(layer, floor);
+		loadFloor(layer, floor);
 		return floor;
 
 
 	}
 
+	/** get all stairs within the given layer */
+	Stairs getStairs(const std::string& layerName) {
+
+		// get the requested SVG layer
+		K::SVGComposite* sc = svg.getLayers();
+		K::SVGLayer* layer = sc->getContainedLayerNamed(layerName);
+		if (!layer) {throw Exception("SVG has no layer named '" + layerName + "'");}
+
+		// create and parse the new floor
+		Stairs stairs;
+		loadStairs(layer, stairs);
+		return stairs;
+
+
+	}
+
 private:
 
 	/** recursive loading/parsing of nested SVG elements */
-	void load(K::SVGElement* el, Floor& floor) {
+	void loadFloor(K::SVGElement* el, Floor& floor) {
 
 		switch (el->getType()) {
 
 			case SVGElementType::COMPOSITE: {
 				for (K::SVGElement* sub : ((K::SVGComposite*)el)->getChilds()) {
-					load(sub, floor);
+					loadFloor(sub, floor);
 				}
 				break;
 			}
@@ -111,7 +129,7 @@ private:
 			case SVGElementType::LAYER: {
 				K::SVGLayer* layer = (K::SVGLayer*) el;
 				for (K::SVGElement* sub : layer->getChilds()) {
-					load(sub, floor);
+					loadFloor(sub, floor);
 				}
 				break;
 			}
@@ -133,6 +151,60 @@ private:
 		}
 	}
 
+	/** recursive loading/parsing of nested SVG elements */
+	void loadStairs(K::SVGElement* el, Stairs& stairs) {
+
+		switch (el->getType()) {
+
+			case SVGElementType::COMPOSITE: {
+				for (K::SVGElement* sub : ((K::SVGComposite*)el)->getChilds()) {
+					loadStairs(sub, stairs);
+				}
+				break;
+			}
+
+			case SVGElementType::LAYER: {
+				K::SVGLayer* layer = (K::SVGLayer*) el;
+				for (K::SVGElement* sub : layer->getChilds()) {
+					loadStairs(sub, stairs);
+				}
+				break;
+			}
+
+			case SVGElementType::PATH: {
+
+				int i = 0;
+				Line2 start;
+				Line2 dir;
+
+				for (const K::Line& line : ((K::SVGPath*)el)->getLines()) {
+					if (++i == 1) {
+						start = scaler.scale(line);
+					} else {
+						Stair s;
+						Line2 dir = scaler.scale(line);
+						s.dir = (dir.p2 - dir.p1);
+						const float d = 9;
+						s.from.add(start.p1 + Point2(-d,-d));
+						s.from.add(start.p1 + Point2(+d,+d));
+						s.from.add(start.p2 + Point2(-d,-d));
+						s.from.add(start.p2 + Point2(+d,+d));
+						stairs.push_back(s);
+					}
+				}
+				break;
+			}
+
+			case SVGElementType::TEXT: {
+				break;
+			}
+
+			default:
+				throw "should not happen!";
+
+		}
+	}
+
 };
 
 #endif // FLOORPLANFACTORYSVG_H

floorplan/Stair.h

@@ -0,0 +1,17 @@
+#ifndef STAIR_H
+#define STAIR_H
+
+#include "../geo/BBox2.h"
+#include "../geo/Point2.h"
+
+struct Stair {
+
+	/** bbox with all starting points */
+	BBox2 from;
+
+	/** the direction to move all the starting points to */
+	Point2 dir;
+
+};
+
+#endif // STAIR_H

floorplan/Stairs.h

@@ -0,0 +1,11 @@
+#ifndef STAIRS_H
+#define STAIRS_H
+
+#include <vector>
+#include "Stair.h"
+
+class Stairs : public std::vector<Stair> {
+
+};
+
+#endif // STAIRS_H

geo/Angle.h

@@ -2,20 +2,34 @@
 #define ANGLE_H
 
 #include <cmath>
+#include <KLib/Assertions.h>
 
 class Angle {
 
 public:
 
-	/** get the radians from (x1,y1) to (x2,y2) */
+	/** get the radians from (x1,y1) to (x2,y2) between 0 (to-the-right) and <2_PI */
-	static float getRAD(const float x1, const float y1, const float x2, const float y2) {
+	static float getRAD_2PI(const float x1, const float y1, const float x2, const float y2) {
+		_assertFalse( (x1==x2)&&(y1==y2), "(x1,y1) must not equal (x2,y2)!!");
 		const float tmp = std::atan2(y2-y1, x2-x1);
 		return (tmp < 0) ? (tmp + 2*M_PI) : (tmp);
 	}
 
-	/** get the degrees from (x1,y1) to (x2,y2) */
+	/** get the degrees from (x1,y1) to (x2,y2) between 0 (to-the-right) and <360 */
-	static float getDEG(const float x1, const float y1, const float x2, const float y2) {
+	static float getDEG_360(const float x1, const float y1, const float x2, const float y2) {
-		return radToDeg(getRAD(x1,y1,x2,y2));
+		return radToDeg(getRAD_2PI(x1,y1,x2,y2));
+	}
+
+	/**
+	 * gets the angular difference between
+	 * - the given radians [0:2PI]
+	 * - as a change-in-direction between [0:PI]
+	 */
+	static float getDiffRAD_2PI_PI(const float r1, const float r2) {
+		_assertBetween(r1, 0, 2*M_PI, "r1 out of bounds");
+		_assertBetween(r2, 0, 2*M_PI, "r2 out of bounds");
+		return fmod(std::abs(r2 - r1), M_PI);
+
 	}
 
 	/** convert degrees to radians */

geo/BBox2.h

@@ -0,0 +1,71 @@
+#ifndef BBOX2_H
+#define BBOX2_H
+
+#include "Point2.h"
+#include "Line2.h"
+
+class BBox2 {
+
+protected:
+
+	static constexpr float MAX = +99999;
+	static constexpr float MIN = -99999;
+
+	/** minimum */
+	Point2 p1;
+
+	/** maximum */
+	Point2 p2;
+
+public:
+
+	BBox2() : p1(MAX,MAX), p2(MIN,MIN) {;}
+
+	/** adjust the bounding-box by adding this point */
+	void add(const Point2& p) {
+
+		if (p.x > p2.x) {p2.x = p.x;}
+		if (p.y > p2.y) {p2.y = p.y;}
+
+		if (p.x < p1.x) {p1.x = p.x;}
+		if (p.y < p1.y) {p1.y = p.y;}
+
+	}
+
+	/** get the bbox's minimum */
+	const Point2& getMin() const {return p1;}
+
+	/** get the bbox's maximum */
+	const Point2& getMax() const {return p2;}
+
+	/** equal? */
+	bool operator == (const BBox2& o) const {
+		return	(p1.x == o.p1.x) &&
+				(p1.y == o.p1.y) &&
+				(p2.x == o.p2.x) &&
+				(p2.y == o.p2.y);
+	}
+
+	/** does the BBox intersect with the given line? */
+	bool intersects (const Line2& l) const {
+		Line2 l1(p1.x, p1.y, p2.x, p1.y);		// upper
+		Line2 l2(p1.x, p2.y, p2.x, p2.y);		// lower
+		Line2 l3(p1.x, p1.y, p1.x, p2.y);		// left
+		Line2 l4(p2.x, p1.y, p2.x, p2.y);		// right
+		return	l.getSegmentIntersection(l1) ||
+				l.getSegmentIntersection(l2) ||
+				l.getSegmentIntersection(l3) ||
+				l.getSegmentIntersection(l4);
+	}
+
+	bool contains(const Point2& p) const {
+		if (p.x < p1.x) {return false;}
+		if (p.x > p2.x) {return false;}
+		if (p.y < p1.y) {return false;}
+		if (p.y > p2.y) {return false;}
+		return true;
+	}
+
+};
+
+#endif // BBOX2_H

geo/BBox3.h

@@ -0,0 +1,54 @@
+#ifndef BBOX3_H
+#define BBOX3_H
+
+#include "Point3.h"
+
+class BBox3 {
+
+private:
+
+	static constexpr float MAX = +99999;
+	static constexpr float MIN = -99999;
+
+	/** minimum */
+	Point3 p1;
+
+	/** maximum */
+	Point3 p2;
+
+public:
+
+	BBox3() : p1(MAX,MAX,MAX), p2(MIN,MIN,MIN) {;}
+
+	/** adjust the bounding-box by adding this point */
+	void add(const Point3& p) {
+
+		if (p.x > p2.x) {p2.x = p.x;}
+		if (p.y > p2.y) {p2.y = p.y;}
+		if (p.z > p2.z) {p2.z = p.z;}
+
+		if (p.x < p1.x) {p1.x = p.x;}
+		if (p.y < p1.y) {p1.y = p.y;}
+		if (p.z < p1.z) {p1.z = p.z;}
+
+	}
+
+	/** get the bbox's minimum */
+	const Point3& getMin() const {return p1;}
+
+	/** get the bbox's maximum */
+	const Point3& getMax() const {return p2;}
+
+	/** equal? */
+	bool operator == (const BBox3& o) const {
+		return	(p1.x == o.p1.x) &&
+				(p1.y == o.p1.y) &&
+				(p1.z == o.p1.z) &&
+				(p2.x == o.p2.x) &&
+				(p2.y == o.p2.y) &&
+				(p2.z == o.p2.z);
+	}
+
+};
+
+#endif // BBOX3_H

geo/Line2.h

@@ -0,0 +1,54 @@
+#ifndef LINE2D_H
+#define LINE2D_H
+
+//#include <KLib/geo/Line.h>
+#include "Point2.h"
+
+class Line2 {
+
+public:
+
+	Point2 p1;
+
+	Point2 p2;
+
+public:
+
+	Line2() : p1(), p2() {;}
+
+	Line2(const float x1, const float y1, const float x2, const float y2) : p1(x1,y1), p2(x2,y2) {;}
+
+//	bool getSegmentIntersection(const Line& other) const {
+//		static K::Point p;
+//		return K::Line::getSegmentIntersection(other, p);
+//	}
+
+
+	bool getSegmentIntersection(const Line2& other) const {
+
+		const double bx = p2.x - p1.x;
+		const double by = p2.y - p1.y;
+
+		const double dx = other.p2.x - other.p1.x;
+		const double dy = other.p2.y - other.p1.y;
+
+		const double b_dot_d_perp = bx*dy - by*dx;
+
+		if(b_dot_d_perp == 0) {return false;}
+
+		const double cx = other.p1.x - p1.x;
+		const double cy = other.p1.y - p1.y;
+
+		const double t = (cx * dy - cy * dx) / b_dot_d_perp;
+		if(t < 0 || t > 1) {return false;}
+
+		const double u = (cx * by - cy * bx) / b_dot_d_perp;
+		if(u < 0 || u > 1) {return false;}
+
+		return true;
+
+	}
+
+};
+
+#endif // LINE2D_H

geo/Line2D.h

@@ -1,21 +0,0 @@
-#ifndef LINE2D_H
-#define LINE2D_H
-
-#include <KLib/geo/Line.h>
-
-class Line2D : public K::Line {
-
-public:
-
-	Line2D() : K::Line() {;}
-
-	Line2D(const float x1, const float y1, const float x2, const float y2) : K::Line(x1,y1,x2,y2) {;}
-
-	bool getSegmentIntersection(const Line& other) const {
-		static K::Point p;
-		return K::Line::getSegmentIntersection(other, p);
-	}
-
-};
-
-#endif // LINE2D_H

geo/Point2.h

@@ -0,0 +1,24 @@
+#ifndef POINT2_H
+#define POINT2_H
+
+/**
+ * 2D Point
+ */
+struct Point2 {
+
+	float x;
+	float y;
+
+	/** ctor */
+	Point2() : x(0), y(0) {;}
+
+	/** ctor */
+	Point2(const float x, const float y) : x(x), y(y) {;}
+
+	Point2 operator + (const Point2& o) const {return Point2(x+o.x, y+o.y);}
+
+	Point2 operator - (const Point2& o) const {return Point2(x-o.x, y-o.y);}
+
+};
+
+#endif // POINT2_H

geo/Point3.h

@@ -0,0 +1,29 @@
+#ifndef POINT3_H
+#define POINT3_H
+
+/**
+ * 3D Point
+ */
+struct Point3 {
+
+	float x;
+	float y;
+	float z;
+
+	/** ctor */
+	Point3() : x(0), y(0), z(0) {;}
+
+	/** ctor */
+	Point3(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
+
+
+
+	Point3 operator + (const Point3& o) const {return Point3(x+o.x, y+o.y, z+o.z);}
+
+	Point3 operator - (const Point3& o) const {return Point3(x-o.x, y-o.y, z-o.z);}
+
+	Point3 operator * (const float v) const {return Point3(v*x, v*y, v*z);}
+
+};
+
+#endif // POINT3_H

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) {
+	void connectBiDir(const int idx1, const int idx2) {
-		T& n1 = nodes[idx1];				// get the first element
+		connectBiDir(nodes[idx1], nodes[idx2]);
-		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) {
+	void connectBiDir(T& n1, T& n2) {
-		n1._neighbors[n1._numNeighbors] = n2._idx;				// add them both as neighbors
+		connectUniDir(n1, n2);
-		n2._neighbors[n2._numNeighbors] = n1._idx;
+		connectUniDir(n2, n1);
-		++n1._numNeighbors;										// increment the neighbor-counter
-		++n2._numNeighbors;
 	}
 
 	/** 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 */

grid/GridNode.h

@@ -4,6 +4,9 @@
 #include "GridNodeBBox.h"
 #include "GridPoint.h"
 
+template<int, typename> class Grid;
+
+
 /**
  * INTERNAL DATASTRUCTURE
  * this data-structure is internally used by the Grid
@@ -17,17 +20,26 @@ class GridNode {
 	/** INTERNAL: array-index */
 	int _idx = -1;
 
-	/** INTERNAL: store neighbors (via index) */
+	/** INTERNAL: number of neighbors */
 	int _numNeighbors = 0;
 
-	/** INTERNAL: number of neighbors */
+	/** INTERNAL: store neighbors (via index) */
-	int _neighbors[10] = {};
+	int _neighbors[12] = {};
 
 public:
 
 	GridNode() {;}
 
+	/** get the node's index within its grid */
+	int getIdx() const {return _idx;}
 
+	/** get the number of neighbors for this node */
+	int getNumNeighbors() const {return _numNeighbors;}
+
+	/** get the n-th neighbor for this node */
+	template <int gridSize_cm, typename T> inline T& getNeighbor(const int nth, const Grid<gridSize_cm, T>& grid) const {
+		return grid.getNeighbor(_idx, nth);
+	}
 
 };
 

grid/GridNodeBBox.h

@@ -2,43 +2,21 @@
 #define GRIDNODEBBOX_H
 
 #include "GridPoint.h"
-#include "../geo/Line2D.h"
+
+#include "../geo/BBox2.h"
 
 /**
  * describes the 2D (one floor)
  * bounding-box for one node on the grid
  */
-struct GridNodeBBox {
+struct GridNodeBBox : public BBox2 {
-
-	/** smaller half */
-	float x1_cm, y1_cm;
-
-	/** larger half */
-	float x2_cm, y2_cm;
 
 	/** ctor */
 	GridNodeBBox(const GridPoint& center, const int gridSize_cm) {
-		x1_cm = center.x_cm - gridSize_cm/2;	// smaller half
+		p1.x = center.x_cm - gridSize_cm/2;	// smaller half
-		y1_cm = center.y_cm - gridSize_cm/2;
+		p1.y = center.y_cm - gridSize_cm/2;
-		x2_cm = center.x_cm + gridSize_cm/2;	// larger half
+		p2.x = center.x_cm + gridSize_cm/2;	// larger half
-		y2_cm = center.y_cm + gridSize_cm/2;
+		p2.y = center.y_cm + gridSize_cm/2;
-	}
-
-	/** equal? */
-	bool operator == (const GridNodeBBox& o) const {
-		return (x1_cm == o.x1_cm) && (y1_cm == o.y1_cm) && (x2_cm == o.x2_cm) && (y2_cm == o.y2_cm);
-	}
-
-	/** does the BBox intersect with the given line? */
-	bool intersects (const Line2D& l) const {
-		Line2D l1(x1_cm, y1_cm, x2_cm, y1_cm);		// upper
-		Line2D l2(x1_cm, y2_cm, x2_cm, y2_cm);		// lower
-		Line2D l3(x1_cm, y1_cm, x1_cm, y2_cm);		// left
-		Line2D l4(x2_cm, y1_cm, x2_cm, y2_cm);		// right
-		return	l.getSegmentIntersection(l1) ||
-				l.getSegmentIntersection(l2) ||
-				l.getSegmentIntersection(l3) ||
-				l.getSegmentIntersection(l4);
 	}
 
 };

grid/GridPoint.h

@@ -6,13 +6,13 @@
 struct GridPoint {
 
 	/** x-position (in centimeter) */
-	const float x_cm;
+	float x_cm;
 
 	/** y-position (in centimeter) */
-	const float y_cm;
+	float y_cm;
 
 	/** z-position (in centimeter) */
-	const float z_cm;
+	float z_cm;
 
 
 	/** empty ctor */

grid/factory/GridFactory.h

@@ -3,6 +3,8 @@
 
 #include <string>
 #include "../../floorplan/Floor.h"
+#include "../../floorplan/Stairs.h"
+
 #include "../../geo/Units.h"
 #include "../GridNodeBBox.h"
 #include "../Grid.h"
@@ -22,6 +24,8 @@ public:
 	/** add the given floor at the provided height (in cm) */
 	void addFloor(const Floor& floor, const float z_cm) {
 
+
+		// build grid-points
 		for(int x_cm = 0; x_cm < floor.getWidth_cm(); x_cm += gridSize_cm) {
 			for (int y_cm = 0; y_cm < floor.getDepth_cm(); y_cm += gridSize_cm) {
 
@@ -35,7 +39,181 @@ public:
 			}
 		}
 
-		int i = 0;
+		connectAdjacent(z_cm);
+
+	}
+
+	void connectAdjacent(const float z_cm) {
+
+		// connect adjacent grid-points
+		for (int idx = 0; idx < grid.getNumNodes(); ++idx) {
+
+			T& n1 = (T&) grid[idx];
+			if (n1.z_cm != z_cm) {continue;}	// ugly... different floor -> skip
+
+			// square around each point
+			for (int x =  -gridSize_cm; x <= gridSize_cm; x += gridSize_cm) {
+				for (int y =  -gridSize_cm; y <= gridSize_cm; y += gridSize_cm) {
+
+					// skip the center (node itself)
+					if ((x == y) && (x == 0)) {continue;}
+
+					// position of the potential neighbor
+					int ox = n1.x_cm + x;
+					int oy = n1.y_cm + y;
+					GridPoint p(ox, oy, n1.z_cm);
+
+					// does the grid contain the potential neighbor?
+					if (grid.hasNodeFor(p)) {
+						T& n2 = (T&) grid.getNodeFor(p);
+						grid.connectUniDir(n1, n2);
+					}
+
+				}
+			}
+
+		}
+
+	}
+
+
+	void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {
+
+		for (const Stair& s : stairs) {
+
+			for (int i = 0; i < grid.getNumNodes(); ++i) {
+
+				// potential starting-point for the stair
+				T& n = (T&) grid[i];
+
+				// real starting point for the stair?
+				if (s.from.contains( Point2(n.x_cm, n.y_cm) )) {
+
+					// construct end-point by using the stair's direction
+					const Point3 end = Point3(n.x_cm, n.y_cm, n.z_cm) + Point3(s.dir.x, s.dir.y, (z2_cm-z1_cm));
+					GridPoint gp(end.x, end.y, end.z);
+
+					// does such and end-point exist within the grap? -> construct stair
+					if (grid.hasNodeFor(gp)) {
+						T& n2 = (T&) grid.getNodeFor(gp);
+
+						buildStair(n, n2);
+
+					}
+					int i = 0;
+				}
+			}
+
+		}
+
+	}
+
+	/** build a stair (z-transition) from n1 to n2 */
+	void buildStair(T& n1, T& n2) {
+
+		//TODO: ensure n1 is below n2
+		const float zDiff = n2.z_cm - n1.z_cm;
+		const float xDiff = n2.x_cm - n1.x_cm;
+		const float yDiff = n2.y_cm - n1.y_cm;
+
+		int idx1 = n1.getIdx();
+		int idx2 = -1;
+		const int idx3 = n2.getIdx();
+
+		// move upards in gridSize steps
+		for (int z = gridSize_cm; z < zDiff; z+= gridSize_cm) {
+
+			// calculate the percentage of reached upwards-distance
+			const float percent = z/zDiff;
+
+			// adjust (x,y) accordingly (interpolate)
+			int x = n1.x_cm + xDiff * percent;
+			int y = n1.y_cm + yDiff * percent;
+
+			// snap (x,y) to the grid???
+			//x = std::round(x / gridSize_cm) * gridSize_cm;
+			//y = std::round(y / gridSize_cm) * gridSize_cm;
+
+			// create a new node add it to the grid, and connect it with the previous one
+			idx2 = grid.addUnaligned(T(x,y,z));
+			grid.connectBiDir(idx1, idx2);
+			idx1 = idx2;
+
+		}
+
+		// add the last segment
+		if (idx2 != -1) {
+			grid.connectBiDir(idx2, idx3);
+		}
+
+	}
+
+	/** add the inverted version of the given z-layer */
+	void addInverted(const Grid<gridSize_cm, T>& gIn, const float z_cm) {
+
+		// get the original grid's bbox
+		BBox3 bb = gIn.getBBox();
+
+		// build new grid-points
+		for(int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
+			for (int y_cm = bb.getMin().y; y_cm < bb.getMax().y; y_cm += gridSize_cm) {
+
+				// does the input-grid contain such a point?
+				GridPoint gp(x_cm, y_cm, z_cm);
+				if (gIn.hasNodeFor(gp)) {continue;}
+
+				// add to the grid
+				grid.add(T(x_cm, y_cm, z_cm));
+
+			}
+		}
+
+	}
+
+	// TODO: how to determine the starting index?!
+
+	// IDEAS: find all segments:
+	// start at a random point, add all connected points to the set
+	// start at a NEW random point ( not part of the already processed points), add connected points to a new set
+	// repeat until all points processed
+	// how to handle multiple floor layers?!?!
+	// run after all floors AND staircases were added??
+	// OR: random start, check segment size, < 50% of all nodes? start again
+
+	void removeIsolated() {
+
+		// get largest connected region
+		std::set<int> set;
+		do {
+			const int idxStart = rand() % grid.getNumNodes();
+			set.clear();
+			getConnected(idxStart, set);
+		} while (set.size() < 0.5 * grid.getNumNodes());
+
+
+		// remove all other
+		for (int i = 0; i < grid.getNumNodes(); ++i) {
+			if (set.find(i) == set.end()) {grid.remove(i);}
+		}
+
+		// clean the grid
+		grid.cleanup();
+
+	}
+
+private:
+
+	/** recursively get all connected nodes and add them to the set */
+	void getConnected(const int idx, std::set<int>& set) {
+
+		T& n1 = (T&) grid[idx];
+		set.insert(n1.getIdx());
+
+		for (T& n2 : grid.neighbors(n1)) {
+			if (set.find(n2.getIdx()) == set.end()) {
+				getConnected(n2.getIdx(), set);
+			}
+		}
 
 	}
 
@@ -43,7 +221,7 @@ private:
 
 	/** does the bbox intersect with any of the floor's walls? */
 	bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
-		for (const Line2D l : floor.getObstacles()) {
+		for (const Line2& l : floor.getObstacles()) {
 			if (bbox.intersects(l)) {return true;}
 		}
 		return false;

grid/factory/GridImportance.h

@@ -0,0 +1,84 @@
+#ifndef GRIDIMPORTANCE_H
+#define GRIDIMPORTANCE_H
+
+#include "../Grid.h"
+#include "GridFactory.h"
+#include "../../misc/KNN.h"
+
+#include <KLib/math/distribution/Normal.h>
+
+class GridImportance {
+
+public:
+
+	/** attach importance-factors to the grid */
+	template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, const float z_cm) {
+
+		// get an inverted version of the grid
+		Grid<gridSize_cm, T> inv;
+		GridFactory<gridSize_cm, T> fac(inv);
+		fac.addInverted(g, z_cm);
+
+		// construct KNN search
+		KNN<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
+
+		for (int idx = 0; idx < g.getNumNodes(); ++idx) {
+
+			// process each point
+			T& n1 = (T&) g[idx];
+
+//			// get its nearest neighbor
+//			size_t idxNear;
+//			float distSquared;
+//			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
+//			knn.getNearest(point, idxNear, distSquared);
+
+//			// calculate importante
+//			const float imp = importance( Units::cmToM(std::sqrt(distSquared)) );
+//			n1.imp = imp;
+
+			size_t indices[10];
+			float squaredDist[10];
+			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
+			knn.get(point, 10, indices, squaredDist);
+
+			const float imp1 = importance( Units::cmToM(std::sqrt(squaredDist[0])) );
+			const float imp2 = door( indices );
+			n1.imp = (imp1 + imp2)/2;
+
+		}
+
+
+
+	}
+
+	float door( size_t* indices ) {
+
+		// build covariance
+
+		//if (dist1_m > 1.0) {return 1;}
+		//return 1.0 - std::abs(dist1_m - dist2_m);
+		return 1;
+	}
+
+	float importance(float dist_m) {
+
+		static K::NormalDistribution d1(0.0, 0.5);
+		//if (dist_m > 1.5) {dist_m = 1.5;}
+		return 1.0 - d1.getProbability(dist_m) * 0.5;
+
+//		static K::NormalDistribution d1(1.0, 0.75);
+//		//static K::NormalDistribution d2(3.0, 0.75);
+
+//		if (dist_m > 3.0) {dist_m = 3.0;}
+//		return 0.8 + d1.getProbability(dist_m);// + d2.getProbability(dist_m);
+
+//		if (dist_m < 0.5)	{return 0.8;}
+//		if (dist_m < 1.5)	{return 1.2;}
+//		if (dist_m < 2.5)	{return 0.8;}
+//		else				{return 1.2;}
+	}
+
+};
+
+#endif // GRIDIMPORTANCE_H

main.cpp

@@ -15,7 +15,7 @@ int main(int argc, char** argv) {
 
 #ifdef WITH_TESTS
 	::testing::InitGoogleTest(&argc, argv);
-	//::testing::GTEST_FLAG(filter) = "*first*";
+	//::testing::GTEST_FLAG(filter) = "*bbox*";
 	return RUN_ALL_TESTS();
 #endif
 

misc/KNN.h

@@ -0,0 +1,76 @@
+#ifndef KNN_H
+#define KNN_H
+
+#include "../lib/nanoflann/nanoflann.hpp"
+
+/**
+ * helper class to extract k-nearest-neighbors
+ * from a given input data structure.
+ * uses nanoflann
+ *
+ * usage:
+ *	KNN<float, Grid<20, T>, T, 3> knn(theGrid);
+ *	float search[] = {0,0,0};
+ *	std::vector<T> elems = knn.get(search, 3);
+ */
+template <typename Scalar, typename DataStructure, typename Element, int dim> class KNN {
+
+private:
+
+	/** type-definition for the nanoflann KD-Tree used for searching */
+	typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<Scalar, DataStructure>, DataStructure, dim> Tree;
+
+	/** the maximum depth of the tree */
+	static constexpr int maxLeafs = 10;
+
+	/** the constructed tree used for searching */
+	Tree tree;
+
+	/** the underlying data-structure we want to search within */
+	DataStructure& data;
+
+public:
+
+	/** ctor */
+	KNN(DataStructure& data) : tree(dim, data, nanoflann::KDTreeSingleIndexAdaptorParams(maxLeafs)), data(data) {
+		tree.buildIndex();
+	}
+
+	/** get the k-nearest-neighbors for the given input point */
+	std::vector<Element> get(const Scalar* point, const int numNeighbors, const float maxDistSquared = 99999) const {
+
+		// buffer for to-be-fetched neighbors
+		size_t indices[numNeighbors];
+		float distances[numNeighbors];
+
+		// find k-nearest-neighbors
+		tree.knnSearch(point, numNeighbors, indices, distances);
+
+		// construct output
+		std::vector<Element> elements;
+		for (int i = 0; i < numNeighbors; ++i) {
+			if (distances[i] > maxDistSquared) {continue;}		// too far away?
+			elements.push_back(data[indices[i]]);
+		}
+		return elements;
+
+	}
+
+	/** get the nearest neighbor and its distance */
+	void getNearest(const Scalar* point, size_t& idx, float& distSquared) {
+
+		// find 1-nearest-neighbors
+		tree.knnSearch(point, 1, &idx, &distSquared);
+
+	}
+
+	void get(const Scalar* point, const int numNeighbors, size_t* indices, float* squaredDist) {
+
+		// find k-nearest-neighbors
+		tree.knnSearch(point, numNeighbors, indices, squaredDist);
+
+	}
+
+};
+
+#endif // KNN_H

tests/Tests.h

@@ -6,7 +6,7 @@
 #include <gtest/gtest.h>
 
 static inline std::string getDataFile(const std::string& name) {
-	return "/mnt/firma/kunden/indoor/tests/data/" + name;
+	return "/apps/workspaces/Indoor/tests/data/" + name;
 }
 
 #endif

tests/data/fp1.svg

@@ -0,0 +1,269 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+   width="256"
+   height="256"
+   viewBox="0 0 256 256"
+   id="svg2"
+   version="1.1"
+   inkscape:version="0.91 r13725"
+   sodipodi:docname="fp1.svg">
+  <defs
+     id="defs4" />
+  <sodipodi:namedview
+     id="base"
+     pagecolor="#ffffff"
+     bordercolor="#666666"
+     borderopacity="1.0"
+     inkscape:pageopacity="0.0"
+     inkscape:pageshadow="2"
+     inkscape:zoom="5.6"
+     inkscape:cx="73.937318"
+     inkscape:cy="144.77356"
+     inkscape:document-units="px"
+     inkscape:current-layer="layer3"
+     showgrid="true"
+     units="px"
+     inkscape:object-nodes="true"
+     inkscape:window-width="1920"
+     inkscape:window-height="1021"
+     inkscape:window-x="0"
+     inkscape:window-y="0"
+     inkscape:window-maximized="1">
+    <inkscape:grid
+       type="xygrid"
+       id="grid3336"
+       empspacing="8" />
+  </sodipodi:namedview>
+  <metadata
+     id="metadata7">
+    <rdf:RDF>
+      <cc:Work
+         rdf:about="">
+        <dc:format>image/svg+xml</dc:format>
+        <dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+        <dc:title></dc:title>
+      </cc:Work>
+    </rdf:RDF>
+  </metadata>
+  <g
+     inkscape:label="1"
+     inkscape:groupmode="layer"
+     id="layer1"
+     transform="translate(0,-796.36219)"
+     style="display:none"
+     sodipodi:insensitive="true">
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 8,1044.3622 0,-240.00001 240,0 0,168 -128,0 0,72.00001 -112,0"
+       id="path3340"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="ccccccc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 8,956.36219 24,0"
+       id="path3342"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 248,852.36219 -48,0"
+       id="path3346"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 184,852.36219 -56,0"
+       id="path3348"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 112,852.36219 -72,0"
+       id="path3350"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 24,852.36219 -16,0"
+       id="path3352"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 80,852.36219 0,-48"
+       id="path3354"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 160,852.36219 0,-48"
+       id="path3356"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 248,924.36219 -64,0"
+       id="path3358"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 168,924.36219 -64,0"
+       id="path3360"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 136,924.36219 0,48"
+       id="path3362"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 88,924.36219 -32,0 0,48 64,0"
+       id="path3364"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cccc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.94280899px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 56,972.36219 0,48.00001"
+       id="path3366"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 56,1036.3622 0,8"
+       id="path3368"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 168,900.36219 -40,0 0,-24 40,0"
+       id="path3389"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cccc" />
+  </g>
+  <g
+     inkscape:groupmode="layer"
+     id="layer3"
+     inkscape:label="1_2"
+     style="display:none">
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 169,104 0,-24 -64,0"
+       id="path3441"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="ccc" />
+  </g>
+  <g
+     inkscape:groupmode="layer"
+     id="layer2"
+     inkscape:label="2"
+     style="display:inline"
+     sodipodi:insensitive="true">
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 112,104 40,0 0,-24 -40,0"
+       id="path3394"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 216,56 -72,0"
+       id="path3396"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 128,56 64,56"
+       id="path3398"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 48,56 8,56"
+       id="path3400"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 8,200 64,0"
+       id="path3402"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 88,200 56,0"
+       id="path3404"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 160,200 40,0"
+       id="path3406"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 96,200 0,48"
+       id="path3408"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 168,200 0,48"
+       id="path3410"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 72,168 -24,0"
+       id="path3416"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 32,168 8,168"
+       id="path3418"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 216,8 0,128"
+       id="path3422"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 8,8 216,8"
+       id="path3426"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 8,8 8,248"
+       id="path3428"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 216,248 8,248"
+       id="path3430"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 216,248 0,-80"
+       id="path3432"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 72,168 144,0"
+       id="path3434"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 72,168 0,-32"
+       id="path3436"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 72,136 144,0"
+       id="path3438"
+       inkscape:connector-curvature="0" />
+  </g>
+</svg>

tests/geo/TestAngle.cpp

@@ -6,15 +6,55 @@
 
 TEST(Angle, calc) {
 
-	ASSERT_EQ(0,	Angle::getDEG(0,0, +1,0));		// to the right
+	ASSERT_EQ(0,	Angle::getDEG_360(0,0, +1,0));		// to the right
-	ASSERT_EQ(90,	Angle::getDEG(0,0, 0,+1));		// upwards
+	ASSERT_EQ(90,	Angle::getDEG_360(0,0, 0,+1));		// upwards
-	ASSERT_EQ(180,	Angle::getDEG(0,0, -1,0));		// to the left
+	ASSERT_EQ(180,	Angle::getDEG_360(0,0, -1,0));		// to the left
-	ASSERT_EQ(270,	Angle::getDEG(0,0, 0,-1));		// downwards
+	ASSERT_EQ(270,	Angle::getDEG_360(0,0, 0,-1));		// downwards
 
-	ASSERT_EQ(45,	Angle::getDEG(0,0, +1,+1));		// to the upper right
+	ASSERT_EQ(45,	Angle::getDEG_360(0,0, +1,+1));		// to the upper right
-	ASSERT_EQ(135,	Angle::getDEG(0,0, -1,+1));		// to the upper left
+	ASSERT_EQ(135,	Angle::getDEG_360(0,0, -1,+1));		// to the upper left
-	ASSERT_EQ(225,	Angle::getDEG(0,0, -1,-1));		// to the lower left
+	ASSERT_EQ(225,	Angle::getDEG_360(0,0, -1,-1));		// to the lower left
-	ASSERT_EQ(315,	Angle::getDEG(0,0, +1,-1));		// to the lower right
+	ASSERT_EQ(315,	Angle::getDEG_360(0,0, +1,-1));		// to the lower right
+
+}
+
+TEST(Angle, diff) {
+
+	const float r = Angle::getRAD_2PI(0,0,	+1,0);		// to the right
+	const float u = Angle::getRAD_2PI(0,0, 0,+1);		// upwards
+	const float l = Angle::getRAD_2PI(0,0, -1,0);		// to the left
+	const float d = Angle::getRAD_2PI(0,0, 0,-1);		// downwards
+
+	const float ur = Angle::getRAD_2PI(0,0, +1,+1);		// to the upper right
+	const float ul = Angle::getRAD_2PI(0,0, -1,+1);		// to the upper left
+	const float dl = Angle::getRAD_2PI(0,0, -1,-1);		// to the lower left
+	const float dr = Angle::getRAD_2PI(0,0, +1,-1);		// to the lower right
+
+
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(r, u), 0.0001);
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(u, r), 0.0001);
+
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(u, l), 0.0001);
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(l, u), 0.0001);
+
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(l, d), 0.0001);
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(d, l), 0.0001);
+
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(d, r), 0.0001);
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(r, d), 0.0001);
+
+
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(ur, ul), 0.0001);
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(ul, ur), 0.0001);
+
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(ul, dl), 0.0001);
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(dl, ul), 0.0001);
+
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(dl, dr), 0.0001);
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(dr, dl), 0.0001);
+
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(dr, ur), 0.0001);
+	ASSERT_NEAR(M_PI_2, Angle::getDiffRAD_2PI_PI(ur, dr), 0.0001);
 
 }
 

tests/grid/GridImportance.cpp

@@ -0,0 +1,29 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#include "../../grid/factory/GridImportance.h"
+#include "../../grid/factory/GridFactory.h"
+#include "../../floorplan/FloorplanFactorySVG.h"
+
+#include "Plot.h"
+
+TEST(GridImportance, a) {
+
+
+
+	Grid<20, GP> g;
+	GridFactory<20, GP> gf(g);
+	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
+
+	Floor f1 = fpf.getFloor("1");
+	gf.addFloor(f1, 20);
+	gf.removeIsolated();
+
+	GridImportance gi;
+	gi.addImportance(g, 20);
+
+	plot(g);
+
+}
+
+#endif

tests/grid/Plot.h

@@ -0,0 +1,66 @@
+#ifndef PLOT_H
+#define PLOT_H
+
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+
+
+class GP : public GridNode, public GridPoint {
+public:
+	float imp = 1.0f;
+public:
+	GP() : GridNode(), GridPoint() {;}
+	GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
+
+};
+
+template <int gridSize_cm, typename T> void plot(Grid<gridSize_cm, T>& g) {
+
+	K::Gnuplot gp;
+	K::GnuplotSplot splot;
+	K::GnuplotSplotElementColorPoints points;
+	K::GnuplotSplotElementLines lines;
+
+	gp << "set ticslevel 0\n";
+	gp << "set view equal xyz\n";
+	gp << "set cbrange[0.5:1.0]\n";
+	gp << "set palette gray negative\n";
+
+	std::set<uint64_t> done;
+
+	int cnt = 0;
+	for (int i = 0; i < g.getNumNodes(); ++i) {
+		const GP& n1 = g[i];
+		points.add(K::GnuplotPoint3(n1.x_cm, n1.y_cm, n1.z_cm), n1.imp);
+
+		for (int n = 0; n < n1.getNumNeighbors(); ++n) {
+			const GP& n2 = n1.getNeighbor(n, g);
+			if (done.find(g.getUID(n2)) == done.end()) {
+				K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
+				K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
+				lines.addSegment(p1, p2);
+				++cnt;
+			}
+		}
+
+		done.insert(g.getUID(n1));
+
+	}
+
+	points.setPointSize(1);
+	//splot.add(&lines);
+	splot.add(&points);
+
+	gp.draw(splot);
+	gp.flush();
+
+	sleep(100);
+
+}
+
+
+#endif // PLOT_H

tests/grid/TestBBox.cpp

@@ -27,20 +27,20 @@ TEST(BBox, intersect) {
 	GridNodeBBox bb1(GridPoint(20,20,20), 20);
 
 	// left
-	ASSERT_FALSE(bb1.intersects( Line2D(9, -999, 9, +999) ));
+	ASSERT_FALSE(bb1.intersects( Line2(9, -999, 9, +999) ));
-	ASSERT_TRUE (bb1.intersects( Line2D(11, -999, 11, +999) ));
+	ASSERT_TRUE (bb1.intersects( Line2(11, -999, 11, +999) ));
 
 	// right
-	ASSERT_TRUE (bb1.intersects( Line2D(29, -999, 29, +999) ));
+	ASSERT_TRUE (bb1.intersects( Line2(29, -999, 29, +999) ));
-	ASSERT_FALSE(bb1.intersects( Line2D(31, -999, 31, +999) ));
+	ASSERT_FALSE(bb1.intersects( Line2(31, -999, 31, +999) ));
 
 	// top
-	ASSERT_FALSE(bb1.intersects( Line2D(-999, 9, +999, 9) ));
+	ASSERT_FALSE(bb1.intersects( Line2(-999, 9, +999, 9) ));
-	ASSERT_TRUE (bb1.intersects( Line2D(-999, 11, +999, 11) ));
+	ASSERT_TRUE (bb1.intersects( Line2(-999, 11, +999, 11) ));
 
 	// bottom
-	ASSERT_TRUE (bb1.intersects( Line2D(-999, 29, +999, 29) ));
+	ASSERT_TRUE (bb1.intersects( Line2(-999, 29, +999, 29) ));
-	ASSERT_FALSE(bb1.intersects( Line2D(-999, 31, +999, 31) ));
+	ASSERT_FALSE(bb1.intersects( Line2(-999, 31, +999, 31) ));
 
 }
 

tests/grid/TestGrid.cpp

@@ -36,15 +36,15 @@ TEST(Grid, BBox) {
 
 	Grid<20, GP> grid;
 	int idx = grid.add(GP(40,40,40));
-	ASSERT_EQ(30, grid.getBBox(idx).x1_cm);
+	ASSERT_EQ(30, grid.getBBox(idx).getMin().x);
-	ASSERT_EQ(50, grid.getBBox(idx).x2_cm);
+	ASSERT_EQ(50, grid.getBBox(idx).getMax().x);
-	ASSERT_EQ(30, grid.getBBox(idx).y1_cm);
+	ASSERT_EQ(30, grid.getBBox(idx).getMin().y);
-	ASSERT_EQ(50, grid.getBBox(idx).y2_cm);
+	ASSERT_EQ(50, grid.getBBox(idx).getMax().y);
 
 }
 
 
-TEST(Grid, neighbors) {
+TEST(Grid, connectBiDir) {
 
 	Grid<1, GP> grid;
 
@@ -54,10 +54,10 @@ TEST(Grid, neighbors) {
 	int idx4 = grid.add(GP( 1, 0, 0));
 	int idx5 = grid.add(GP(-1, 0, 0));
 
-	grid.connect(idx1, idx2);
+	grid.connectBiDir(idx1, idx2);
-	grid.connect(idx1, idx3);
+	grid.connectBiDir(idx1, idx3);
-	grid.connect(idx1, idx4);
+	grid.connectBiDir(idx1, idx4);
-	grid.connect(idx1, idx5);
+	grid.connectBiDir(idx1, idx5);
 
 	ASSERT_EQ(4, grid.getNumNeighbors(idx1));
 	ASSERT_EQ(1, grid.getNumNeighbors(idx2));
@@ -65,6 +65,54 @@ TEST(Grid, neighbors) {
 	ASSERT_EQ(1, grid.getNumNeighbors(idx4));
 	ASSERT_EQ(1, grid.getNumNeighbors(idx5));
 
+	ASSERT_EQ(grid[idx2], grid.getNeighbor(idx1, 0));
+	ASSERT_EQ(grid[idx3], grid.getNeighbor(idx1, 1));
+	ASSERT_EQ(grid[idx4], grid.getNeighbor(idx1, 2));
+	ASSERT_EQ(grid[idx5], grid.getNeighbor(idx1, 3));
+
+}
+
+TEST(Grid, disconnectBiDir) {
+
+	Grid<1, GP> grid;
+
+	int idx1 = grid.add(GP( 0, 0, 0));
+	int idx2 = grid.add(GP( 0, 1, 0));
+	int idx3 = grid.add(GP( 0,-1, 0));
+	int idx4 = grid.add(GP( 1, 0, 0));
+	int idx5 = grid.add(GP(-1, 0, 0));
+
+	grid.connectBiDir(idx1, idx2);
+	grid.connectBiDir(idx1, idx3);
+	grid.connectBiDir(idx1, idx4);
+	grid.connectBiDir(idx1, idx5);
+
+	// remove 1 <-> 4
+	grid.disconnectBiDir(idx1, idx4);
+	ASSERT_EQ(3, grid.getNumNeighbors(idx1));
+	ASSERT_EQ(0, grid.getNumNeighbors(idx4));
+	ASSERT_EQ(grid[idx2], grid.getNeighbor(idx1, 0));
+	ASSERT_EQ(grid[idx3], grid.getNeighbor(idx1, 1));
+	ASSERT_EQ(grid[idx5], grid.getNeighbor(idx1, 2));
+
+	// remove 1 <-> 5
+	grid.disconnectBiDir(idx1, idx5);
+	ASSERT_EQ(2, grid.getNumNeighbors(idx1));
+	ASSERT_EQ(0, grid.getNumNeighbors(idx5));
+	ASSERT_EQ(grid[idx2], grid.getNeighbor(idx1, 0));
+	ASSERT_EQ(grid[idx3], grid.getNeighbor(idx1, 1));
+
+	// remove 1 <-> 2
+	grid.disconnectBiDir(idx1, idx2);
+	ASSERT_EQ(1, grid.getNumNeighbors(idx1));
+	ASSERT_EQ(0, grid.getNumNeighbors(idx2));
+	ASSERT_EQ(grid[idx3], grid.getNeighbor(idx1, 0));
+
+	// remove 1 <-> 2
+	grid.disconnectBiDir(idx1, idx3);
+	ASSERT_EQ(0, grid.getNumNeighbors(idx1));
+	ASSERT_EQ(0, grid.getNumNeighbors(idx3));
+
 }
 
 TEST(Grid, uid) {
@@ -81,6 +129,87 @@ TEST(Grid, uid) {
 
 }
 
+TEST(Grid, remove) {
+
+	Grid<1, GP> grid;
+
+	GP gp1( 0, 0, 0);
+	GP gp2( 0, 1, 0);
+	GP gp3( 0,-1, 0);
+	GP gp4( 1, 0, 0);
+	GP gp5(-1, 0, 0);
+
+	int idx1 = grid.add(gp1);
+	int idx2 = grid.add(gp2);
+	int idx3 = grid.add(gp3);
+	int idx4 = grid.add(gp4);
+	int idx5 = grid.add(gp5);
+
+	grid.connectBiDir(idx1, idx2);
+	grid.connectBiDir(idx1, idx3);
+	grid.connectBiDir(idx1, idx4);
+	grid.connectBiDir(idx1, idx5);
+
+	grid.remove(idx1);
+	grid.cleanup();
+
+	ASSERT_EQ(4, grid.getNumNodes());
+	ASSERT_EQ(gp2, grid[0]);
+	ASSERT_EQ(gp3, grid[1]);
+	ASSERT_EQ(gp4, grid[2]);
+	ASSERT_EQ(gp5, grid[3]);
+
+	ASSERT_EQ(0, grid.getNumNeighbors(grid[0]));
+	ASSERT_EQ(0, grid.getNumNeighbors(grid[1]));
+	ASSERT_EQ(0, grid.getNumNeighbors(grid[2]));
+	ASSERT_EQ(0, grid.getNumNeighbors(grid[3]));
+
+}
+
+TEST(Grid, neighborIter) {
+
+	Grid<1, GP> grid;
+
+	int idx1 = grid.add(GP( 0, 0, 0));
+	int idx2 = grid.add(GP( 0, 1, 0));
+	int idx3 = grid.add(GP( 0,-1, 0));
+	int idx4 = grid.add(GP( 1, 0, 0));
+	int idx5 = grid.add(GP(-1, 0, 0));
+
+	grid.connectBiDir(idx1, idx2);
+	grid.connectBiDir(idx1, idx3);
+	grid.connectBiDir(idx1, idx4);
+	grid.connectBiDir(idx1, idx5);
+
+	int i = 0;
+	for (GP& node : grid.neighbors(idx1)) {++i;}
+	ASSERT_EQ(4, i);
+
+}
+
+TEST(Grid, bbox) {
+
+	Grid<1, GP> grid;
+
+	int idx1 = grid.add(GP( 0, 0, 0));
+	int idx2 = grid.add(GP( 0, 1, 0));
+	int idx3 = grid.add(GP( 0,-1, 0));
+	int idx4 = grid.add(GP( 1, 0, 0));
+	int idx5 = grid.add(GP(-1, 0, 0));
+
+	BBox3 bb = grid.getBBox();
+
+	ASSERT_EQ(bb.getMin().x, -1);
+	ASSERT_EQ(bb.getMin().y, -1);
+	ASSERT_EQ(bb.getMin().z,  0);
+
+	ASSERT_EQ(bb.getMax().x, +1);
+	ASSERT_EQ(bb.getMax().y, +1);
+	ASSERT_EQ(bb.getMax().z,  0);
+
+
+}
+
 TEST(Grid, nearest) {
 
 	Grid<20, GP> grid;

tests/grid/TestGridFactory.cpp

@@ -4,42 +4,35 @@
 #include "../../grid/factory/GridFactory.h"
 #include "../../floorplan/FloorplanFactorySVG.h"
 
-#include <KLib/misc/gnuplot/Gnuplot.h>
-#include <KLib/misc/gnuplot/GnuplotSplot.h>
-#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
 
-class GP : public GridNode, public GridPoint {
-public:
-	GP() : GridNode(), GridPoint() {;}
-	GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
 
-};
+#include "Plot.h"
 
 TEST(GridFactory, create) {
 
 	Grid<20, GP> g;
+	Grid<20, GP> gInv;
+
 	GridFactory<20, GP> gf(g);
-	FloorplanFactorySVG fpf(getDataFile("test.svg"), 2);
+	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 4);
 	Floor f1 = fpf.getFloor("1");
+	Floor f2 = fpf.getFloor("2");
+	Stairs s1_2 = fpf.getStairs("1_2");
 
-	gf.addFloor(f1, 0);
+	gf.addFloor(f1, 20);
+	gf.addFloor(f2, 340);
+	gf.addStairs(s1_2, 20, 340);
+	gf.removeIsolated();
 
-	K::Gnuplot gp;
+	GridFactory<20, GP> gfInv(gInv);
-	K::GnuplotSplot splot;
+	gfInv.addInverted(g, 20);
-	K::GnuplotSplotElementPoints points;
+	gfInv.addInverted(g, 340);
 
-	for (int i = 0; i < g.getNumNodes(); ++i) {
+//	plot(gInv);
-		const GP& node = g[i];
-		points.add(K::GnuplotPoint3(node.x_cm, node.y_cm, node.z_cm));
-	}
-
-	splot.add(&points);
-	gp.draw(splot);
-	gp.flush();
-
-	sleep(10);
 
 
 }
 
+
+
 #endif