diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 6c82947..a90a480 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -452,7 +452,7 @@ namespace Floorplan { /** snap quad-edges together if their distance is below the given maximum. is used to close minor gaps */ - static void snapQuads(std::vector& quads, const float maxDist) { + static inline void snapQuads(std::vector& quads, const float maxDist) { for (Floorplan::Quad3& quad1 : quads) { for (int i1 = 0; i1 < 4; ++i1) { @@ -479,7 +479,7 @@ namespace Floorplan { } /** convert stair-parts to quads. the scaling factor may be used to slightly grow each quad. e.g. needed to ensure that the quads overlap */ - static std::vector getQuads(const std::vector& parts, const Floor* floor, const float s = 1.0f) { + static inline std::vector getQuads(const std::vector& parts, const Floor* floor, const float s = 1.0f) { std::vector vec; diff --git a/geo/Line2.h b/geo/Line2.h index 3224b6e..3e4ad8b 100755 --- a/geo/Line2.h +++ b/geo/Line2.h @@ -33,29 +33,41 @@ public: // } - /** get intersection between these two lines (unlimited length!) */ - bool getLineIntersection(const Line2& other, Point2& result) const { + float getAngle() const { + return std::atan2(p2.y-p1.y, p2.x-p1.x); + } - double bx = p2.x - p1.x; - double by = p2.y - p1.y; + void reverse() { + std::swap(p1, p2); + } - double dx = other.p2.x - other.p1.x; - double dy = other.p2.y - other.p1.y; + float getLength() const { + return p1.getDistance(p2); + } - double b_dot_d_perp = bx*dy - by*dx; + /** get intersection between these two lines (unlimited length!) */ + bool getLineIntersection(const Line2& other, Point2& result) const { - if(b_dot_d_perp == 0) {return false;} + double bx = p2.x - p1.x; + double by = p2.y - p1.y; - double cx = other.p1.x - p1.x; - double cy = other.p1.y - p1.y; - double t = (cx*dy - cy*dx) / b_dot_d_perp; + double dx = other.p2.x - other.p1.x; + double dy = other.p2.y - other.p1.y; - result.x = p1.x + t * bx; - result.y = p1.y + t * by; + double b_dot_d_perp = bx*dy - by*dx; - return true; + if(b_dot_d_perp == 0) {return false;} - } + double cx = other.p1.x - p1.x; + double cy = other.p1.y - p1.y; + double t = (cx*dy - cy*dx) / b_dot_d_perp; + + result.x = p1.x + t * bx; + result.y = p1.y + t * by; + + return true; + + } bool getSegmentIntersection(const Line2& other) const { @@ -207,4 +219,50 @@ public: }; + +static inline bool intersects(const Line2& l1, const Line2& l2, bool limit, Point2& pos) { + + // (sx1,sy1) + (dx1, dy1)*u = (sx2, sy2) + (dx2, dy2)*v + // solve((c+d*v-a)/b = (g+h*v-e)/f, v) + // solve((a+b*u-c)/d = (e+f*u-g)/h, u) + // bf != 0 + // dh != 0 + // df != bh // kollinear (b/d != f/h) + + const float sx1 = l1.p1.x; // a + const float sy1 = l1.p1.y; // e + + const float dx1 = l1.p2.x - l1.p1.x; // b + const float dy1 = l1.p2.y - l1.p1.y; // f + + const float sx2 = l2.p1.x; // c + const float sy2 = l2.p1.y; // g + + const float dx2 = l2.p2.x - l2.p1.x; // d + const float dy2 = l2.p2.y - l2.p1.y; // h + + // collinear? + const float a1 = dx2*dy1; + const float a2 = dx1*dy2; + if (std::abs(a1-a2) < 0.0001) {return false;} + + const float v = (dy1*(sx1-sx2) + dx1*(sy2-sy1)) / (dx2*dy1 - dx1*dy2); + const float u = (dy2*(sx1-sx2) + dx2*(sy2-sy1)) / (dx2*dy1 - dx1*dy2); + + //const float x = sx2 + v*dx2; + //const float y = sy2 + v*dy2; + + const float x = sx1 + u*dx1; + const float y = sy1 + u*dy1; + + if (!limit || (u >= 0 && v >= 0 && u <= 1 && v <= 1)) { + pos = Point2(x,y); + return true; + } + + return false; + +} + + #endif // LINE2D_H diff --git a/geo/Point2.h b/geo/Point2.h index a7d594a..0bf78c3 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -78,4 +78,14 @@ inline void swap(Point2& p1, Point2& p2) { std::swap(p1.y, p2.y); } +namespace std { + template <> struct hash { + std::size_t operator()(const Point2& p) const { + uint32_t x = *((uint32_t*)&(p.x)); + uint32_t y = *((uint32_t*)&(p.y)); + return std::hash()(x^y); + } + }; +} + #endif // POINT2_H diff --git a/geo/Polygon2.h b/geo/Polygon2.h new file mode 100644 index 0000000..436f8a9 --- /dev/null +++ b/geo/Polygon2.h @@ -0,0 +1,125 @@ +#ifndef POLYGON2_H +#define POLYGON2_H + +#include "Point2.h" +#include +#include "Line2.h" +#include "BBox2.h" + +static bool polygonContainsPoint(const std::vector& poly, const Point2 p); + +class Polygon2 { + + std::vector pts; + +public: + + void add(const Point2 p) { + pts.push_back(p); + } + + std::vector::iterator begin() {return pts.begin();} + std::vector::iterator end() {return pts.end();} + + std::vector::const_iterator begin() const {return pts.begin();} + std::vector::const_iterator end() const {return pts.end();} + + std::vector& getVector() {return pts;} + + Point2& operator [] (const size_t idx) { + return pts[idx]; + } + + + + /** get polygon as list of lines */ + std::vector getLines() const { + std::vector res; + for (size_t i = 0; i < pts.size(); ++i) { + const Point2 p1 = pts[(i+0)]; + const Point2 p2 = pts[(i+1)%pts.size()]; + res.push_back(Line2(p1, p2)); + } + return res; + } + + struct CutRes { + Point2 p; + size_t i1,i2; // line, given by two points, within this poly + size_t j1,j2; // line, given by two points, within other, given poly + CutRes(Point2 p, size_t i1, size_t i2, size_t j1, size_t j2) : p(p), i1(i1), i2(i2), j1(j1), j2(j2) {;} + }; + + + BBox2 getBBox() const { + BBox2 bb; + for (const Point2 p : pts) {bb.add(p);} + return bb; + } + + + bool contains(const Point2 p) const { + return polygonContainsPoint(pts, p); + } + + std::vector getIntersections(const Polygon2& o, bool limit = true) const { + + std::vector res; + for (size_t i = 0; i < pts.size(); ++i) { + const size_t i1 = (i+0); + const size_t i2 = (i+1) % pts.size(); + const Line2 l1(pts[i1], pts[i2]); + + for (size_t j = 0; j < o.pts.size(); ++j) { + const size_t j1 = (j+0); + const size_t j2 = (j+1) % o.pts.size(); + const Line2 l2(o.pts[j1], o.pts[j2]); + + Point2 p; + //if (l1.getSegmentIntersection(l2, p)) { + if (intersects(l1, l2, limit, p)) { + res.push_back(CutRes(p, i1,i2, j1,j2)); + } + } + } + + return res; + + } + +}; + +/** does the given polygon (list of points) contain the given point? */ +static bool polygonContainsPoint(const std::vector& poly, const Point2 p) { + + // https://en.wikipedia.org/wiki/Winding_number + // http://geomalgorithms.com/a03-_inclusion.html + +// BBox2 bb +// bb.grow(Point2(0.001, 0.001)); +// if (!bb.contains(p)) {return false;} + + double sum = 0; + for (size_t i = 0; i < poly.size(); ++i) { + + const Point2 p1 = poly[i]; + const Point2 p2 = poly[(i+1)%poly.size()]; + + const Point2 d1 = (p1-p).normalized(); + const Point2 d2 = (p2-p).normalized(); + + sum += std::acos(dot(d1, d2)); + + } + + // normalize (x = number of windings) + const double x = sum / M_PI / 2; + + // only look at the fractional part + //const double y = x - std::floor(x); + + return std::abs(x) >= 0.975;// && (std::abs(y) >= 0.9 || std::abs(y) < 0.1); + +} + +#endif // POLYGON2_H diff --git a/geo/Triangle3.h b/geo/Triangle3.h index ca09888..f656272 100644 --- a/geo/Triangle3.h +++ b/geo/Triangle3.h @@ -55,6 +55,24 @@ public: return *this; } + /** switch between CW<->CCW */ + void reverse() { + std::swap(p2, p3); + } + + void rotate_deg(const Point3 rot) { + p1 = p1.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI); + p2 = p2.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI); + p3 = p3.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI); + } + +// void ensureCCW() { +// Point3 norm = cross(p3-p1, p2-p1); +// if (norm.z < 0) { +// reverse(); +// } +// } + Point3 getNormal() const { return cross( p2-p1, p3-p1 ).normalized(); } diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index bf8a78c..be76a10 100644 --- a/navMesh/NavMeshFactory.h +++ b/navMesh/NavMeshFactory.h @@ -363,7 +363,7 @@ namespace NM { return false; } - bool m_keepInterResults = false; +// bool m_keepInterResults = false; bool m_filterLowHangingObstacles = false; bool m_filterLedgeSpans = false; bool m_filterWalkableLowHeightSpans = false; diff --git a/wifi/estimate/ray3/FloorPos.h b/wifi/estimate/ray3/FloorPos.h new file mode 100644 index 0000000..d175f54 --- /dev/null +++ b/wifi/estimate/ray3/FloorPos.h @@ -0,0 +1,31 @@ +#ifndef FLOORPOS_H +#define FLOORPOS_H + +namespace Ray3D { + + /** used to model ceiling thickness */ + struct FloorPos { + float fh; + float z1; + float z2; + float height; + FloorPos(const Floorplan::Floor* f) : fh(0.01), z1(f->getStartingZ()), z2(f->getEndingZ()-fh), height(z2-z1) {;} + }; + + static Obstacle3D::Type getType(const Floorplan::FloorObstacleLine* l) { + switch (l->type) { + case Floorplan::ObstacleType::WALL: return Obstacle3D::Type::WALL; + case Floorplan::ObstacleType::WINDOW: return Obstacle3D::Type::WINDOW; + case Floorplan::ObstacleType::HANDRAIL: return Obstacle3D::Type::HANDRAIL; + default: return Obstacle3D::Type::UNKNOWN; + } + } + + static Obstacle3D::Type getType(const Floorplan::FloorObstacleCircle* c) { + (void) c; + return Obstacle3D::Type::WALL; + } + +} + +#endif // FLOORPOS_H diff --git a/wifi/estimate/ray3/FloorplanMesh.h b/wifi/estimate/ray3/FloorplanMesh.h index 46ef4ba..c7b949f 100644 --- a/wifi/estimate/ray3/FloorplanMesh.h +++ b/wifi/estimate/ray3/FloorplanMesh.h @@ -205,7 +205,8 @@ namespace Ray3D { for (const Obstacle3D& obs : elements) { for (const Triangle3& tria : obs.triangles) { (void) tria; - res << "3 " << (vidx++) << " " << (vidx++) << " " << (vidx++) << "\n"; + res << "3 " << (vidx+0) << " " << (vidx+1) << " " << (vidx+2) << "\n"; + vidx += 3; } } diff --git a/wifi/estimate/ray3/ModelFactory.h b/wifi/estimate/ray3/ModelFactory.h index adbe864..9ed0ca6 100644 --- a/wifi/estimate/ray3/ModelFactory.h +++ b/wifi/estimate/ray3/ModelFactory.h @@ -10,6 +10,11 @@ #include "Tube.h" #include "Cylinder.h" #include "FloorplanMesh.h" +#include "FloorPos.h" + +#include "Walls.h" +#include "WallsViaCubes.h" +#include "WallsViaCuttedQuads.h" #include "OBJPool.h" @@ -23,7 +28,7 @@ namespace Ray3D { public: bool exportCeilings = true; - bool exportObstacles = true; + bool exportObstacles = true; bool exportStairs = true; bool fancyStairs = true; bool exportHandrails = true; @@ -39,6 +44,9 @@ namespace Ray3D { /** the to-be-exported map */ const Floorplan::IndoorMap* map; + //Walls* walls = new WallsViaCubes(); + Walls* walls = new WallsViaCuttedQuads(); + public: @@ -82,6 +90,9 @@ namespace Ray3D { if (!f->enabled) {continue;} + // reset wall-factory + walls->clear(); + // triangulize the floor itself (floor/ceiling) if (exportCeilings) { std::vector tmp = getFloor(f); @@ -96,12 +107,14 @@ namespace Ray3D { } } + // append all created walls + const std::vector& oWalls = walls->get(); + res.insert(res.end(),oWalls.begin(), oWalls.end()); + // stairs if (f->stairs.enabled) { for (const Floorplan::Stair* stair : f->stairs) { - if (exportStairs) {res.push_back(getStairs(f, stair));} - } } @@ -265,7 +278,8 @@ namespace Ray3D { case Floorplan::ObstacleType::WINDOW: return getWindow(f, fol, aboveDoor); case Floorplan::ObstacleType::WALL: - return getWall(f, fol, aboveDoor); + addWall(f, fol, aboveDoor); + return Obstacle3D(); default: throw Exception("invalid obstacle type"); } @@ -273,7 +287,8 @@ namespace Ray3D { } Obstacle3D getWindow(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const { - return getWall(f, fol, aboveDoor); + //return getWall(f, fol, aboveDoor); + return Obstacle3D(); } Obstacle3D getPillar(const Floorplan::Floor* f, const Floorplan::FloorObstacleCircle* foc) const { @@ -297,39 +312,8 @@ namespace Ray3D { } - Obstacle3D getWall(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const { - - FloorPos fpos(f); - - const float thickness_m = fol->thickness_m; - const Point2 from = (!aboveDoor) ? (fol->from) : (aboveDoor->from); - const Point2 to = (!aboveDoor) ? (fol->to) : (aboveDoor->to); - const Point2 cen2 = (from+to)/2; - - const float rad = std::atan2(to.y - from.y, to.x - from.x); - const float deg = rad * 180 / M_PI; - - // cube's destination center - const float _height = (fol->height_m > 0) ? (fol->height_m) : (fpos.height); // use either floor's height or user height - const double height = (!aboveDoor) ? (_height) : (fpos.height - aboveDoor->height); - const double cenZ = (!aboveDoor) ? (fpos.z1 + height/2) : (fpos.z1 + aboveDoor->height + height/2);// (fpos.z2 - (fpos.height - aboveDoor->height) / 2); - const Point3 pos(cen2.x, cen2.y, cenZ); - - // div by 2.01 to prevent overlapps and z-fighting - const float sx = from.getDistance(to) / 2; - const float sy = thickness_m / 2; - const float sz = height / 2.01f; // prevent overlaps - const Point3 size(sx, sy, sz); - const Point3 rot(0,0,deg); - - // build - Cube cube(pos, size, rot, cubeParts); - - // done - Obstacle3D res(getType(fol), fol->material); - res.triangles = cube.getTriangles(); - return res; - + void addWall(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) const { + walls->add(f, fol, aboveDoor); } /** 3D Obstacle from .obj 3D mesh */ @@ -351,26 +335,6 @@ namespace Ray3D { obs = obs.translated(foo->pos + Point3(0,0,fpos.z1)); obs.type = Obstacle3D::Type::OBJECT; -// std::vector trias; -// for (const OBJReader::Face& face : reader.getData().faces) { -// Point3 p1 = face.vnt[0].vertex; -// Point3 p2 = face.vnt[1].vertex; -// Point3 p3 = face.vnt[2].vertex; -// p1 = p1.rot(foo->rot.x/180.0f*M_PI, foo->rot.y/180.0f*M_PI, foo->rot.z/180.0f*M_PI); -// p2 = p2.rot(foo->rot.x/180.0f*M_PI, foo->rot.y/180.0f*M_PI, foo->rot.z/180.0f*M_PI); -// p3 = p3.rot(foo->rot.x/180.0f*M_PI, foo->rot.y/180.0f*M_PI, foo->rot.z/180.0f*M_PI); -// p1 += foo->pos; p1.z += fpos.z1; -// p2 += foo->pos; p2.z += fpos.z1; -// p3 += foo->pos; p3.z += fpos.z1; -// const Triangle3 tria(p1, p2, p3); -// trias.push_back(tria); -// } - -// // done -// Obstacle3D res(Obstacle3D::Type::OBJECT, Floorplan::Material::WOOD); -// res.triangles = trias; -// return res; - return obs; } @@ -672,30 +636,6 @@ namespace Ray3D { } */ - - static Obstacle3D::Type getType(const Floorplan::FloorObstacleLine* l) { - switch (l->type) { - case Floorplan::ObstacleType::WALL: return Obstacle3D::Type::WALL; - case Floorplan::ObstacleType::WINDOW: return Obstacle3D::Type::WINDOW; - case Floorplan::ObstacleType::HANDRAIL: return Obstacle3D::Type::HANDRAIL; - default: return Obstacle3D::Type::UNKNOWN; - } - } - - static Obstacle3D::Type getType(const Floorplan::FloorObstacleCircle* c) { - (void) c; - return Obstacle3D::Type::WALL; - } - - /** used to model ceiling thickness */ - struct FloorPos { - float fh; - float z1; - float z2; - float height; - FloorPos(const Floorplan::Floor* f) : fh(0.01), z1(f->getStartingZ()), z2(f->getEndingZ()-fh), height(z2-z1) {;} - }; - }; } diff --git a/wifi/estimate/ray3/Obstacle3.h b/wifi/estimate/ray3/Obstacle3.h index cfbfc71..720f0d3 100644 --- a/wifi/estimate/ray3/Obstacle3.h +++ b/wifi/estimate/ray3/Obstacle3.h @@ -38,6 +38,27 @@ namespace Ray3D { /** ctor */ Obstacle3D(Type type, Floorplan::Material mat) : type(type), mat(mat) {;} + + /** append a new triangle. REFERENCE ONLY VALID UNTIL NEXT ADD */ + Triangle3& addTriangle(const Point3 p1, const Point3 p2, const Point3 p3) { + triangles.push_back(Triangle3(p1, p2, p3)); + return triangles.back(); + } + + /** append a new quad by splitting into two triangles */ + void addQuad(const Point3 p1, const Point3 p2, const Point3 p3, const Point3 p4) { + addTriangle(p1, p2, p3); + addTriangle(p1, p3, p4); + } + + /** reverse all faces (CW<->CCW) */ + void reverseFaces() { + for (Triangle3& t : triangles) { + t.reverse(); + } + } + + /** scaled copy */ Obstacle3D scaled(const Point3 scale) const { Obstacle3D copy = *this; @@ -60,9 +81,10 @@ namespace Ray3D { Obstacle3D rotated_deg(const Point3 rot) const { Obstacle3D copy = *this; for (Triangle3& tria : copy.triangles) { - tria.p1 = tria.p1.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI); - tria.p2 = tria.p2.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI); - tria.p3 = tria.p3.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI); +// tria.p1 = tria.p1.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI); +// tria.p2 = tria.p2.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI); +// tria.p3 = tria.p3.rot(rot.x/180.0f*M_PI, rot.y/180.0f*M_PI, rot.z/180.0f*M_PI); + tria.rotate_deg(rot); } return copy; } diff --git a/wifi/estimate/ray3/Walls.h b/wifi/estimate/ray3/Walls.h new file mode 100644 index 0000000..067e195 --- /dev/null +++ b/wifi/estimate/ray3/Walls.h @@ -0,0 +1,22 @@ +#ifndef WALLS_H +#define WALLS_H + +#include "Obstacle3.h" + +namespace Ray3D { + + class Walls { + + public: + + virtual void clear() = 0; + + virtual void add(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) = 0; + + virtual const std::vector& get() = 0; + + }; + +} + +#endif // WALLS_H diff --git a/wifi/estimate/ray3/WallsViaCubes.h b/wifi/estimate/ray3/WallsViaCubes.h new file mode 100644 index 0000000..3b871d0 --- /dev/null +++ b/wifi/estimate/ray3/WallsViaCubes.h @@ -0,0 +1,69 @@ +#ifndef WALLSVIACUBE_H +#define WALLSVIACUBE_H + +#include "Walls.h" +#include "FloorPos.h" +#include "Cube.h" + +namespace Ray3D { + + /** + * simply use one 3D cube per wall + * if walls intersect in the 2D view, cubes will also intersect + */ + class WallsViaCubes : public Walls { + + Cube::Part cubeParts = (Cube::Part) 63; // leftright,topbottom,rearfront + + std::vector vec; + + public: + + void clear() override { + vec.clear(); + } + + void add(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) override { + + FloorPos fpos(f); + + const float thickness_m = fol->thickness_m; + const Point2 from = (!aboveDoor) ? (fol->from) : (aboveDoor->from); + const Point2 to = (!aboveDoor) ? (fol->to) : (aboveDoor->to); + const Point2 cen2 = (from+to)/2; + + const float rad = std::atan2(to.y - from.y, to.x - from.x); + const float deg = rad * 180 / M_PI; + + // cube's destination center + const float _height = (fol->height_m > 0) ? (fol->height_m) : (fpos.height); // use either floor's height or user height + const double height = (!aboveDoor) ? (_height) : (fpos.height - aboveDoor->height); + const double cenZ = (!aboveDoor) ? (fpos.z1 + height/2) : (fpos.z1 + aboveDoor->height + height/2);// (fpos.z2 - (fpos.height - aboveDoor->height) / 2); + const Point3 pos(cen2.x, cen2.y, cenZ); + + // div by 2.01 to prevent overlapps and z-fighting + const float sx = from.getDistance(to) / 2; + const float sy = thickness_m / 2; + const float sz = height / 2.01f; // prevent overlaps + const Point3 size(sx, sy, sz); + const Point3 rot(0,0,deg); + + // build + Cube cube(pos, size, rot, cubeParts); + + // done + Obstacle3D res(getType(fol), fol->material); + res.triangles = cube.getTriangles(); + vec.push_back(res); + + } + + const std::vector& get() override { + return vec; + } + + }; + +} + +#endif // WALLSVIACUBE_H diff --git a/wifi/estimate/ray3/WallsViaCuttedQuads.h b/wifi/estimate/ray3/WallsViaCuttedQuads.h new file mode 100644 index 0000000..6113cb7 --- /dev/null +++ b/wifi/estimate/ray3/WallsViaCuttedQuads.h @@ -0,0 +1,293 @@ +#ifndef WALLSVIACUTTEDQUADS_H +#define WALLSVIACUTTEDQUADS_H + +#include "../../../geo/Line2.h" +#include "../../../geo/Polygon2.h" +#include "Walls.h" + + +namespace Ray3D { + + /** + * interpret walls als quads (polygons) + * intersect them with each other to prevent overlaps + */ + class WallsViaCuttedQuads : public Walls { + + private: + + struct Wall { + + const Floorplan::FloorObstacleLine* line; + Line2 l1; + Line2 l2; + + Wall(const Floorplan::FloorObstacleLine* line) : line(line) { + + const Point2 from = line->from; + const Point2 to = line->to; + const Point2 dir = (to-from).normalized(); + const Point2 dirP = dir.perpendicular(); + const float w = line->thickness_m; + const float w2 = w/2; + + const Point2 p1 = from + dirP * w2; + const Point2 p2 = from - dirP * w2; + const Point2 p3 = to - dirP * w2; + const Point2 p4 = to + dirP * w2; + + l1 = Line2(p1, p4); + l2 = Line2(p2, p3); + + } + + Point2 getP1() const {return l1.p1;} + Point2 getP2() const {return l1.p2;} + Point2 getP3() const {return l2.p2;} + Point2 getP4() const {return l2.p1;} + + struct CutRes { + Point2 p; + Line2* l1; // within this wall + Line2* l2; // within the other wall + CutRes(Point2 p, Line2* l1, Line2* l2) : p(p), l1(l1), l2(l2) {;} + }; + + std::vector getIntersections(Wall& o, bool limit = true) { + std::vector res; + Point2 p; + if (intersects(l1, o.l1, limit, p)) {res.push_back(CutRes(p,&l1, &o.l1));} + if (intersects(l1, o.l2, limit, p)) {res.push_back(CutRes(p,&l1, &o.l2));} + if (intersects(l2, o.l1, limit, p)) {res.push_back(CutRes(p,&l2, &o.l1));} + if (intersects(l2, o.l2, limit, p)) {res.push_back(CutRes(p,&l2, &o.l2));} + return res; + } + + bool containsPoint(const Point2 p) { + return polygonContainsPoint({l1.p1, l1.p2, l2.p2, l2.p1}, p); + } + + }; + + void cropLine(Line2* l, Point2 p) { + + // determine which line-end to crop + if (p.getDistance(l->p1) < p.getDistance(l->p2)) { + l->p1 = p; + } else { + l->p2 = p; + } + + } + + std::vector walls; + const Floorplan::Floor* floor; + std::vector obs; + + public: + + + void clear() override { + walls.clear(); + } + + void add(const Floorplan::Floor* f, const Floorplan::FloorObstacleLine* fol, const Floorplan::FloorObstacleDoor* aboveDoor) override { + if (fol->type != Floorplan::ObstacleType::WALL) {return;} + if (aboveDoor) {return;} + this->floor = f; + walls.push_back(Wall(fol)); + } + + virtual const std::vector& get() override { + std::vector tmp = cut(walls); + obs = toObs(tmp, floor); + return obs; + } + + private: + + /** convert all walls to obstacles */ + std::vector toObs(const std::vector& walls, const Floorplan::Floor* f) { + std::vector res; + for (const Wall& w : walls) { + res.push_back(toObs(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 obs(getType(wall.line), 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); + const Point3 p4 = Point3(wall.getP4().x, wall.getP4().y, z1); + + const Point3 p1u = Point3(wall.getP1().x, wall.getP1().y, z2); + const Point3 p2u = Point3(wall.getP2().x, wall.getP2().y, z2); + const Point3 p3u = Point3(wall.getP3().x, wall.getP3().y, z2); + const Point3 p4u = Point3(wall.getP4().x, wall.getP4().y, z2); + + obs.addQuad(p1, p2, p2u, p1u); + obs.addQuad(p2, p3, p3u, p2u); + obs.addQuad(p3, p4, p4u, p3u); + obs.addQuad(p4, p1, p1u, p4u); + + obs.addQuad(p1u, p2u, p3u, p4u); + obs.addQuad(p4, p3, p2, p1); + + 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) { + + for (size_t i = 0; i < walls.size(); ++i) { + Wall& w1 = walls[i]; + + for (size_t j = i+1; j < walls.size(); ++j) { + Wall& w2 = walls[j]; + + if (i == j) {continue;} + + std::vector isect = w1.getIntersections(w2); + + // for (const auto c : isect) { + // out << "\n"; + // } + + // check all detected intersections + for (const auto c : isect) { + + // 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 both polygons end within the other one (this is the case for edges where two lines are conencted) -> ignore + if (p1EndsWithinP2 && p2EndsWithinP1) { + + 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; + + // 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 + + bool use45 = false; + + if (use45) { + + // farthest from center + cropLine(isect[3].l1, isect[3].p); + cropLine(isect[3].l2, isect[3].p); + + // 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); + } + + } + + } + + } + + return walls; + + } + + }; + +} + +#endif // WALLSVIACUTTEDQUADS_H