initial version

2016-01-21 11:10:55 +01:00
parent 8818a9b216
commit a7dc0cabbb
21 changed files with 1397 additions and 0 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -0,0 +1,87 @@
+# Usage:
+#  Create build folder, like RC-build next to RobotControl and WifiScan folder
+#  CD into build folder and execute 'cmake -DCMAKE_BUILD_TYPE=Debug ../RobotControl'
+#  make
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+
+# select build type
+SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" )
+
+PROJECT(graphModel)
+
+IF(NOT CMAKE_BUILD_TYPE)
+	MESSAGE(STATUS "No build type selected. Default to Debug")
+	SET(CMAKE_BUILD_TYPE "Debug")
+ENDIF()
+
+
+
+INCLUDE_DIRECTORIES(
+	../
+	/mnt/firma/kunden/HandyGames/
+)
+
+
+FILE(GLOB HEADERS
+	./*.h
+	./*/*.h
+	./*/*/*.h
+	./*/*/*/*.h
+	./*/*/*/*/*.h
+	./*/*/*/*/*/*.h
+)
+
+FILE(GLOB SOURCES
+	./*.cpp
+	./*/*.cpp
+	./*/*/*.cpp
+	/mnt/firma/kunden/HandyGames/KLib/inc/tinyxml/*.cpp
+)
+
+
+if(${CMAKE_GENERATOR} MATCHES "Visual Studio")
+
+  SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /D_X86_ /D_USE_MATH_DEFINES")
+  SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox /D_X86_ /D_USE_MATH_DEFINES")
+  SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /DEBUG")
+  SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG /INCREMENTAL:NO")
+
+  set(CMAKE_CONFIGURATION_TYPES Release Debug)
+
+else()
+
+# system specific compiler flags
+ADD_DEFINITIONS(
+
+    -std=gnu++11
+
+	-Wall
+	-Werror=return-type
+	-Wextra
+
+	-g
+	-O0
+
+	-DWITH_TESTS
+
+)
+
+endif()
+
+# build a binary file
+ADD_EXECUTABLE(
+	${PROJECT_NAME}
+	${HEADERS}
+	${SOURCES}
+)
+
+# needed external libraries
+TARGET_LINK_LIBRARIES(
+	${PROJECT_NAME}
+	gtest
+	pthread
+)
+
+SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER})
+
--- a/Exception.h
+++ b/Exception.h
@@ -0,0 +1,23 @@
+#ifndef EXCEPTION_H
+#define EXCEPTION_H
+
+#include <exception>
+#include <string>
+
+class Exception : public std::exception {
+
+private:
+
+	/** the exception message */
+	std::string str;
+
+public:
+
+	/** ctor */
+	Exception(const std::string& str) : str(str) {;}
+
+	const char* what() const throw() {return str.c_str();}
+
+};
+
+#endif // EXCEPTION_H
--- a/floorplan/Floor.h
+++ b/floorplan/Floor.h
@@ -0,0 +1,49 @@
+#ifndef FLOOR_H
+#define FLOOR_H
+
+#include <vector>
+#include "../geo/Line2D.h"
+
+/**
+ * represents one floor by describing all contained obstacles
+ */
+class Floor {
+
+private:
+
+
+	/** all obstacles within the floor */
+	std::vector<Line2D> lines;
+
+	/** total width of the floor (in meter) */
+	double width_cm;
+
+	/** total depth of the floor (in meter) */
+	double depth_cm;
+
+
+public:
+
+	/** ctor */
+	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 {
+		return lines;
+	}
+
+	/** add a new obstacle to this floor */
+	void addObstacle(const Line2D& l ) {
+		lines.push_back(l);
+	}
+
+	/** get the floorplan's total width */
+	float getWidth_cm() const {return width_cm;}
+
+	/** get the floorplan's total depth */
+	float getDepth_cm() const {return depth_cm;}
+
+
+};
+
+#endif // FLOOR_H
--- a/floorplan/FloorplanFactorySVG.h
+++ b/floorplan/FloorplanFactorySVG.h
@@ -0,0 +1,138 @@
+#ifndef FLOORPLANFACTORYSVG_H
+#define FLOORPLANFACTORYSVG_H
+
+#include "Floor.h"
+#include <string>
+#include "../Exception.h"
+
+#include <KLib/gfx/svg/SVGLoader.h>
+
+/**
+ * load floorplans from SVG files
+ */
+class FloorplanFactorySVG {
+
+	/**
+	 * helper class for SVG floorplans.
+	 *
+	 * converts between the SVG's scale and real-world scale
+	 */
+	class SVGScaler {
+
+	private:
+
+		/** the scaling factor to apply to the svg data */
+		const double scalingFactor;
+
+	public:
+
+		/** ctor */
+		SVGScaler(const double scalingFactor) : scalingFactor(scalingFactor) {
+			;
+		}
+
+		/** scale (x, y) into (_x, _y) */
+		void scale(const double x, const double y, double& _x, double& _y) const {
+			_x = x * scalingFactor;
+			_y = y * scalingFactor;
+		}
+
+		/** scale the given point into a new output point */
+		K::Point scale(const K::Point p) const {
+			K::Point ret;
+			scale (p.x, p.y, ret.x, ret.y);
+			return ret;
+		}
+
+		/** scale the given line into a new output line */
+		Line2D scale(const K::Line l) const {
+			Line2D 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;
+		}
+
+	};
+
+private:
+
+	K::SVGFile svg;
+	SVGScaler scaler;
+	double width;
+	double depth;
+
+public:
+
+	/** ctor with svg filename and scaling factor */
+	FloorplanFactorySVG(const std::string& file, const float scaling) : scaler(scaling) {
+
+		// load the SVG
+		K::SVGLoader::load(K::File(file), &svg);
+		scaler.scale(svg.getWidth(), svg.getHeight(), width, depth);
+
+	}
+
+	/** get the width of the building */
+	float getWidth() const {return width;}
+
+	/** get the depth of the building */
+	float getDepth() const {return depth;}
+
+	/** get the floor identified by the given layer name */
+	Floor getFloor(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
+		Floor floor(width, depth);
+		load(layer, floor);
+		return floor;
+
+
+	}
+
+private:
+
+	/** recursive loading/parsing of nested SVG elements */
+	void load(K::SVGElement* el, Floor& floor) {
+
+		switch (el->getType()) {
+
+			case SVGElementType::COMPOSITE: {
+				for (K::SVGElement* sub : ((K::SVGComposite*)el)->getChilds()) {
+					load(sub, floor);
+				}
+				break;
+			}
+
+			case SVGElementType::LAYER: {
+				K::SVGLayer* layer = (K::SVGLayer*) el;
+				for (K::SVGElement* sub : layer->getChilds()) {
+					load(sub, floor);
+				}
+				break;
+			}
+
+			case SVGElementType::PATH: {
+				for (const K::Line& line : ((K::SVGPath*)el)->getLines()) {
+					floor.addObstacle( scaler.scale(line) );
+				}
+				break;
+			}
+
+			case SVGElementType::TEXT: {
+				break;
+			}
+
+			default:
+				throw "should not happen!";
+
+		}
+	}
+
+};
+
+#endif // FLOORPLANFACTORYSVG_H
--- a/geo/Angle.h
+++ b/geo/Angle.h
@@ -0,0 +1,33 @@
+#ifndef ANGLE_H
+#define ANGLE_H
+
+#include <cmath>
+
+class Angle {
+
+public:
+
+	/** get the radians from (x1,y1) to (x2,y2) */
+	static float getRAD(const float x1, const float y1, const float x2, const float 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) */
+	static float getDEG(const float x1, const float y1, const float x2, const float y2) {
+		return radToDeg(getRAD(x1,y1,x2,y2));
+	}
+
+	/** convert degrees to radians */
+	static float degToRad(const float deg) {
+		return deg / 180 * M_PI;
+	}
+
+	/** convert radians to degrees */
+	static float radToDeg(const float rad) {
+		return rad * 180 / M_PI;
+	}
+
+};
+
+#endif // ANGLE_H
--- a/geo/Line2D.h
+++ b/geo/Line2D.h
@@ -0,0 +1,21 @@
+#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
--- a/geo/Units.h
+++ b/geo/Units.h
@@ -0,0 +1,16 @@
+#ifndef UNITS_H
+#define UNITS_H
+
+class Units {
+
+public:
+
+	/** convert from m to cm */
+	template <typename T> static inline T mToCm(const T m) {return m * 100;}
+
+	/** convert from cm to m */
+	template <typename T> static inline T cmToM(const T cm) {return cm / 100;}
+
+};
+
+#endif // UNITS_H
--- a/grid/Grid.h
+++ b/grid/Grid.h
@@ -0,0 +1,141 @@
+#ifndef GRID_H
+#define GRID_H
+
+#include <vector>
+#include <unordered_map>
+
+#include "../Exception.h"
+#include "GridPoint.h"
+#include "GridNode.h"
+#include <iostream>
+
+/**
+ * grid of the given grid-size, storing some value which
+ * extends GridPoint
+ */
+template <int gridSize_cm, typename T> class Grid {
+
+typedef uint64_t UID;
+
+private:
+
+	/** all elements (nodes) within the grid */
+	std::vector<T> nodes;
+
+	/** UID -> index mapping */
+	std::unordered_map<UID, int> hashes;
+
+public:
+
+	/** ctor */
+	Grid() {
+		static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
+		static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
+	}
+
+	/** no-copy */
+	Grid(const Grid& o) = delete;
+
+	/** no-assign */
+	void operator = (const Grid& o) = delete;
+
+
+	/**
+	 * 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
+		const int idx = nodes.size();					// next free index
+		const UID uid = getUID(elem);					// get the UID for this new element
+		nodes.push_back(elem);							// add it to the grid
+		nodes.back()._idx = idx;
+		hashes[uid] = idx;								// add an UID->index lookup
+		return idx;										// done
+	}
+
+	/**
+	 * connect (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);
+	}
+
+	/**
+	 * 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;
+	}
+
+	/** 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 center-node the given Point belongs to */
+	const T& getNodeFor(const GridPoint& p) {
+		const UID uid = getUID(p);
+		if (hashes.find(uid) == hashes.end()) {throw Exception("element not found!");}
+		return nodes[hashes[uid]];
+	}
+
+	/** get the BBox for the given node */
+	GridNodeBBox getBBox(const int idx) const {
+		return getBBox(nodes[idx]);
+	}
+
+	/** get the BBox for the given node */
+	GridNodeBBox getBBox(const T& node) const {
+		return GridNodeBBox(node, gridSize_cm);
+	}
+
+	/**
+	 * get an UID for the given point.
+	 * this works only for aligned points.
+	 *
+	 */
+	UID getUID(const GridPoint& p) const {
+		const uint64_t x = std::round(p.x_cm / gridSize_cm);
+		const uint64_t y = std::round(p.y_cm / gridSize_cm);
+		const uint64_t z = std::round(p.z_cm / gridSize_cm);
+		return (z << 40) | (y << 20) | (x << 0);
+	}
+
+	/** array access */
+	const T& operator [] (const int idx) const {
+		return nodes[idx];
+	}
+
+private:
+
+	/** asssert that the given element is aligned to the grid */
+	void assertAligned(const T& elem) {
+		if (((int)elem.x_cm % gridSize_cm) != 0) {throw Exception("element's x is not aligned!");}
+		if (((int)elem.y_cm % gridSize_cm) != 0) {throw Exception("element's y is not aligned!");}
+		if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");}
+	}
+
+};
+
+#endif // GRID_H
--- a/grid/GridNode.h
+++ b/grid/GridNode.h
@@ -0,0 +1,34 @@
+#ifndef GRIDNODE_H
+#define GRIDNODE_H
+
+#include "GridNodeBBox.h"
+#include "GridPoint.h"
+
+/**
+ * INTERNAL DATASTRUCTURE
+ * this data-structure is internally used by the Grid
+ * to store additional information for each node besides
+ * the user's requested data-structure
+ */
+class GridNode {
+
+	template<int, typename> friend class Grid;
+
+	/** INTERNAL: array-index */
+	int _idx = -1;
+
+	/** INTERNAL: store neighbors (via index) */
+	int _numNeighbors = 0;
+
+	/** INTERNAL: number of neighbors */
+	int _neighbors[10] = {};
+
+public:
+
+	GridNode() {;}
+
+
+
+};
+
+#endif // GRIDNODE_H
--- a/grid/GridNodeBBox.h
+++ b/grid/GridNodeBBox.h
@@ -0,0 +1,46 @@
+#ifndef GRIDNODEBBOX_H
+#define GRIDNODEBBOX_H
+
+#include "GridPoint.h"
+#include "../geo/Line2D.h"
+
+/**
+ * describes the 2D (one floor)
+ * bounding-box for one node on the grid
+ */
+struct GridNodeBBox {
+
+	/** 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
+		y1_cm = center.y_cm - gridSize_cm/2;
+		x2_cm = center.x_cm + gridSize_cm/2;	// larger half
+		y2_cm = 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);
+	}
+
+};
+
+#endif // GRIDNODEBBOX_H
--- a/grid/GridPoint.h
+++ b/grid/GridPoint.h
@@ -0,0 +1,41 @@
+#ifndef GRIDPOINT_H
+#define GRIDPOINT_H
+
+#include <cmath>
+
+struct GridPoint {
+
+	/** x-position (in centimeter) */
+	const float x_cm;
+
+	/** y-position (in centimeter) */
+	const float y_cm;
+
+	/** z-position (in centimeter) */
+	const float z_cm;
+
+
+	/** empty ctor */
+	GridPoint() : x_cm(0), y_cm(0), z_cm(0) {;}
+
+	/** ctor */
+	GridPoint(const float x_cm, const float y_cm, const float z_cm) : x_cm(x_cm), y_cm(y_cm), z_cm(z_cm) {;}
+
+
+	/** equal? */
+	bool operator == (const GridPoint& o) const {
+		return x_cm == o.x_cm && y_cm == o.y_cm && z_cm == o.z_cm;
+	}
+
+	/** get the distance (in meter) betwen this and the given point */
+	float getDistanceInMeter(const GridPoint& other) const {
+		const int dx = x_cm - other.x_cm;
+		const int dy = y_cm - other.y_cm;
+		const int dz = z_cm - other.z_cm;
+		return std::sqrt(dx*dx + dy*dy + dz*dz) / 100.0f;
+	}
+
+
+};
+
+#endif // GRIDPOINT_H
--- a/grid/factory/GridFactory.h
+++ b/grid/factory/GridFactory.h
@@ -0,0 +1,55 @@
+#ifndef GRIDFACTORY_H
+#define GRIDFACTORY_H
+
+#include <string>
+#include "../../floorplan/Floor.h"
+#include "../../geo/Units.h"
+#include "../GridNodeBBox.h"
+#include "../Grid.h"
+
+template <int gridSize_cm, typename T> class GridFactory {
+
+private:
+
+	Grid<gridSize_cm, T>& grid;
+
+
+public:
+
+	/** ctor with the grid to fill */
+	GridFactory(Grid<gridSize_cm, T>& grid) : grid(grid) {;}
+
+	/** add the given floor at the provided height (in cm) */
+	void addFloor(const Floor& floor, const float z_cm) {
+
+		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) {
+
+				// check intersection with the floorplan
+				GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), gridSize_cm);
+				if (intersects(bbox, floor)) {continue;}
+
+				// add to the grid
+				grid.add(T(x_cm, y_cm, z_cm));
+
+			}
+		}
+
+		int i = 0;
+
+	}
+
+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()) {
+			if (bbox.intersects(l)) {return true;}
+		}
+		return false;
+	}
+
+
+};
+
+#endif // GRIDFACTORY_H
--- a/main.cpp
+++ b/main.cpp
@@ -0,0 +1,25 @@
+
+#include "grid/Grid.h"
+#include "grid/GridPoint.h"
+
+class Test : public GridPoint {
+
+
+
+};
+
+#include "tests/Tests.h"
+
+int main(int argc, char** argv) {
+
+
+#ifdef WITH_TESTS
+	::testing::InitGoogleTest(&argc, argv);
+	//::testing::GTEST_FLAG(filter) = "*first*";
+	return RUN_ALL_TESTS();
+#endif
+
+
+	return 0;
+
+}
--- a/tests/Tests.h
+++ b/tests/Tests.h
@@ -0,0 +1,14 @@
+#ifndef TESTS_H
+#define TESTS_H
+
+#ifdef WITH_TESTS
+
+#include <gtest/gtest.h>
+
+static inline std::string getDataFile(const std::string& name) {
+	return "/mnt/firma/kunden/indoor/tests/data/" + name;
+}
+
+#endif
+
+#endif // TESTS_H
--- a/tests/data/test.svg
+++ b/tests/data/test.svg
@@ -0,0 +1,402 @@
+<?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="512"
+   height="512"
+   id="svg3269"
+   version="1.1"
+   inkscape:version="0.48.4 r9939"
+   sodipodi:docname="test.svg">
+  <defs
+     id="defs3271" />
+  <sodipodi:namedview
+     id="base"
+     pagecolor="#ffffff"
+     bordercolor="#666666"
+     borderopacity="1.0"
+     inkscape:pageopacity="0.0"
+     inkscape:pageshadow="2"
+     inkscape:zoom="0.98994949"
+     inkscape:cx="145.84558"
+     inkscape:cy="284.04468"
+     inkscape:document-units="px"
+     inkscape:current-layer="layer16"
+     showgrid="false"
+     inkscape:window-width="1366"
+     inkscape:window-height="672"
+     inkscape:window-x="0"
+     inkscape:window-y="33"
+     inkscape:window-maximized="1" />
+  <metadata
+     id="metadata3274">
+    <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 />
+      </cc:Work>
+    </rdf:RDF>
+  </metadata>
+  <g
+     inkscape:groupmode="layer"
+     id="layer2"
+     inkscape:label="1"
+     style="display:inline">
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 204.28571,352.71429 141.42858,1.42857"
+       id="path4093"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 339.28571,372.71429 -0.71428,-71.42858"
+       id="path4095"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 329.28571,302.71429 84.28572,75.71428"
+       id="path4097"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 423.57143,363.42857 -67.85714,59.28572"
+       id="path4099"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 362.85714,441.28571 -5.71428,-45.71428"
+       id="path4101"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 365.71429,397 222.85714,407"
+       id="path4103"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 235.71429,429.85714 222.14286,333.42857"
+       id="path4105"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 195,109.14286 320,237.71429"
+       id="path4107"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 297.14286,237.71429 424.66569,149.83612"
+       id="path4109"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 417.24799,166.28571 370.71429,54.142857"
+       id="path4111"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 390.71429,49.857142 345,151.28571"
+       id="path4113"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 366.0522,143.55112 183.57143,119.85714"
+       id="path4115"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 40.714286,63.428571 74.285714,190.57143"
+       id="path4117"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 58.571429,184.14286 130.71429,92.714285"
+       id="path4119"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 146.42857,109.85714 35.714286,69.142857"
+       id="path4121"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 26.428571,295.57143 133.57143,202.71429"
+       id="path4123"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 118.57143,194.85714 50,87.85715"
+       id="path4125"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 150.71429,289.85714 75.71428,-50"
+       id="path4127"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 212.85714,238.42857 35,42.14286"
+       id="path4129"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 255,269.85714 155,326.28571"
+       id="path4131"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 166.42857,309.85714 143.57143,452"
+       id="path4133"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 165.71429,447 59.683107,422.43945"
+       id="path4135"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 77.591012,449.2654 32.857143,277"
+       id="path4137"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cc" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 6.0609153,9.954185 10.101525,502.90863"
+       id="path4139"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m -7.0710678,495.83756 526.2894778,2.0203"
+       id="path4141"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 507.09658,520.08122 503.05597,-4.1879507"
+       id="path4143"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 527.29963,10.964338 -17.172593,16.0151"
+       id="path4145"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 361.63461,259.46186 102.02541,59.599"
+       id="path3780"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 353.55339,277.64461 509.11688,207.94408"
+       id="path3782"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 445.47727,326.13193 498.00521,202.89332"
+       id="path3784"
+       inkscape:connector-curvature="0" />
+  </g>
+  <g
+     inkscape:groupmode="layer"
+     id="layer15"
+     inkscape:label="2"
+     style="display:inline">
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 29.294424,91.77654 139.40105,29.147082"
+       id="path13690"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 117.1777,22.076014 43.43656,83.842666"
+       id="path13692"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 27.274119,80.664862 180.81731,98.847608"
+       id="path13694"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 33.335034,129.15218 164.65487,159.45676"
+       id="path13696"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 151.52288,143.29432 36.36549,108.08632"
+       id="path13698"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 168.69548,249.36034 119.198,-19.1929"
+       id="path13700"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 271.73104,224.10652 19.19289,66.67007"
+       id="path13702"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 318.19805,274.61415 127.27922,335.2233"
+       id="path13704"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 144.45181,347.34513 38.385797,124.10142"
+       id="path13706"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 248.49753,73.593794 279.81226,185.72073"
+       id="path13708"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 257.5889,183.70042 406.08132,149.35524"
+       id="path13710"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 240.41631,79.654709 375.77675,59.451658"
+       id="path13712"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 363.65492,48.33998 41.41625,115.15739"
+       id="path13714"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 366.68538,465.53298 446.48743,354.4162"
+       id="path13716"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 425.27422,352.3959 48.48733,91.92388"
+       id="path13718"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 359.61431,444.31978 484.87322,434.21825"
+       id="path13720"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 38.385797,387.75124 161.62441,483.71573"
+       id="path13722"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 43.43656,395.83246 136.37059,1.01015"
+       id="path13724"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 159.6041,383.71063 61.61931,62.62945"
+       id="path13726"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 148.49242,487.75634 84.85282,-57.5787"
+       id="path13728"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 269.71073,361.48727 20.20305,51.51778"
+       id="path13730"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 261.62951,371.5888 74.75129,-3.03046"
+       id="path13732"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 273.75134,420.07612 60.60915,-69.70053"
+       id="path13734"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 383.85797,231.17759 473.76155,154.406"
+       id="path13736"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 458.60926,156.4263 33.33503,83.84266"
+       id="path13738"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 445.47727,292.7969 493.9646,223.09637"
+       id="path13740"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 384.86812,217.03546 79.80205,80.8122"
+       id="path13742"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 433.35544,23.086167 59.599,2.020305"
+       id="path13744"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 484.87322,21.065862 -3.03045,45.456864"
+       id="path13746"
+       inkscape:connector-curvature="0" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 438.40621,17.025251 58.58884,49.497475"
+       id="path13748"
+       inkscape:connector-curvature="0" />
+  </g>
+  <g
+     inkscape:groupmode="layer"
+     id="layer16"
+     inkscape:label="1_2">
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="M 406.08132,60.461811 443.45697,135.2131 476.792,119.05066 439.41636,51.370438 406.08132,60.461811"
+       id="path13751"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="ccccc" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 243.44676,440.27917 8.08122,42.42641 80.81221,-2.02031 1.01015,-40.4061 -89.90358,0"
+       id="path13753"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="ccccc" />
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 185.86807,167.53798 12.12183,51.51778 41.41625,-10.10152 -14.14213,-51.51778 -39.39595,10.10152"
+       id="path13755"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="ccccc" />
+  </g>
+  <g
+     inkscape:groupmode="layer"
+     id="layer1"
+     inkscape:label="path"
+     style="display:none">
+    <path
+       style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+       d="m 23.233509,328.15223 5.498667,29.84991 5.04738,38.94466 5.726216,27.35837 6.778541,32.38611 10.28423,17.93307 19.192898,6.06092 21.213204,-2.0203 21.213205,1.01015 26.26396,0 24.24367,-9.09137 15.15228,-16.16245 6.06092,-22.22335 -2.02031,-26.26397 0,-25.25381 3.03046,-27.27412 6.06092,-18.18275 20.20305,-19.19289 28.28427,-11.11168 25.25381,-6.06092 19.1929,-14.14213 -4.04061,-28.28427 -13.13198,-27.27412 -22.22336,-11.11168 -24.24366,-6.06092 -24.24366,-12.12183 -20.20305,-13.13198 -11.11168,-22.22336 -3.03046,-25.25381 8.08122,-32.32488 13.13199,-23.23351 25.25381,-16.162441 26.26397,-14.142136 27.27412,-7.071068 27.27411,-4.04061 35.35534,-5.050763 25.25382,-7.071068 27.27412,-2.020305 20.20305,13.131983 23.23351,16.162441 27.27411,12.121831 22.22336,15.152288 5.05076,30.304578 -6.43065,37.00542 -18.34108,26.88819 -31.1166,16.74083 -31.95699,18.46513 -36.0808,17.41488 -23.9052,28.67296 10.79173,29.03585 29.28066,13.55625 26.653,8.13847 27.19771,14.80725 13.80025,29.23264 -4.56452,29.55883 -18.1487,24.13104 -18.94412,26.82052 -22.51132,18.63377 -32.37271,9.74766 -30.9338,4.32323 -31.41691,-1.63983"
+       id="path3778"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc" />
+  </g>
+</svg>
--- a/tests/floorplan/TestFloorplanFactorySVG.cpp
+++ b/tests/floorplan/TestFloorplanFactorySVG.cpp
@@ -0,0 +1,27 @@
+#ifdef WITH_TESTS
+
+//#include "../Tests.h"
+//#include "../../floorplan/FloorplanFactorySVG.h"
+//#include <cstdlib>
+
+
+
+//TEST(FloorplanFactorySVG, parse) {
+
+//	const std::string filename = getDataFile("test.svg");
+//	FloorplanFactorySVG factory(filename, 1.0);
+
+//	Floor f1 = factory.getFloor("1");
+//	ASSERT_EQ(30, f1.getObstacles().size());
+
+//	Floor f2 = factory.getFloor("2");
+//	ASSERT_EQ(30, f2.getObstacles().size());
+
+//	Floor f3 = factory.getFloor("1_2");
+//	ASSERT_EQ(12, f3.getObstacles().size());
+
+//}
+
+
+
+#endif
--- a/tests/geo/TestAngle.cpp
+++ b/tests/geo/TestAngle.cpp
@@ -0,0 +1,21 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "../../geo/Angle.h"
+
+TEST(Angle, calc) {
+
+	ASSERT_EQ(0,	Angle::getDEG(0,0, +1,0));		// to the right
+	ASSERT_EQ(90,	Angle::getDEG(0,0, 0,+1));		// upwards
+	ASSERT_EQ(180,	Angle::getDEG(0,0, -1,0));		// to the left
+	ASSERT_EQ(270,	Angle::getDEG(0,0, 0,-1));		// downwards
+
+	ASSERT_EQ(45,	Angle::getDEG(0,0, +1,+1));		// to the upper right
+	ASSERT_EQ(135,	Angle::getDEG(0,0, -1,+1));		// to the upper left
+	ASSERT_EQ(225,	Angle::getDEG(0,0, -1,-1));		// to the lower left
+	ASSERT_EQ(315,	Angle::getDEG(0,0, +1,-1));		// to the lower right
+
+}
+
+#endif
--- a/tests/grid/TestBBox.cpp
+++ b/tests/grid/TestBBox.cpp
@@ -0,0 +1,47 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#include "../../grid/GridNodeBBox.h"
+
+TEST(BBox, equals) {
+
+	GridNodeBBox bb1(GridPoint(2,2,2), 2);
+	GridNodeBBox bb2(GridPoint(2,2,2), 2);
+
+	GridNodeBBox bb3(GridPoint(3,2,2), 2);
+	GridNodeBBox bb4(GridPoint(2,3,2), 2);
+	GridNodeBBox bb5(GridPoint(2,2,3), 2);
+	GridNodeBBox bb6(GridPoint(2,2,2), 4);
+
+	ASSERT_TRUE(bb1 == bb2);
+	ASSERT_TRUE(bb1 == bb5);		// z doesnt matter for the bbox
+
+	ASSERT_FALSE(bb1 == bb3);
+	ASSERT_FALSE(bb1 == bb4);
+	ASSERT_FALSE(bb1 == bb6);
+
+}
+
+TEST(BBox, intersect) {
+
+	GridNodeBBox bb1(GridPoint(20,20,20), 20);
+
+	// left
+	ASSERT_FALSE(bb1.intersects( Line2D(9, -999, 9, +999) ));
+	ASSERT_TRUE (bb1.intersects( Line2D(11, -999, 11, +999) ));
+
+	// right
+	ASSERT_TRUE (bb1.intersects( Line2D(29, -999, 29, +999) ));
+	ASSERT_FALSE(bb1.intersects( Line2D(31, -999, 31, +999) ));
+
+	// top
+	ASSERT_FALSE(bb1.intersects( Line2D(-999, 9, +999, 9) ));
+	ASSERT_TRUE (bb1.intersects( Line2D(-999, 11, +999, 11) ));
+
+	// bottom
+	ASSERT_TRUE (bb1.intersects( Line2D(-999, 29, +999, 29) ));
+	ASSERT_FALSE(bb1.intersects( Line2D(-999, 31, +999, 31) ));
+
+}
+
+#endif
--- a/tests/grid/TestGrid.cpp
+++ b/tests/grid/TestGrid.cpp
@@ -0,0 +1,112 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#include "../../grid/Grid.h"
+#include "../../grid/GridPoint.h"
+#include "../../grid/GridNode.h"
+
+class GP : public GridNode, public GridPoint {
+public:
+	GP() : GridNode(), GridPoint() {;}
+	GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
+
+};
+
+TEST(Grid, add) {
+
+	Grid<20, GP> grid;
+	ASSERT_EQ(0, grid.add(GP()));
+	ASSERT_EQ(1, grid.add(GP()));
+	ASSERT_EQ(2, grid.add(GP()));
+	ASSERT_EQ(3, grid.add(GP()));
+
+	// not aligned? -> error
+	ASSERT_THROW(grid.add(GP(10,10,10)), std::exception);
+	ASSERT_THROW(grid.add(GP(10,20,20)), std::exception);
+	ASSERT_THROW(grid.add(GP(20,10,20)), std::exception);
+	ASSERT_THROW(grid.add(GP(20,20,10)), std::exception);
+	ASSERT_EQ(4, grid.add(GP(20,20,20)));
+
+	// access
+	ASSERT_EQ(GP(20,20,20), grid[4]);
+
+}
+
+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(50, grid.getBBox(idx).x2_cm);
+	ASSERT_EQ(30, grid.getBBox(idx).y1_cm);
+	ASSERT_EQ(50, grid.getBBox(idx).y2_cm);
+
+}
+
+
+TEST(Grid, neighbors) {
+
+	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.connect(idx1, idx2);
+	grid.connect(idx1, idx3);
+	grid.connect(idx1, idx4);
+	grid.connect(idx1, idx5);
+
+	ASSERT_EQ(4, grid.getNumNeighbors(idx1));
+	ASSERT_EQ(1, grid.getNumNeighbors(idx2));
+	ASSERT_EQ(1, grid.getNumNeighbors(idx3));
+	ASSERT_EQ(1, grid.getNumNeighbors(idx4));
+	ASSERT_EQ(1, grid.getNumNeighbors(idx5));
+
+}
+
+TEST(Grid, uid) {
+
+	Grid<20, GP> grid;
+
+	GP gp(20,40,60);
+	uint64_t uid = grid.getUID(gp);
+
+	const int mask = (1 << 20) - 1;	// 20-bit mask
+	ASSERT_EQ(3, uid >> 40 & mask);	// x
+	ASSERT_EQ(2, uid >> 20 & mask);	// y
+	ASSERT_EQ(1, uid >> 0 & mask);	// z
+
+}
+
+TEST(Grid, nearest) {
+
+	Grid<20, GP> grid;
+
+	GP c1(20,20,20);
+	GP c2(40,40,40);
+
+	grid.add(c1);
+	grid.add(c2);
+
+	ASSERT_EQ(c1, grid.getNodeFor(GP(15,20,20)));
+	ASSERT_EQ(c1, grid.getNodeFor(GP(20,15,20)));
+	ASSERT_EQ(c1, grid.getNodeFor(GP(20,20,15)));
+
+	ASSERT_EQ(c1, grid.getNodeFor(GP(25,20,20)));
+	ASSERT_EQ(c1, grid.getNodeFor(GP(20,25,20)));
+	ASSERT_EQ(c1, grid.getNodeFor(GP(20,20,25)));
+
+	ASSERT_EQ(c2, grid.getNodeFor(GP(35,40,40)));
+	ASSERT_EQ(c2, grid.getNodeFor(GP(40,35,40)));
+	ASSERT_EQ(c2, grid.getNodeFor(GP(40,40,35)));
+
+	ASSERT_EQ(c2, grid.getNodeFor(GP(45,40,40)));
+	ASSERT_EQ(c2, grid.getNodeFor(GP(40,45,40)));
+	ASSERT_EQ(c2, grid.getNodeFor(GP(40,40,45)));
+
+}
+
+#endif
--- a/tests/grid/TestGridFactory.cpp
+++ b/tests/grid/TestGridFactory.cpp
@@ -0,0 +1,45 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#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) {;}
+
+};
+
+TEST(GridFactory, create) {
+
+	Grid<20, GP> g;
+	GridFactory<20, GP> gf(g);
+	FloorplanFactorySVG fpf(getDataFile("test.svg"), 2);
+	Floor f1 = fpf.getFloor("1");
+
+	gf.addFloor(f1, 0);
+
+	K::Gnuplot gp;
+	K::GnuplotSplot splot;
+	K::GnuplotSplotElementPoints points;
+
+	for (int i = 0; i < g.getNumNodes(); ++i) {
+		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
--- a/tests/grid/TestGridPoint.cpp
+++ b/tests/grid/TestGridPoint.cpp
@@ -0,0 +1,20 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+
+#include "../../grid/GridPoint.h"
+
+TEST(GridPoint, distance) {
+
+	GridPoint p1(0,0,0);
+	GridPoint p2(100,0,0);
+	GridPoint p3(0,100,0);
+	GridPoint p4(0,0,100);
+
+	ASSERT_EQ(1, p1.getDistanceInMeter(p2));
+	ASSERT_EQ(1, p1.getDistanceInMeter(p3));
+	ASSERT_EQ(1, p1.getDistanceInMeter(p4));
+
+}
+
+#endif