added dijkstra support to navmesh

floorplan/v2/FloorplanReader.h

@@ -36,9 +36,253 @@ namespace Floorplan {
 			if (res != tinyxml2::XMLError::XML_SUCCESS) {
 				throw Exception(
 					std::string() + "error while loading XML " + file + "\n" +
-				    ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
-				    ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
-				);
+					((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
+					((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+							);
 			}
 			IndoorMap* map = parse(doc);
 			return map;
@@ -53,8 +297,8 @@ namespace Floorplan {
 			if (res != tinyxml2::XMLError::XML_SUCCESS) {
 				throw Exception(
 					std::string() + "error while parsing XML\n" +
-				    ((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
-				    ((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
+					((doc.GetErrorStr1()) ? (doc.GetErrorStr1()) : ("")) + "\n" +
+					((doc.GetErrorStr2()) ? (doc.GetErrorStr2()) : (""))
 				);
 			}
 			IndoorMap* map = parse(doc);
@@ -63,7 +307,7 @@ namespace Floorplan {
 
 	private:
 
-    #define FOREACH_NODE(out, in)	for( const XMLElem* out = in->FirstChildElement(); out; out = out->NextSiblingElement() )
+	#define FOREACH_NODE(out, in)	for( const XMLElem* out = in->FirstChildElement(); out; out = out->NextSiblingElement() )
 
 		static void assertNode(const std::string& node, const XMLElem* el) {
 			std::string err = std::string("unexpected node '") + el->Name() + "' expected '" + node + "'";
@@ -138,7 +382,7 @@ namespace Floorplan {
 				if (std::string("pois") == n->Name())			{floor->pois = parseFloorPOIs(n);}
 				if (std::string("stairs") == n->Name())			{floor->stairs = parseFloorStairs(n);}
 				if (std::string("elevators") == n->Name())		{floor->elevators = parseFloorElevators(n);}
-                                if (std::string("gtpoints") == n->Name())       {floor->gtpoints = parseFloorGroundTruthPoints(n);}
+								if (std::string("gtpoints") == n->Name())       {floor->gtpoints = parseFloorGroundTruthPoints(n);}
 			}
 			return floor;
 		}
@@ -226,22 +470,22 @@ namespace Floorplan {
 		}
 
 
-                /** parse the <gtpoints> tag */
-                static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
-                    std::vector<GroundTruthPoint*> vec;
-                    FOREACH_NODE(n, el) {
-                        if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
-                    }
-                    return vec;
-                }
+				/** parse the <gtpoints> tag */
+				static std::vector<GroundTruthPoint*> parseFloorGroundTruthPoints(const XMLElem* el) {
+					std::vector<GroundTruthPoint*> vec;
+					FOREACH_NODE(n, el) {
+						if (std::string("gtpoint") == n->Name()) { vec.push_back(parseFloorGroundTruthPoint(n)); }
+					}
+					return vec;
+				}
 
-                /** parse a <gtpoint> tag */
-                static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
-                    GroundTruthPoint* gtp = new GroundTruthPoint();
-                    gtp->id = el->IntAttribute("id");
-                    gtp->pos = parsePoint3(el);
-                    return gtp;
-                }
+				/** parse a <gtpoint> tag */
+				static GroundTruthPoint* parseFloorGroundTruthPoint(const XMLElem* el) {
+					GroundTruthPoint* gtp = new GroundTruthPoint();
+					gtp->id = el->IntAttribute("id");
+					gtp->pos = parsePoint3(el);
+					return gtp;
+				}
 
 
 		/** parse the <accesspoints> tag */