fixed some grid-factory issues

added some new attributes
minor changes

floorplan/v2/Floorplan.h

@@ -253,7 +253,7 @@ namespace Floorplan {
             int id; //TODO: this value can be changed and isn't set incremental within the indoormap
 			Point3 pos;		// TODO: splint into 2D position + float for "heightAboveGround" [waypoints' height is relative to the floor's height!
             GroundTruthPoint() : id(), pos() {;}
-            GroundTruthPoint(const int& id, const Point3& pos) : id(id), pos(pos) {;}
+			GroundTruthPoint(const int id, const Point3& pos) : id(id), pos(pos) {;}
 			const Point3 getPosition(const Floor& f) const {return pos + Point3(0,0,f.atHeight);}
             bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);}
         };
@@ -319,8 +319,9 @@ namespace Floorplan {
 		ObstacleType type;
 		Point2 from;
 		Point2 to;
-		FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(material), type(type), from(from), to(to) {;}
+		float thickness_m;
-		FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2) {;}
+		FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_m) {;}
+		FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float thickness_m = 0.2f) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m) {;}
 	};
 
 	/** circle obstacle */

floorplan/v2/FloorplanReader.h

@@ -402,7 +402,8 @@ namespace Floorplan {
 				parseObstacleType(el->Attribute("type")),
 				parseMaterial(el->Attribute("material")),
 				el->FloatAttribute("x1"), el->FloatAttribute("y1"),
-				el->FloatAttribute("x2"), el->FloatAttribute("y2")
+				el->FloatAttribute("x2"), el->FloatAttribute("y2"),
+				(el->FloatAttribute("thickness") > 0) ? (el->FloatAttribute("thickness")) : (0.15)	// default wall thickness in m
 			);
 		}
 

floorplan/v2/FloorplanWriter.h

@@ -330,6 +330,7 @@ namespace Floorplan {
 			obstacle->SetAttribute("y1", line->from.y);
 			obstacle->SetAttribute("x2", line->to.x);
 			obstacle->SetAttribute("y2", line->to.y);
+			obstacle->SetAttribute("thickness", line->thickness_m);
 			obstacles->InsertEndChild(obstacle);
 		}
 

geo/Line2.h

@@ -14,15 +14,18 @@ public:
 
 public:
 
-        /** empty ctor */
+	/** empty ctor */
 	Line2() : p1(), p2() {;}
 
-        /** value ctor */
+	/** value ctor */
-        Line2(const Point2 p1, const Point2 p2) : p1(p1), p2(p2) {;}
+	Line2(const Point2 p1, const Point2 p2) : p1(p1), p2(p2) {;}
 
-        /** value ctor */
+	/** value ctor */
 	Line2(const float x1, const float y1, const float x2, const float y2) : p1(x1,y1), p2(x2,y2) {;}
 
+
+	Line2 operator * (const float v) const {return Line2(p1*v, p2*v);}
+
 //	bool getSegmentIntersection(const Line& other) const {
 //		static K::Point p;
 //		return K::Line::getSegmentIntersection(other, p);

grid/factory/v2/Elevators.h

@@ -79,8 +79,13 @@ public:
 
 			// connect each of the new nodes with the node below it. NOW ALSO EXAMINE THE floor above (z2_cm + gs_cm)
 			for (int z_cm = z1_cm; z_cm <= z2_cm + gs_cm; z_cm += gs_cm) {
-				const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm);
+				GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm);
-				const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm);
+				GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm);
+
+				// above the ending floor? -> snap to ending floor
+				// note: this one is needed if the floor-heights are not dividable by the grid-size
+				if (gp.z_cm > floor->getEndingZ()*100) {gp.z_cm = floor->getEndingZ()*100;}
+
 				Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node");
 				Assert::isTrue(grid.hasNodeFor(gp), "missing node");
 				T& n1 = (T&) grid.getNodeFor(gpBelow);

grid/factory/v2/GridFactory.h

@@ -487,7 +487,23 @@ private:
 
 private:
 
-
+	/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
+	static std::vector<Line2> getThickLines(const Floorplan::FloorObstacleLine* line) {
+		//const Line2 base(line->from*100, line->to*100);
+		const float thickness_m = line->thickness_m;
+		const Point2 dir = (line->to - line->from);				// obstacle's direction
+		const Point2 perp = dir.perpendicular().normalized();	// perpendicular direction (90 degree)
+		const Point2 p1 = line->from + perp * thickness_m/2;		// start-up
+		const Point2 p2 = line->from - perp * thickness_m/2;		// start-down
+		const Point2 p3 = line->to + perp * thickness_m/2;		// end-up
+		const Point2 p4 = line->to - perp * thickness_m/2;		// end-down
+		return {
+			Line2(p1, p2),
+			Line2(p3, p4),
+			Line2(p2, p4),
+			Line2(p1, p3),
+		};
+	}
 
 	/** does the bbox intersect with any of the floor's walls? */
 	static inline bool intersects(const GridNodeBBox& bbox, const Floorplan::Floor* floor) {
@@ -499,8 +515,15 @@ private:
 			// depends on the type of obstacle
 			if (dynamic_cast<Floorplan::FloorObstacleLine*>(fo)) {
 				const Floorplan::FloorObstacleLine* line = (Floorplan::FloorObstacleLine*) fo;
-				const Line2 l2(line->from*100, line->to*100);
+
-				if (bbox.intersects(l2)) {return true;}
+				// old method (does not support thickness)
+				//const Line2 l2(line->from*100, line->to*100);
+				//if (bbox.intersects(l2)) {return true;}
+				const std::vector<Line2> lines = getThickLines(line);
+				for (const Line2& l : lines) {
+					if (bbox.intersects(l*100)) {return true;}
+				}
+
 
 			} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
 				const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;