From b03804d378010fee43a5a8f03bf964fe9ecfc2ab Mon Sep 17 00:00:00 2001
From: FrankE <support@frank-ebner.de>
Date: Tue, 21 Mar 2017 21:11:58 +0100
Subject: [PATCH] added earth-registration load/save

---
 floorplan/v2/Floorplan.h       |  5 ++++-
 floorplan/v2/FloorplanReader.h | 26 +++++++++++++++++++++++++-
 floorplan/v2/FloorplanWriter.h | 23 +++++++++++++++++++++++
 geo/EarthPos.h                 |  5 +++++
 4 files changed, 57 insertions(+), 2 deletions(-)

diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h
index 0a71fb0..1f62b72 100644
--- a/floorplan/v2/Floorplan.h
+++ b/floorplan/v2/Floorplan.h
@@ -545,6 +545,9 @@ namespace Floorplan {
 
 		Point3 posOnMap_m;
 
+		/** empty ctor */
+		EarthPosMapPos() : posOnEarth(), posOnMap_m() {;}
+
 		/** ctor */
 		EarthPosMapPos(const EarthPos posOnEarth, const Point3 posOnMap_m) : posOnEarth(posOnEarth), posOnMap_m(posOnMap_m) {;}
 
@@ -555,7 +558,7 @@ namespace Floorplan {
 	struct EarthRegistration {
 
 		/** all available correspondences: earth <-> map */
-		std::vector<EarthPosMapPos> correspondences;
+		std::vector<EarthPosMapPos*> correspondences;
 
 	};
 
diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h
index 7c2f397..63a6ac7 100644
--- a/floorplan/v2/FloorplanReader.h
+++ b/floorplan/v2/FloorplanReader.h
@@ -81,12 +81,36 @@ namespace Floorplan {
 			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);}
+				if (std::string("floors") == n->Name())		{map->floors = parseFloors(n);}
+				if (std::string("earthReg") == n->Name())	{map->earthReg = parseEarthReg(n);}
 			}
 			return map;
 		}
 
+		/** parse the <earthReg> node */
+		static EarthRegistration parseEarthReg(const XMLElem* el) {
+			EarthRegistration reg;
+			FOREACH_NODE(n, el) {
+				if (std::string("correspondences") == n->Name()) {
+					FOREACH_NODE(n2, n) {
+						if (std::string("point") == n2->Name()) {
+							Floorplan::EarthPosMapPos* pos = new Floorplan::EarthPosMapPos();
+							pos->posOnMap_m.x = n2->FloatAttribute("mx");
+							pos->posOnMap_m.y = n2->FloatAttribute("my");
+							pos->posOnMap_m.z = n2->FloatAttribute("mz");
+							pos->posOnEarth.lat = n2->FloatAttribute("lat");
+							pos->posOnEarth.lon = n2->FloatAttribute("lon");
+							pos->posOnEarth.height = n2->FloatAttribute("alt");
+							reg.correspondences.push_back(pos);
+						}
+					}
+				}
+			}
+			return reg;
+		}
+
 		/** parse the <floors> node */
 		static std::vector<Floor*> parseFloors(const XMLElem* el) {
 			std::vector<Floor*> floors;
diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h
index 3c09292..4294ff4 100644
--- a/floorplan/v2/FloorplanWriter.h
+++ b/floorplan/v2/FloorplanWriter.h
@@ -49,11 +49,34 @@ namespace Floorplan {
 			root->SetAttribute("width", map->width);
 			root->SetAttribute("depth", map->depth);
 
+			// add earth registration to the map
+			addEarthReg(doc, root, map);
+
 			// add all floors to the map
 			addFloors(doc, root, map);
 
 		}
 
+		/** add earth registration to the map */
+		static void addEarthReg(XMLDoc& doc, XMLElem* root, const IndoorMap* map) {
+			XMLElem* earthReg = doc.NewElement("earthReg"); {
+				XMLElem* correspondences = doc.NewElement("correspondences");
+				for (const Floorplan::EarthPosMapPos* reg : map->earthReg.correspondences) {
+					XMLElem* point = doc.NewElement("point");
+					point->SetAttribute("lat", reg->posOnEarth.lat);
+					point->SetAttribute("lon", reg->posOnEarth.lon);
+					point->SetAttribute("alt", reg->posOnEarth.height);
+					point->SetAttribute("mx", reg->posOnMap_m.x);
+					point->SetAttribute("my", reg->posOnMap_m.y);
+					point->SetAttribute("mz", reg->posOnMap_m.z);
+					correspondences->InsertEndChild(point);
+				}
+				earthReg->InsertEndChild(correspondences);
+			} root->InsertEndChild(earthReg);
+		}
+
+
+
 		/** add all floors to the map */
 		static void addFloors(XMLDoc& doc, XMLElem* root, const IndoorMap* map) {
 			XMLElem* floors = doc.NewElement("floors");
diff --git a/geo/EarthPos.h b/geo/EarthPos.h
index d038fc1..c8bac44 100644
--- a/geo/EarthPos.h
+++ b/geo/EarthPos.h
@@ -11,6 +11,11 @@ struct EarthPos {
 	/** height above sea level */
 	float height;
 
+	/** empty ctor */
+	EarthPos() : lat(NAN), lon(NAN), height(NAN) {
+		;
+	}
+
 	/** ctor with values */
 	EarthPos(const double lat, const double lon, const float height) : lat(lat), lon(lon), height(height) {
 		;