refactored the new floorplan

new helper methods/operator toe geo classes

Assertions.h

@@ -13,45 +13,47 @@
 namespace Assert {
 
 
-	static inline void doThrow(const char* err) {
+	template <typename STR> static inline void doThrow(const STR err) {
 		#ifdef WITH_ASSERTIONS
 			std::string str = "in: ";
 			str += __PRETTY_FUNCTION__;
 			str += " error: ";
 			str += err;
 			throw Exception(err);
+                #else
+                    (void) err;
 		#endif
 	}
 
-	template <typename T> static inline void equal(const T v1, const T v2, const char* err) {
+	template <typename T, typename STR> static inline void equal(const T v1, const T v2, const STR err) {
 		if (v1 != v2) {doThrow(err);}
 	}
 
-	template <typename T> static inline void isTrue(const T v, const char* err) {
+	template <typename T, typename STR> static inline void isTrue(const T v, const STR err) {
 		if (!v) {doThrow(err);}
 	}
 
-	template <typename T> static inline void isFalse(const T v, const char* err) {
+	template <typename T, typename STR> static inline void isFalse(const T v, const STR err) {
 		if (v) {doThrow(err);}
 	}
 
-	template <typename T> static inline void isNull(const T v, const char* err) {
+	template <typename T, typename STR> static inline void isNull(const T v, const STR err) {
 		if (v != nullptr) {doThrow(err);}
 	}
 
-	template <typename T> static inline void isNotNull(const T v, const char* err) {
+	template <typename T, typename STR> static inline void isNotNull(const T v, const STR err) {
 		if (v == nullptr) {doThrow(err);}
 	}
 
-	template <typename T> static inline void isNotNaN(const T v, const char* err) {
+	template <typename T, typename STR> static inline void isNotNaN(const T v, const STR err) {
 		if (v != v) {doThrow(err);}
 	}
 
-	template <typename T> static inline void isNot0(const T v, const char* err) {
+	template <typename T, typename STR> static inline void isNot0(const T v, const STR err) {
 		if (v == 0) {doThrow(err);}
 	}
 
-	template <typename T> static inline void isBetween(const T v, const T min, const T max, const char* err) {
+	template <typename T, typename STR> static inline void isBetween(const T v, const T min, const T max, const STR err) {
 		if (v < min || v > max) {doThrow(err);}
 	}
 

floorplan/v2/Floorplan.h

@@ -0,0 +1,188 @@
+#ifndef FLOORPLAN_H
+#define FLOORPLAN_H
+
+#include <string>
+#include <vector>
+#include <initializer_list>
+#include <unordered_map>
+
+#include "../../geo/Point3.h"
+#include "../../geo/Point2.h"
+
+namespace Floorplan {
+
+	/** 3D polygon */
+	struct Polygon2 {
+		std::vector<Point2> points;
+		Polygon2() : points() {;}
+		Polygon2(const std::initializer_list<Point2> lst) : points(lst) {;}
+		bool operator == (const Polygon2& o) const {return std::equal(o.points.begin(), o.points.end(), this->points.begin());}
+	};
+
+	/** additional type-info for obstacles */
+	enum class ObstacleType {
+		UNKNOWN,
+		WALL,
+		DOOR,
+		WINDOW,
+		HANDRAIL,
+		PILLAR,
+		_END,
+	};
+
+	/** all supported material types */
+	enum class Material {
+		UNKNOWN,
+		CONCRETE,
+		WOOD,
+		DRYWALL,
+		GLASS,
+		_END,
+	};
+
+	/** types of outlines. either add or remove the selected region */
+	enum class OutlineMethod {
+		ADD,
+		REMOVE,
+		_END,
+	};
+
+
+
+	/** an AccessPoint located somewhere within the map */
+	struct AccessPoint {
+		std::string name;
+		std::string mac;
+		Point3 pos;
+		AccessPoint() : name(), mac(), pos() {;}
+		AccessPoint(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;}
+		bool operator == (const AccessPoint& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
+	};
+
+	/** a beacon located somewhere within the map */
+	struct Beacon {
+		std::string name;
+		std::string mac;
+		Point3 pos;
+		Beacon() : name(), mac(), pos() {;}
+		Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;}
+		bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
+	};
+
+
+	/** a polygon denoting a floor's outline. is either added or removed */
+	struct FloorOutlinePolygon {
+		OutlineMethod method;
+		std::string name;
+		Polygon2 poly;
+		FloorOutlinePolygon() : method(OutlineMethod::ADD), name(), poly() {;}
+		FloorOutlinePolygon(const OutlineMethod method, const std::string& name, const Polygon2& poly) : method(method), name(name), poly(poly) {;}
+		bool operator == (const FloorOutlinePolygon& o) const {return (o.method == method) && (o.name == name) && (o.poly == poly);}
+	};
+
+	/** base-class for one obstacle (wall, door, window, pillar, ..) within a floor */
+	struct FloorObstacle {
+		ObstacleType type;
+		Material material;
+		FloorObstacle() : type(), material() {;}
+		FloorObstacle(const ObstacleType type, const Material material) : type(type), material(material) {;}
+		virtual ~FloorObstacle() {;}
+	};
+
+	/** line obstacle */
+	struct FloorObstacleLine : public FloorObstacle {
+		Point2 from;
+		Point2 to;
+		FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(type, material), from(from), to(to) {;}
+		FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2) : FloorObstacle(type, material), from(x1,y1), to(x2,y2) {;}
+	};
+
+	/** circle obstacle */
+	struct FloorObstacleCircle : public FloorObstacle {
+		Point2 center;
+		float radius;
+		FloorObstacleCircle(const ObstacleType type, const Material material, const Point2 center, const float radius) : FloorObstacle(type, material), center(center), radius(radius) {;}
+		FloorObstacleCircle(const ObstacleType type, const Material material, const float cx, const float cy, const float radius) : FloorObstacle(type, material), center(cx,cy), radius(radius) {;}
+	};
+
+	/** one region (e.g. a room) within the floor, described using a polygon */
+	struct FloorRegion {
+		std::string name;
+		Polygon2 poly;
+		FloorRegion() : name(), poly() {;}
+		FloorRegion(const std::string& name, const Polygon2& poly) : name(name), poly(poly) {;}
+	};
+
+
+
+	/** an image file that can be added to the map */
+		struct UnderlayImage {
+		std::string name;
+		std::string filename;
+		Point2 anchor;
+		float scaleX;
+		float scaleY;
+	};
+
+	/** a free key-value element */
+	struct KeyValueElement {
+		std::string empty;
+		std::unordered_map<std::string, std::string> params;
+	   const std::string& getVal(const std::string& key) const {
+			auto it = params.find(key);
+			return (it == params.end()) ? (empty) : (it->second);
+		}
+		float getFloat(const std::string& key) const { return std::stof(getVal(key)); }
+		void setVal(const std::string& key, const std::string& val) {
+			params[key] = val;
+		}
+		void setFloat(const std::string& key, const float val) {
+			params[key] = std::to_string(val);
+		}
+	};
+
+	using FloorOutline = std::vector<FloorOutlinePolygon*>;
+	using FloorObstacles = std::vector<FloorObstacle*>;
+	using FloorAccessPoints = std::vector<AccessPoint*>;
+	using FloorBeacons = std::vector<Beacon*>;
+	using FloorRegions = std::vector<FloorRegion*>;
+	using FloorUnderlays = std::vector<UnderlayImage*>;
+
+	/** describes one floor within the map, starting at a given height */
+	struct Floor {
+
+		float atHeight;							// the floor's starting height
+		float height;							// the floor's total height (from start)
+		std::string name;						// the floor's name
+		FloorOutline outline;                   // the floor's outline (ground)
+		FloorObstacles obstacles;               // all obstacles (wall, door, window, ..) within the floor
+		FloorRegions regions;                   // all regions within the floor (rooms, ...)
+		FloorAccessPoints accesspoints;
+		FloorBeacons beacons;
+                FloorUnderlays underlays;				// underlay images (used for map-building)
+		//FloorKeyValue other;					// other, free elements
+
+		Floor() {;}
+
+		Floor(const Floor& o) = delete;
+		void operator = (const Floor& o) = delete;
+
+	};
+
+	/** describes the whole indoor map */
+	struct IndoorMap {
+
+		float width;
+		float depth;
+		std::vector<Floor*> floors;
+
+		IndoorMap() {;}
+
+		IndoorMap(const IndoorMap& o) = delete;
+		void operator = (const IndoorMap& o) = delete;
+
+	};
+
+}
+
+#endif // FLOORPLAN_H

floorplan/v2/FloorplanReader.h

@@ -0,0 +1,302 @@
+#ifndef FLOORPLANREADER_H
+#define FLOORPLANREADER_H
+
+#include <iostream>
+
+#include "Floorplan.h"
+
+#include "../../Assertions.h"
+#include "../../lib/tinyxml/tinyxml2.h"
+
+namespace Floorplan {
+
+	using namespace tinyxml2;
+
+	/**
+	 * read an IndoorMaps from XML-data
+	 */
+	class Reader {
+
+	public:
+
+		/** read an IndoorMap from the given XML-file */
+		static IndoorMap* readFromFile(const std::string& file) {
+			setlocale(LC_NUMERIC, "C");
+			XMLDocument doc;
+			doc.LoadFile(file.c_str());
+			return parse(doc);
+		}
+
+		/** read an IndoorMap from the given XMl-string */
+		static IndoorMap* readFromString(const std::string& str) {
+			setlocale(LC_NUMERIC, "C");
+			XMLDocument doc;
+			doc.Parse(str.c_str(), str.length());
+			return parse(doc);
+		}
+
+	private:
+
+	#define FOREACH_NODE(out, in)	for( const XMLElement* out = (XMLElement*) in->FirstChild(); out; out = (XMLElement*) out->NextSibling() )
+
+		static void assertNode(const std::string& node, const XMLElement* el) {
+			std::string err = std::string("unexpected node '") + el->Name() + "' expected '" + node + "'";
+			Assert::equal(node, std::string(el->Name()), err);
+		}
+
+		/** parse the complete document */
+		static IndoorMap* parse(XMLDocument& doc) {
+			return parseMap((XMLElement*)doc.FirstChild());
+		}
+
+		/** parse the <map> node */
+		static IndoorMap* parseMap(const XMLElement* el) {
+			std::cout << el->Name() << std::endl;
+			IndoorMap* map = new IndoorMap();
+			map->width = el->FloatAttribute("width");
+			map->depth = el->FloatAttribute("depth");
+			FOREACH_NODE(n, el) {
+				if (std::string("floors") == n->Name()) {map->floors = parseFloors(n);}
+			}
+			return map;
+		}
+
+		/** parse the <floors> node */
+		static std::vector<Floor*> parseFloors(const XMLElement* el) {
+			std::vector<Floor*> floors;
+			FOREACH_NODE(n, el) {
+				if (std::string("floor") == n->Name()) {floors.push_back(parseFloor(n));}
+			}
+			return floors;
+		}
+
+		/** parse one <floor> node */
+		static Floor* parseFloor(const XMLElement* el) {
+			Floor* floor = new Floor();
+			floor->atHeight = el->FloatAttribute("atHeight");
+			floor->height = el->FloatAttribute("height");
+			floor->name = el->Attribute("name");
+			FOREACH_NODE(n, el) {
+				if (std::string("outline") == n->Name())        {floor->outline = parseFloorOutline(n);}
+				if (std::string("obstacles") == n->Name())	{floor->obstacles = parseFloorObstacles(n);}
+				if (std::string("accesspoints") == n->Name())	{floor->accesspoints = parseFloorAccessPoints(n);}
+				if (std::string("beacons") == n->Name())	{floor->beacons = parseFloorBeacons(n);}
+				if (std::string("regions") == n->Name())	{floor->regions = parseFloorRegions(n);}
+                                if (std::string("underlays") == n->Name())      {floor->underlays = parseFloorUnderlays(n);}
+			}
+			return floor;
+		}
+
+		/** parse the <accesspoints> tag */
+		static std::vector<AccessPoint*> parseFloorAccessPoints(const XMLElement* el) {
+			assertNode("accesspoints", el);
+			std::vector<AccessPoint*> vec;
+			FOREACH_NODE(n, el) {
+				if (std::string("accesspoint") == n->Name()) { vec.push_back(parseAccessPoint(n)); }
+			}
+			return vec;
+		}
+
+                /** parse the <underlays> tag */
+                static FloorUnderlays parseFloorUnderlays(const XMLElement* el) {
+                    FloorUnderlays res;
+                    FOREACH_NODE(n, el) {
+                        if (std::string("underlay") == n->Name()) { res.push_back(parseFloorUnderlay(n)); }
+                    }
+                    return res;
+                }
+
+                /** parse an underlay image */
+                static UnderlayImage* parseFloorUnderlay(const XMLElement* el) {
+                    UnderlayImage* img = new UnderlayImage();
+                    img->anchor.x = el->FloatAttribute("x");
+                    img->anchor.y = el->FloatAttribute("y");
+                    img->scaleX = el->FloatAttribute("sx");
+                    img->scaleY = el->FloatAttribute("sy");
+                    img->name = el->Attribute("name");
+                    img->filename = el->Attribute("file");
+                    return img;
+                }
+
+//		/** parse the <other> tag */
+//		static FloorKeyValue parseFloorKeyValue(const XMLElement* el) {
+//                        FloorKeyValue res;
+//			FOREACH_NODE(n, el) {
+//				if (std::string("keyval") == n->Name()) { res.elements.push_back(parseKeyValueElement(n)); }
+//			}
+//			return res;
+//		}
+
+		/** parse one <keyval> element */
+		static KeyValueElement* parseKeyValueElement(const XMLElement* n) {
+			KeyValueElement* elem = new KeyValueElement();
+			const XMLAttribute* attr = n->FirstAttribute();
+			while (attr) {
+				elem->params[attr->Name()] = attr->Value();
+				attr = attr->Next();
+			}
+			return elem;
+		}
+
+		static AccessPoint* parseAccessPoint(const XMLElement* n) {
+			assertNode("accesspoint", n);
+			AccessPoint* ap = new AccessPoint();
+			ap->mac = n->Attribute("mac");
+			ap->name = n->Attribute("name");
+			ap->pos = parsePoint3(n);
+			return ap;
+		}
+
+
+		/** parse the <beacons> tag */
+		static std::vector<Beacon*> parseFloorBeacons(const XMLElement* el) {
+			std::vector<Beacon*> vec;
+			FOREACH_NODE(n, el) {
+				if (std::string("beacon") == n->Name()) { vec.push_back(parseBeacon(n)); }
+			}
+			return vec;
+		}
+
+		static Beacon* parseBeacon(const XMLElement* n) {
+			assertNode("beacon", n);
+			Beacon* b = new Beacon();
+			b->mac = n->Attribute("mac");
+			b->name = n->Attribute("name");
+			b->pos = parsePoint3(n);
+			return b;
+		}
+
+
+		static std::vector<FloorRegion*> parseFloorRegions(const XMLElement* el) {
+			std::vector<FloorRegion*> vec;
+			FOREACH_NODE(n, el) {
+				if (std::string("region") == n->Name()) { vec.push_back(parseFloorRegion(n)); }
+			}
+			return vec;
+		}
+
+		static FloorRegion* parseFloorRegion(const XMLElement* n) {
+			assertNode("region", n);
+			FloorRegion* reg = new FloorRegion();
+			reg->name = n->Attribute("name");
+			reg->poly = parsePoly2(n);
+			return reg;
+		}
+
+		/** parse the <obstacles> tag */
+		static std::vector<FloorObstacle*> parseFloorObstacles(const XMLElement* el) {
+			assertNode("obstacles", el);
+			std::vector<FloorObstacle*> obstacles;
+			FOREACH_NODE(n, el) {
+				//                if (std::string("wall") == n->Name())   {obstacles.push_back(parseFloorObstacleWall(n));}
+				//                if (std::string("door") == n->Name())   {obstacles.push_back(parseFloorObstacleDoor(n));}
+				//                if (std::string("window") == n->Name()) {obstacles.push_back(parseFloorObstacleWindow(n));}
+				//                if (std::string("pillar") == n->Name()) {obstacles.push_back(parseFloorObstaclePillar(n));}
+				if (std::string("obstacle") == n->Name())       {obstacles.push_back(parseFloorObstacleLine(n));}       // OLD
+				if (std::string("line") == n->Name())       {obstacles.push_back(parseFloorObstacleLine(n));}
+				if (std::string("circle") == n->Name())     {obstacles.push_back(parseFloorObstacleCircle(n));}
+			}
+			return obstacles;
+		}
+
+		/** parse one line */
+		static FloorObstacleLine* parseFloorObstacleLine(const XMLElement* el) {
+			return new FloorObstacleLine(
+						parseObstacleType(el->Attribute("type")),
+						parseMaterial(el->Attribute("material")),
+						el->FloatAttribute("x1"), el->FloatAttribute("y1"),
+						el->FloatAttribute("x2"), el->FloatAttribute("y2")
+						//                        parsePoint2((XMLElement*) el->FirstChildElement("from")),
+						//                        parsePoint2((XMLElement*) el->FirstChildElement("to"))
+						);
+		}
+
+		/** parse one cirlce */
+		static FloorObstacleCircle* parseFloorObstacleCircle(const XMLElement* el) {
+			return new FloorObstacleCircle(
+						parseObstacleType(el->Attribute("type")),
+						parseMaterial(el->Attribute("material")),
+						el->FloatAttribute("cx"), el->FloatAttribute("cy"),
+						//parsePoint2((XMLElement*) el->FirstChildElement("from")),
+						el->FloatAttribute("radius")
+						);
+		}
+
+		/** parse a floor's <outline> tag */
+		static FloorOutline parseFloorOutline(const XMLElement* el) {
+			FloorOutline outline;
+			FOREACH_NODE(n, el) {
+				if (std::string("polygon") == n->Name()) {
+					outline.push_back(parseFloorPolygon(n));       // TODO
+				}
+			}
+			return outline;
+		}
+
+		/** parse one polygon */
+		static FloorOutlinePolygon* parseFloorPolygon(const XMLElement* el) {
+			FloorOutlinePolygon* poly = new FloorOutlinePolygon();
+			poly->name = el->Attribute("name");
+			poly->method = parseOutlineMethod(el->Attribute("method"));
+			poly->poly = parsePoly2(el);
+			return poly;
+		}
+
+		/** parse a 2d-polygon denoted by several points */
+		static Polygon2 parsePoly2(const XMLElement* el) {
+			Polygon2 poly;
+			FOREACH_NODE(n, el) {
+				if (std::string("point") == n->Name()) {
+					const Point2 p2 = parsePoint2(n);
+					poly.points.push_back(p2);
+				}
+			}
+			return poly;
+		}
+
+		/** parse a 2D point (x,y) using the given tag's attributes. missing attributes are set to 0 */
+		static Point2 parsePoint2(const XMLElement* el) {
+			Point2 point;
+			point.x = el->FloatAttribute("x");
+			point.y = el->FloatAttribute("y");
+			return point;
+		}
+
+		/** parse a 3D point (x,y,z) using the given tag's attributes. missing attributes are set to 0 */
+		static Point3 parsePoint3(const XMLElement* el) {
+			Point3 point;
+			point.x = el->FloatAttribute("x");
+			point.y = el->FloatAttribute("y");
+			point.z = el->FloatAttribute("z");
+			return point;
+		}
+
+		static ObstacleType parseObstacleType(const std::string type) {
+			try {
+				return (ObstacleType) std::stoi(type);
+			} catch (...) {
+				return ObstacleType::UNKNOWN;
+			}
+		}
+
+		static Material parseMaterial(const std::string material) {
+			try {
+				return (Material) std::stoi(material);
+			} catch (...) {
+				return Material::UNKNOWN;
+			}
+		}
+
+		static OutlineMethod parseOutlineMethod(const std::string method) {
+			try {
+				return (OutlineMethod) std::stoi(method);
+			} catch (...) {
+				return OutlineMethod::ADD;
+			}
+		}
+
+	};
+
+}
+
+#endif // FLOORPLANREADER_H

floorplan/v2/FloorplanWriter.h

@@ -0,0 +1,243 @@
+#ifndef FLOORPLANWRITER_H
+#define FLOORPLANWRITER_H
+
+#include <iostream>
+
+#include "Floorplan.h"
+
+#include "../../Assertions.h"
+#include "../../lib/tinyxml/tinyxml2.h"
+
+namespace Floorplan {
+
+	using namespace tinyxml2;
+
+	/**
+	 * write floorplans as XML
+	 */
+	class Writer {
+
+	public:
+
+		static void writeToFile(const IndoorMap* map, const std::string& file) {
+			XMLDocument doc;
+			construct(doc, map);
+			doc.SaveFile(file.c_str(), false);
+		}
+
+		static std::string writeToString(const IndoorMap* map) {
+			XMLDocument doc;
+			construct(doc, map);
+			XMLPrinter printer;
+			doc.Accept( &printer );
+			return printer.CStr();
+		}
+
+	private:
+
+		static std::string toString(const ObstacleType t) {
+			return std::to_string((int)t);
+		}
+
+		static std::string toString(const Material m) {
+			return std::to_string((int)m);
+		}
+
+		static std::string toString(const OutlineMethod m) {
+			return std::to_string((int)m);
+		}
+
+		static void construct(XMLDocument& doc, const IndoorMap* map) {
+
+			XMLElement* root = doc.NewElement("map");
+			doc.InsertEndChild(root);
+			root->SetAttribute("width", map->width);
+			root->SetAttribute("depth", map->depth);
+
+			// add all floors to the map
+			addFloors(doc, root, map);
+
+		}
+
+		/** add all floors to the map */
+		static void addFloors(XMLDocument& doc, XMLElement* root, const IndoorMap* map) {
+			XMLElement* floors = doc.NewElement("floors");
+			for(const Floor* mf : map->floors) {addFloor(doc, floors, mf);}
+			root->InsertEndChild(floors);
+		}
+
+		/** add one floor to the floors-node */
+		static void addFloor(XMLDocument& doc, XMLElement* floors, const Floor* mf) {
+
+			XMLElement* floor = doc.NewElement("floor");
+			floor->SetAttribute("atHeight", mf->atHeight);
+			floor->SetAttribute("height", mf->height);
+			floor->SetAttribute("name", mf->name.c_str());
+			floors->InsertEndChild(floor);
+
+			addFloorOutline(doc, floor, mf);
+			addFloorObstacles(doc, floor, mf);
+                        addFloorUnderlays(doc, floor, mf);
+
+			// add all sorts of POI
+			addFloorPOI(doc, floor, mf);
+
+		}
+
+		/** add all sorts of POI to the floor */
+		static void addFloorPOI(XMLDocument& doc, XMLElement* floor, const Floor* mf) {
+
+			XMLElement* accesspoints = doc.NewElement("accesspoints");
+			for (const AccessPoint* ap : mf->accesspoints) {
+				XMLElement* accesspoint = doc.NewElement("accesspoint");
+				accesspoint->SetAttribute("name", ap->name.c_str());
+				accesspoint->SetAttribute("mac", ap->mac.c_str());
+				accesspoint->SetAttribute("x", ap->pos.x);
+				accesspoint->SetAttribute("y", ap->pos.y);
+				accesspoint->SetAttribute("z", ap->pos.z);
+				accesspoints->InsertEndChild(accesspoint);
+			}
+			floor->InsertEndChild(accesspoints);
+
+			XMLElement* beacons = doc.NewElement("beacons");
+			for (const Beacon* b : mf->beacons) {
+				XMLElement* beacon = doc.NewElement("beacon");
+				beacon->SetAttribute("name", b->name.c_str());
+				beacon->SetAttribute("mac", b->mac.c_str());
+				beacon->SetAttribute("x", b->pos.x);
+				beacon->SetAttribute("y", b->pos.y);
+				beacon->SetAttribute("z", b->pos.z);
+				beacons->InsertEndChild(beacon);
+			}
+			floor->InsertEndChild(beacons);
+
+
+		}
+
+		static void addFloorOutline(XMLDocument& doc, XMLElement* floor, const Floor* mf) {
+
+			XMLElement* outline = doc.NewElement("outline");
+			floor->InsertEndChild(outline);
+
+			//            XMLElement* add = doc.NewElement("add");
+			//            outline->InsertEndChild(add);
+			for (const FloorOutlinePolygon* poly : mf->outline) { addFloorOutlinePolygon(doc, outline, poly); }
+
+			//            XMLElement* remove = doc.NewElement("remove");
+			//            outline->InsertEndChild(remove);
+			//            for (const FloorOutlinePolygon& poly : mf.outline.remove) { addFloorOutlinePolygon(doc, remove, poly); }
+
+		}
+
+		static void addFloorOutlinePolygon(XMLDocument& doc, XMLElement* dst, const FloorOutlinePolygon* poly) {
+
+			std::string method = toString(poly->method);
+
+			XMLElement* polygon = doc.NewElement("polygon");
+			polygon->SetAttribute("name", poly->name.c_str());
+			polygon->SetAttribute("method", method.c_str());
+			dst->InsertEndChild(polygon);
+
+			for (Point2 p : poly->poly.points) {
+				XMLElement* point = doc.NewElement("point");
+				point->SetAttribute("x", p.x);
+				point->SetAttribute("y", p.y);
+				// skip the z-attribute as the value is also given by the polygons floor: floor.atHeight
+				polygon->InsertEndChild(point);
+			}
+
+		}
+
+		/** write one <keyval> element */
+		static void addKeyValueElement(XMLDocument& doc, XMLElement* other, const KeyValueElement* kv) {
+			XMLElement* elem = doc.NewElement("keyval");
+			for (auto it : kv->params) {
+				elem->Attribute(it.first.c_str(), it.second.c_str());
+			}
+			other->InsertEndChild(elem);
+		}
+
+
+                /** write the <underlays> tag */
+                static void addFloorUnderlays(XMLDocument& doc, XMLElement* floor, const Floor* mf) {
+                    XMLElement* other = doc.NewElement("underlays");
+                    for (const UnderlayImage* kv : mf->underlays) {
+                        addFloorUnderlay(doc, other, kv);
+                    }
+                    floor->InsertEndChild(other);
+                }
+
+                /** write one underlay */
+                static void addFloorUnderlay(XMLDocument& doc, XMLElement* underlays, const UnderlayImage* img) {
+                    XMLElement* underlay = doc.NewElement("underlay");
+                    underlay->SetAttribute("x", img->anchor.x);
+                    underlay->SetAttribute("y", img->anchor.y);
+                    underlay->SetAttribute("sx", img->scaleX);
+                    underlay->SetAttribute("sy", img->scaleY);
+                    underlay->SetAttribute("name", img->name.c_str());
+                    underlay->SetAttribute("file", img->filename.c_str());
+                    underlays->InsertEndChild(underlay);
+                }
+
+
+//		/** write the <other> tag */
+//		static void addFloorKeyValue(XMLDocument& doc, XMLElement* floor, const Floor* mf) {
+//			XMLElement* other = doc.NewElement("other");
+//			for (const KeyValueElement* kv : mf->other.elements) {
+//				addKeyValueElement(doc, other, kv);
+//			}
+//			floor->InsertEndChild(other);
+//		}
+
+
+
+		static void addFloorObstacles(XMLDocument& doc, XMLElement* floor, const Floor* mf) {
+
+			XMLElement* obstacles = doc.NewElement("obstacles");
+
+			for (FloorObstacle* fo : mf->obstacles) {
+				if (dynamic_cast<FloorObstacleLine*>(fo)) {
+					addFloorObstacleLine(doc, obstacles, (FloorObstacleLine*)fo);
+				} else if (dynamic_cast<FloorObstacleCircle*>(fo)) {
+					addFloorObstacleCircle(doc, obstacles, (FloorObstacleCircle*)fo);
+				}
+			}
+
+			floor->InsertEndChild(obstacles);
+
+		}
+
+		static void addFloorObstacleLine(XMLDocument& doc, XMLElement* obstacles, FloorObstacleLine* line) {
+
+			XMLElement* obstacle = doc.NewElement("line");
+			const std::string type = toString(line->type);
+			const std::string material = toString(line->material);
+			obstacle->SetAttribute("material", material.c_str());
+			obstacle->SetAttribute("type", type.c_str());
+			obstacle->SetAttribute("x1", line->from.x);
+			obstacle->SetAttribute("y1", line->from.y);
+			obstacle->SetAttribute("x2", line->to.x);
+			obstacle->SetAttribute("y2", line->to.y);
+			obstacles->InsertEndChild(obstacle);
+
+		}
+
+		static void addFloorObstacleCircle(XMLDocument& doc, XMLElement* obstacles, FloorObstacleCircle* circle) {
+
+			XMLElement* obstacle = doc.NewElement("line");
+			const std::string type = toString(circle->type);
+			const std::string material = toString(circle->material);
+			obstacle->SetAttribute("material", material.c_str());
+			obstacle->SetAttribute("type", type.c_str());
+			obstacle->SetAttribute("cx", circle->center.x);
+			obstacle->SetAttribute("cy", circle->center.y);
+			obstacle->SetAttribute("radius", circle->radius);
+			obstacles->InsertEndChild(obstacle);
+
+		}
+
+	};
+
+}
+
+#endif // FLOORPLANWRITER_H

geo/BBox2.h

@@ -70,6 +70,12 @@ public:
 		return true;
 	}
 
+	/** grow the bbox by the amount given for each dimension */
+	void grow(const Point2& p) {
+		p1 -= p;	// decrease minimum
+		p2 += p;	// increase maximum
+	}
+
 };
 
 #endif // BBOX2_H

geo/BBox3.h

@@ -18,8 +18,12 @@ private:
 
 public:
 
+        /** empty ctor */
 	BBox3() : p1(MAX,MAX,MAX), p2(MIN,MIN,MIN) {;}
 
+        /** ctor with min and max */
+        BBox3(const Point3 min, const Point3 max) : p1(min), p2(max) {;}
+
 	/** adjust the bounding-box by adding this point */
 	void add(const Point3& p) {
 
@@ -71,6 +75,12 @@ public:
 		p2 += p;	// increase maximum
 	}
 
+        /** set both, min/max z to the same value */
+        void setZ(const float z) {
+            p1.z = z;
+            p2.z = z;
+        }
+
 	/** does the bbox contain the given point? */
 	bool contains(const Point3& p) const {
 		if (p.x < p1.x) {return false;}

geo/Line2.h

@@ -14,8 +14,13 @@ public:
 
 public:
 
+        /** empty ctor */
 	Line2() : p1(), p2() {;}
 
+        /** value ctor */
+        Line2(const Point2 p1, const Point2 p2) : p1(p1), p2(p2) {;}
+
+        /** value ctor */
 	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 {
@@ -24,6 +29,30 @@ public:
 //	}
 
 
+        /** get intersection between these two lines (unlimited length!) */
+        bool getLineIntersection(const Line2& other, Point2& result) const {
+
+                double bx = p2.x - p1.x;
+                double by = p2.y - p1.y;
+
+                double dx = other.p2.x - other.p1.x;
+                double dy = other.p2.y - other.p1.y;
+
+                double b_dot_d_perp = bx*dy - by*dx;
+
+                if(b_dot_d_perp == 0) {return false;}
+
+                double cx = other.p1.x - p1.x;
+                double cy = other.p1.y - p1.y;
+                double t = (cx*dy - cy*dx) / b_dot_d_perp;
+
+                result.x = p1.x + t * bx;
+                result.y = p1.y + t * by;
+
+                return true;
+
+        }
+
 	bool getSegmentIntersection(const Line2& other) const {
 
 		const float bx = p2.x - p1.x;
@@ -49,6 +78,34 @@ public:
 
 	}
 
+        bool getSegmentIntersection(const Line2& other, Point2& result) 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;}
+
+                result.x = p1.x + t * bx;
+                result.y = p1.y + t * by;
+
+                return true;
+
+        }
+
 };
 
 #endif // LINE2D_H

geo/Point2.h

@@ -38,6 +38,10 @@ struct Point2 {
 
 	bool operator == (const Point2& o) const {return x==o.x && y==o.y;}
 
+        bool operator != (const Point2& o) const {return x!=o.x || y!=o.y;}
+
+
+        Point2 perpendicular() const {return Point2(-y, x);}
 
 	/** get the distance between this point and the other one */
 	float getDistance(const Point2& o) const {

geo/Point3.h

@@ -46,6 +46,8 @@ struct Point3 {
 
 	bool operator == (const Point3& o) const {return x==o.x && y==o.y && z==o.z;}
 
+        bool operator != (const Point3& o) const {return x!=o.x || y!=o.y || z!=o.z;}
+
 	Point2 xy() const {return Point2(x,y);}
 
 	/** read-only array access */

tests/floorplan/TestNewFloorplan.cpp

@@ -0,0 +1,114 @@
+#ifdef WITH_TESTS
+
+#include "../Tests.h"
+#include "../../floorplan/v2/Floorplan.h"
+#include <cstdlib>
+
+
+
+TEST(NewFloorplan, saveAndLoad) {
+
+	std::string xml;
+
+	float f1AtHeight = 1.1f;
+	float f2AtHeight = 3.3f;
+
+	Floorplan::AccessPoint ap0("ap0", "11:11:11:22:22:22", Point3(1,1,1));
+	Floorplan::AccessPoint ap1("ap1", "01:23:45:67:89:ab", Point3(1,2,3));
+	Floorplan::AccessPoint ap2("ap2", "22:33:44:55:66:77", Point3(4,5,6));
+
+	Floorplan::Beacon b0("b0", "22:22:22:11:11:11", Point3(2,2,2));
+	Floorplan::Beacon b1("b1", "aa:bb:cc:dd:ee:ff", Point3(3,2,1));
+	Floorplan::Beacon b2("b2", "77:66:55:44:33:22", Point3(6,5,4));
+
+	Floorplan::FloorObstacle fo0(Floorplan::ObstacleType::WALL, Floorplan::Material::CONCRETE, Point2(3,2), Point2(1,2));
+	Floorplan::FloorObstacle fo1(Floorplan::ObstacleType::DOOR, Floorplan::Material::WOOD, Point2(4,5), Point2(7,8));
+
+	Floorplan::FloorOutlinePolygon pa0("pa0", {Point2(1,2), Point2(4,5), Point2(7,8)});
+	Floorplan::FloorOutlinePolygon pr0("pr0", {Point2(3,2), Point2(6,5), Point2(9,8)});
+
+	Floorplan::FloorOutlinePolygon pa1("pa1", {Point2(1,2), Point2(7,8)});
+	Floorplan::FloorOutlinePolygon pa2("pa2", {Point2(4,5), Point2(7,8)});
+
+	// create and export
+	{
+
+		Floorplan::IndoorMap im;
+		im.width = 123;
+		im.depth = 456;
+
+		// floor1
+		Floorplan::Floor f1;
+		f1.atHeight = f1AtHeight;
+		f1.accesspoints.push_back(ap0);
+		f1.beacons.push_back(b0);
+		f1.obstacles.push_back(fo0);
+		f1.obstacles.push_back(fo1);
+		f1.outline.add.push_back(pa0);
+		f1.outline.remove.push_back(pr0);
+		im.floors.push_back(f1);
+
+		// floor2
+		Floorplan::Floor f2;
+		f2.atHeight = f2AtHeight;
+		f2.accesspoints.push_back(ap1);
+		f2.accesspoints.push_back(ap2);
+		f2.beacons.push_back(b1);
+		f2.beacons.push_back(b2);
+		f2.outline.add.push_back(pa1);
+		f2.outline.add.push_back(pa2);
+		im.floors.push_back(f2);
+
+		xml = Floorplan::Writer::writeToString(im);
+
+	}
+
+	// load and check
+	{
+
+		const float delta = 0.0001;
+
+		Floorplan::IndoorMap im = Floorplan::Reader::readFromString(xml);
+		ASSERT_NEAR(123, im.width, delta);
+		ASSERT_NEAR(456, im.depth, delta);
+		ASSERT_EQ(2, im.floors.size());
+
+		// floor1
+		ASSERT_NEAR(f1AtHeight, im.floors[0].atHeight, delta);
+		ASSERT_EQ(1, im.floors[0].accesspoints.size());
+		ASSERT_EQ(1, im.floors[0].beacons.size());
+		ASSERT_EQ(2, im.floors[0].obstacles.size());
+		ASSERT_EQ(1, im.floors[0].outline.add.size());
+		ASSERT_EQ(1, im.floors[0].outline.remove.size());
+
+		ASSERT_EQ(ap0, im.floors[0].accesspoints[0]);
+		ASSERT_EQ(b0, im.floors[0].beacons[0]);
+		ASSERT_EQ(fo0, im.floors[0].obstacles[0]);
+		ASSERT_EQ(fo1, im.floors[0].obstacles[1]);
+		ASSERT_EQ(pa0, im.floors[0].outline.add[0]);
+		ASSERT_EQ(pr0, im.floors[0].outline.remove[0]);
+
+		// floor2
+		ASSERT_NEAR(f2AtHeight, im.floors[1].atHeight, delta);
+		ASSERT_EQ(2, im.floors[1].accesspoints.size());
+		ASSERT_EQ(2, im.floors[1].beacons.size());
+		ASSERT_EQ(0, im.floors[1].obstacles.size());
+		ASSERT_EQ(2, im.floors[1].outline.add.size());
+		ASSERT_EQ(0, im.floors[1].outline.remove.size());
+
+		ASSERT_EQ(ap1, im.floors[1].accesspoints[0]);
+		ASSERT_EQ(ap2, im.floors[1].accesspoints[1]);
+		ASSERT_EQ(b1, im.floors[1].beacons[0]);
+		ASSERT_EQ(b2, im.floors[1].beacons[1]);
+		ASSERT_EQ(pa1, im.floors[1].outline.add[0]);
+		ASSERT_EQ(pa2, im.floors[1].outline.add[1]);
+
+
+	}
+
+
+}
+
+
+
+#endif