From 9ea7da557b85131ad253a39a8d3e65dfce236c29 Mon Sep 17 00:00:00 2001 From: FrankE Date: Thu, 1 Jun 2017 15:58:58 +0200 Subject: [PATCH] fixed some grid-factory issues added some new attributes minor changes --- floorplan/v2/Floorplan.h | 7 ++++--- floorplan/v2/FloorplanReader.h | 3 ++- floorplan/v2/FloorplanWriter.h | 1 + geo/Line2.h | 11 +++++++---- grid/factory/v2/Elevators.h | 9 +++++++-- grid/factory/v2/GridFactory.h | 29 ++++++++++++++++++++++++++--- 6 files changed, 47 insertions(+), 13 deletions(-) diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index f1dcaf4..8732839 100644 --- a/floorplan/v2/Floorplan.h +++ b/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) {;} - 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) {;} + float thickness_m; + 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 */ diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 63a6ac7..b9cb84e 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/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 ); } diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 4294ff4..e8eff0e 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/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); } diff --git a/geo/Line2.h b/geo/Line2.h index b8ee038..f7e1b93 100755 --- a/geo/Line2.h +++ b/geo/Line2.h @@ -14,15 +14,18 @@ public: public: - /** empty ctor */ + /** empty ctor */ Line2() : p1(), p2() {;} - /** value ctor */ - Line2(const Point2 p1, const Point2 p2) : p1(p1), p2(p2) {;} + /** value ctor */ + 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); diff --git a/grid/factory/v2/Elevators.h b/grid/factory/v2/Elevators.h index ae20b3a..a10f0de 100644 --- a/grid/factory/v2/Elevators.h +++ b/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); - const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm); + GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_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); diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index a42ed81..8d576d1 100755 --- a/grid/factory/v2/GridFactory.h +++ b/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 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(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 lines = getThickLines(line); + for (const Line2& l : lines) { + if (bbox.intersects(l*100)) {return true;} + } + } else if (dynamic_cast(fo)) { const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;