diff --git a/geo/Line2.h b/geo/Line2.h index 3e4ad8b..f063e32 100755 --- a/geo/Line2.h +++ b/geo/Line2.h @@ -45,6 +45,18 @@ public: return p1.getDistance(p2); } + /** keep the starting point, but make the line longer by the given length */ + Line2 longerAtEnd(const float l) { + Point2 dir = p2-p1; + return Line2(p1, p1+dir+dir.normalized()*l); + } + + /** keep the ending point, but make the line longer by the given length */ + Line2 longerAtStart(const float l) { + Point2 dir = p1-p2; + return Line2(p2, p2+dir+dir.normalized()*l); + } + /** get intersection between these two lines (unlimited length!) */ bool getLineIntersection(const Line2& other, Point2& result) const { diff --git a/geo/Point2.h b/geo/Point2.h index 0bf78c3..587ee11 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -44,6 +44,7 @@ struct Point2 { bool operator != (const Point2& o) const {return x!=o.x || y!=o.y;} + bool eq (const Point2& o, const float delta) const { return eq(x,o.x,delta) && eq(y,o.y,delta); } Point2 perpendicular() const {return Point2(-y, x);} @@ -66,6 +67,11 @@ struct Point2 { return "(" + std::to_string(x) + "," + std::to_string(y) + ")"; } +private: + + static inline bool eq(const float a, const float b, const float delta) {return std::abs(a-b) <= delta;} + static inline bool ne(const float a, const float b, const float delta) {return std::abs(a-b) > delta;} + }; inline float dot(const Point2 p1, const Point2 p2) { diff --git a/wifi/estimate/ray3/FloorPos.h b/wifi/estimate/ray3/FloorPos.h index d175f54..d89241c 100644 --- a/wifi/estimate/ray3/FloorPos.h +++ b/wifi/estimate/ray3/FloorPos.h @@ -1,6 +1,8 @@ #ifndef FLOORPOS_H #define FLOORPOS_H +#include "Obstacle3.h" + namespace Ray3D { /** used to model ceiling thickness */ diff --git a/wifi/estimate/ray3/Obstacle3.h b/wifi/estimate/ray3/Obstacle3.h index 720f0d3..758f230 100644 --- a/wifi/estimate/ray3/Obstacle3.h +++ b/wifi/estimate/ray3/Obstacle3.h @@ -26,6 +26,7 @@ namespace Ray3D { WALL, WINDOW, OBJECT, + ERROR, }; Type type; diff --git a/wifi/estimate/ray3/WallsViaCuttedQuads.h b/wifi/estimate/ray3/WallsViaCuttedQuads.h index 6113cb7..8e63f9e 100644 --- a/wifi/estimate/ray3/WallsViaCuttedQuads.h +++ b/wifi/estimate/ray3/WallsViaCuttedQuads.h @@ -4,7 +4,9 @@ #include "../../../geo/Line2.h" #include "../../../geo/Polygon2.h" #include "Walls.h" +#include "FloorPos.h" +#include namespace Ray3D { @@ -18,7 +20,13 @@ namespace Ray3D { struct Wall { + /** algorithms set error flag here */ + bool error = false; + + /** original line from floorplan */ const Floorplan::FloorObstacleLine* line; + + /** outlines after applying thickness */ Line2 l1; Line2 l2; @@ -41,18 +49,25 @@ namespace Ray3D { } + /** get points for CCW wall outline (p1->p2->p3->p4) */ Point2 getP1() const {return l1.p1;} Point2 getP2() const {return l1.p2;} Point2 getP3() const {return l2.p2;} Point2 getP4() const {return l2.p1;} + Point2& getP1() {return l1.p1;} + Point2& getP2() {return l1.p2;} + Point2& getP3() {return l2.p2;} + Point2& getP4() {return l2.p1;} + struct CutRes { Point2 p; - Line2* l1; // within this wall - Line2* l2; // within the other wall + Line2* l1; // affected line within this wall + Line2* l2; // affected line within the other wall CutRes(Point2 p, Line2* l1, Line2* l2) : p(p), l1(l1), l2(l2) {;} }; + /** get all intersecting points between the two walls */ std::vector getIntersections(Wall& o, bool limit = true) { std::vector res; Point2 p; @@ -63,7 +78,23 @@ namespace Ray3D { return res; } - bool containsPoint(const Point2 p) { + /** is this wall directly attached to the given wall? */ + bool directlyConnectedTo(const Wall& o) const { + const float d = 0.001; + return + (line->from.eq( o.line->from, d)) || + (line->to.eq( o.line->from, d)) || + (line->from.eq( o.line->to, d)) || + (line->to.eq( o.line->to, d)); + } + + /** does this wall end within the given wall? */ + bool endsWithin(const Wall& o) const { + return o.containsPoint(line->from) || o.containsPoint(line->to); + } + + /** does this wall contain the given point */ + bool containsPoint(const Point2 p) const { return polygonContainsPoint({l1.p1, l1.p2, l2.p2, l2.p1}, p); } @@ -80,6 +111,22 @@ namespace Ray3D { } + void cutInMiddle(Point2& pa1, Point2& pa2, Point2& pb1, Point2& pb2) { + + // line from pa1->pa2, and pb1->pb2 + // intersection expected near pa2|pb1 + + Line2 l1(pa1, pa2); l1 = l1.longerAtEnd(10); + Line2 l2(pb1, pb2); l2 = l2.longerAtStart(10); + + Point2 pi; + if (intersects(l1, l2, true, pi)) { + pa2 = pi; // replace end of line1 + pb1 = pi; // replace start of line2 + } + + } + std::vector walls; const Floorplan::Floor* floor; std::vector obs; @@ -99,43 +146,34 @@ namespace Ray3D { } virtual const std::vector& get() override { - std::vector tmp = cut(walls); - obs = toObs(tmp, floor); + std::vector tmp = walls; + tmp = cutConnected(tmp); + tmp = cutProtruding(tmp); + obs = toObstacle(tmp, floor); return obs; } private: /** convert all walls to obstacles */ - std::vector toObs(const std::vector& walls, const Floorplan::Floor* f) { + std::vector toObstacle(const std::vector& walls, const Floorplan::Floor* f) { std::vector res; for (const Wall& w : walls) { - res.push_back(toObs(w, f)); + res.push_back(toObstacle(w, f)); } return res; } -// bool isCW(Line2 l) { -// float a = l.getAngle(); -// return a < 0 || a <= M_PI/2 || a >= M_PI*1.5; -// //return a >= 0 && a <= M_PI; -// } /** convert one wall into an obstacle */ - Obstacle3D toObs(const Wall& wall, const Floorplan::Floor* f) { + Obstacle3D toObstacle(const Wall& wall, const Floorplan::Floor* f) { - Obstacle3D obs(getType(wall.line), wall.line->material); + Obstacle3D::Type type = (wall.error) ? (Obstacle3D::Type::ERROR) : (getType(wall.line)); + Obstacle3D obs(type, wall.line->material); const float z1 = f->getStartingZ(); const float z2 = f->getEndingZ(); -// Line2 l1(wall.getP1(), wall.getP2()); -// Line2 l2(wall.getP3(), wall.getP4()); - -// float len1 = l1.getLength(); -// float a1 = l1.getAngle() * 180 / M_PI; -// float a2 = l2.getAngle() * 180 / M_PI; - const Point3 p1 = Point3(wall.getP1().x, wall.getP1().y, z1); const Point3 p2 = Point3(wall.getP2().x, wall.getP2().y, z1); const Point3 p3 = Point3(wall.getP3().x, wall.getP3().y, z1); @@ -156,30 +194,14 @@ namespace Ray3D { obs.reverseFaces(); -// // front -// Triangle3 t1( Point3(0, 0, z1), Point3(len1, 0, z1), Point3(len1, 0, z2) ); -// t1.reverse(); t1.rotate_deg(Point3(0,0,a1)); t1 += Point3(l1.p1.x, l1.p1.y, 0); -// Triangle3 t2( Point3(0, 0, z1), Point3(len1, 0, z2), Point3(0, 0, z2) ); -// t2.reverse(); t2.rotate_deg(Point3(0,0,a1)); t2 += Point3(l1.p1.x, l1.p1.y, 0); - -// // rear -// Triangle3 t3( Point3(0, 0, z1), Point3(len1, 0, z1), Point3(len1, 0, z2) ); -// t3.reverse(); t3.rotate_deg(Point3(0,0,a2)); t3 += Point3(l2.p1.x, l2.p1.y, 0); -// Triangle3 t4( Point3(0, 0, z1), Point3(len1, 0, z2), Point3(0, 0, z2) ); -// t4.reverse(); t4.rotate_deg(Point3(0,0,a2)); t4 += Point3(l2.p1.x, l2.p1.y, 0); - - -// obs.triangles.push_back(t1); -// obs.triangles.push_back(t2); -// obs.triangles.push_back(t3); -// obs.triangles.push_back(t4); -//// obs.triangles.push_back(t4); - return obs; } - std::vector cut(std::vector walls) { + /** cut off walls ending within another wall */ + std::vector cutProtruding(std::vector walls) { + + // if one wall ends within another one cut it off for (size_t i = 0; i < walls.size(); ++i) { Wall& w1 = walls[i]; @@ -189,93 +211,71 @@ namespace Ray3D { if (i == j) {continue;} - std::vector isect = w1.getIntersections(w2); + // if the two walls are directly connected (share one node) -> ignore this case here! + if (w1.directlyConnectedTo(w2)) {continue;} - // for (const auto c : isect) { - // out << "\n"; - // } + // not the case we are looking for? + if (!w1.endsWithin(w2) && !w2.endsWithin(w1)) {continue;} + + // get all intersection points between the two walls + std::vector isects = w1.getIntersections(w2); + + // this should be 0 (no intersections) or 2 (one for each outline) + if (!isects.empty() && isects.size() != 2) { + w1.error = true; + w2.error = true; + std::cout << "detected strange wall intersection" << std::endl; + } // check all detected intersections - for (const auto c : isect) { + for (const auto isect : isects) { - // we have two polygons and need to decide which line to crop: - // we want to cut-off the polygon, that has ending points within the other polygon! - // -> check which line (that contains the intersection point) ends within the other polygon - const bool p1EndsWithinP2 = w2.containsPoint(c.l1->p1) || w2.containsPoint(c.l1->p2); - const bool p2EndsWithinP1 = w1.containsPoint(c.l2->p1) || w1.containsPoint(c.l2->p2); + // if one of the line-ends p1/p2 from wall1 ends within wall2, crop it by setting it to the intersection + if (w2.containsPoint(isect.l1->p1)) {isect.l1->p1 = isect.p;} + if (w2.containsPoint(isect.l1->p2)) {isect.l1->p2 = isect.p;} - // if both polygons end within the other one (this is the case for edges where two lines are conencted) -> ignore - if (p1EndsWithinP2 && p2EndsWithinP1) { + // if one of the line-ends p1/p2 from wall2 ends within wall1, crop it by setting it to the intersection + if (w1.containsPoint(isect.l2->p1)) {isect.l2->p1 = isect.p;} + if (w1.containsPoint(isect.l2->p2)) {isect.l2->p2 = isect.p;} - isect = w1.getIntersections(w2, false); - if (isect.size() != 4) {continue;} + } - // center of the 4 intersections - Point2 isectCen; - for (const auto c2 : isect) {isectCen += c2.p;} - isectCen /= isect.size(); + } - // 2 vectors along the two walls, both pointing AWAY from the center of intersection - const Point2 dir1 = isectCen.getDistance(w1.line->from) < isectCen.getDistance(w1.line->to) ? (w1.line->to - w1.line->from) : (w1.line->from - w1.line->to); - const Point2 dir2 = isectCen.getDistance(w2.line->from) < isectCen.getDistance(w2.line->to) ? (w2.line->to - w2.line->from) : (w2.line->from - w2.line->to); + } - // average between both, after normalization - const Point2 dirFromCen = (dir1.normalized()+dir2.normalized()) / 2; + return walls; - // pointing INWARDS (between both walls) away from the intersection center - const Point2 innerCenter = isectCen + dirFromCen * 10; + } - // sort the 4 intersections by distance from innerCenter - // we want to know the farthest and the nearest of the 4 - auto comp = [innerCenter] (const Wall::CutRes& a, const Wall::CutRes& b) { - return a.p.getDistance(innerCenter) < b.p.getDistance(innerCenter); - }; - std::sort(isect.begin(), isect.end(), comp); - // w1 will be made longer - // w2 will be made shorter + /** if two walls share one node, cut both ends in the middle (like 45 degree) */ + std::vector cutConnected(std::vector walls) { - bool use45 = false; + for (size_t i = 0; i < walls.size(); ++i) { + Wall& w1 = walls[i]; - if (use45) { + for (size_t j = i+1; j < walls.size(); ++j) { + Wall& w2 = walls[j]; - // farthest from center - cropLine(isect[3].l1, isect[3].p); - cropLine(isect[3].l2, isect[3].p); + if (i == j) {continue;} - // nearest to center - cropLine(isect[0].l2, isect[0].p); - cropLine(isect[0].l1, isect[0].p); - - } else { - - // farthest from center - cropLine(isect[3].l1, isect[3].p); - - // nearest to center - cropLine(isect[0].l2, isect[0].p); - - //cropLine(isect[2].l2, isect[2].p); - - // shared point - if (isect[3].l1 != isect[2].l1) { - cropLine(isect[2].l2, isect[2].p); - cropLine(isect[2].l1, isect[2].p); - } else { - cropLine(isect[1].l2, isect[1].p); - cropLine(isect[1].l1, isect[1].p); - } - - } - - } else if (p1EndsWithinP2) { - // crop the intersecting line within polygon 1 - cropLine(c.l1, c.p); - } else if (p2EndsWithinP1) { - // crop the intersecting line within polygon 2 - cropLine(c.l2, c.p); - } + // if the two walls are note directly connected -> ignore + if (!w1.directlyConnectedTo(w2)) {continue;} + const float d = 0.001; + if (w1.line->to.eq(w2.line->from, d)) { + cutInMiddle(w1.getP1(), w1.getP2(), w2.getP1(), w2.getP2()); + cutInMiddle(w1.getP4(), w1.getP3(), w2.getP4(), w2.getP3()); + } else if (w1.line->to.eq(w2.line->to, d)) { + cutInMiddle(w1.getP1(), w1.getP2(), w2.getP3(), w2.getP4()); + cutInMiddle(w1.getP4(), w1.getP3(), w2.getP2(), w2.getP1()); + } else if (w1.line->from.eq(w2.line->to, d)) { + cutInMiddle(w1.getP3(), w1.getP4(), w2.getP3(), w2.getP4()); + cutInMiddle(w1.getP2(), w1.getP1(), w2.getP2(), w2.getP1()); + } else if (w1.line->from.eq(w2.line->from, d)) { + cutInMiddle(w1.getP3(), w1.getP4(), w2.getP1(), w2.getP2()); + cutInMiddle(w1.getP2(), w1.getP1(), w2.getP4(), w2.getP3()); } }