diff --git a/floorplan/3D/WallsViaCuttedQuads.h b/floorplan/3D/WallsViaCuttedQuads.h index 901c694..b91b25a 100644 --- a/floorplan/3D/WallsViaCuttedQuads.h +++ b/floorplan/3D/WallsViaCuttedQuads.h @@ -3,9 +3,12 @@ #include "../../geo/Line2.h" #include "../../geo/Polygon2.h" +#include "../../geo/GPCPolygon2.h" + #include "Walls.h" #include "misc.h" +#include #include namespace Floorplan3D { @@ -174,32 +177,133 @@ namespace Floorplan3D { Obstacle3D obs(type, wall.line->material); const float z1 = fp.z1; - const float z2 = fp.z2; + const float z2 = (wall.line->height_m > 0) ? (fp.z1 + wall.line->height_m) : (fp.z2); - 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 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); +// 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(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.addQuad(p1u, p2u, p3u, p4u); +// obs.addQuad(p4, p3, p2, p1); - obs.reverseFaces(); +// obs.reverseFaces(); + +// return obs; + + Point3 p0 = Point3(wall.line->from.x, wall.line->from.y, z1); + + Point3 p1 = Point3(wall.getP1().x, wall.getP1().y, z1); + Point3 p2 = Point3(wall.getP2().x, wall.getP2().y, z1); + Point3 p3 = Point3(wall.getP3().x, wall.getP3().y, z1); + Point3 p4 = Point3(wall.getP4().x, wall.getP4().y, z1); + + Point3 p1u = Point3(wall.getP1().x, wall.getP1().y, z2); + Point3 p2u = Point3(wall.getP2().x, wall.getP2().y, z2); + Point3 p3u = Point3(wall.getP3().x, wall.getP3().y, z2); + Point3 p4u = Point3(wall.getP4().x, wall.getP4().y, z2); + + Point3 o = p0; + float t = wall.line->thickness_m / 2; + + const Point3 o1;// = p1; + const Point3 o2;// = p4; + const float a = std::atan2(wall.getP2().y - wall.getP1().y, wall.getP2().x - wall.getP1().x); + + auto flatten = [a,o] (const Point3 p) {return (p - o).rotZ(-a).xz();}; + //auto flatten2 = [a,o] (const Point3 p) {return (p - o).rotZ(-a).xz();}; + + auto unFlattenFront = [o,t,a] (const Point3 p) {return Point3(p.x, +t, p.y).rotZ(a)+o;}; + auto unFlattenBack = [o,t,a] (const Point3 p) {return Point3(p.x, -t, p.y).rotZ(a)+o;}; + + const Point2 fp1 = flatten(p1); + const Point2 fp2 = flatten(p2); + const Point2 fp3 = flatten(p3); + const Point2 fp4 = flatten(p4); + const Point2 fp1u = flatten(p1u); + const Point2 fp2u = flatten(p2u); + const Point2 fp3u = flatten(p3u); + const Point2 fp4u = flatten(p4u); + + + Polygon2 front; + front.add({fp1, fp2, fp2u, fp1u}); + + GPCPolygon2 gpFront; + gpFront.add(front); + + Polygon2 back; + back.add({fp3, fp4, fp4u, fp3u}); + + GPCPolygon2 gpBack; + gpBack.add(back); + + for (const Floorplan::FloorObstacleWallDoor* door : wall.line->doors) { + Polygon2 pDoor; + + const Point2 pds = door->getStart(wall.line); + const Point2 pde = door->getEnd(wall.line); + + const Point3 dp1(pds.x, pds.y, z1); + const Point3 dp2(pde.x, pde.y, z1); + const Point3 dp2u(pde.x, pde.y, z1+door->height); + const Point3 dp1u(pds.x, pds.y, z1+door->height); + + //auto flattenD1 = [dp1,a] (const Point3 p) {return (p - dp1).rotZ(-a).xz();}; + //auto unFlatten1 = [dp1,a] (const Point3 p) {return Point3(p.x, 0, p.y).rotZ(a)+dp1;}; + + pDoor.add(flatten(dp1)); + pDoor.add(flatten(dp2)); + pDoor.add(flatten(dp2u)); + pDoor.add(flatten(dp1u)); + + gpFront.remove(pDoor); + gpBack.remove(pDoor); + + } + + std::vector triasFront = gpFront.getTriangles(); + + for (Triangle3 tria : triasFront) { + fixTria(tria, unFlattenFront); + tria.reverse(); + obs.triangles.push_back(tria); + } + + std::vector triasBack = gpBack.getTriangles(); + + for (Triangle3 tria : triasBack) { + fixTria(tria, unFlattenBack); + obs.triangles.push_back(tria); + } + + +// for (const Floorplan::FloorObstacleWallDoor* door : wall.line->doors) { +// const Point2 p1 = door->getStart(wall.line); +// const Point2 p2 = door->getEnd(wall.line); + +// } return obs; } + void fixTria(Triangle3& t, std::function func) { + t.p1 = func(t.p1); + t.p2 = func(t.p2); + t.p3 = func(t.p3); + } + /** cut off walls ending within another wall */ std::vector cutProtruding(std::vector walls) { diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index a90a480..a98ef2b 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -192,6 +192,7 @@ namespace Floorplan { struct Stair; struct Elevator; struct GroundTruthPoint; + struct FloorObstacleWallDoor; struct FloorOutline : public std::vector { bool enabled = true; @@ -350,6 +351,7 @@ namespace Floorplan { Point2 to; float thickness_m; float height_m = 0; // 0 = floor's height + std::vector doors; FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_m), height_m(height_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, const float height_m = 0) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m), height_m(height_m) {;} }; @@ -376,6 +378,25 @@ namespace Floorplan { float getSize() const {return (to-from).length();} }; + /** door obstacle */ + struct FloorObstacleWallDoor : public FloorObstacle { + DoorType type; + float atLinePos; + float width; + float height; + bool leftRight = false; + bool inOut = false; + FloorObstacleWallDoor(const DoorType type, const Material material, const float atLinePos, const float width, const float height) : FloorObstacle(material), type(type), atLinePos(atLinePos), width(width), height(height) {;} + Point2 getStart(const FloorObstacleLine* wall) const { + const Point2 dir = wall->to - wall->from; + return wall->from + dir * atLinePos; + } + Point2 getEnd(const FloorObstacleLine* wall) const { + const Point2 dir = wall->to - wall->from; + return getStart(wall) + dir.normalized() * (leftRight ? -width : +width); + } + }; + /** 3D obstacle */ struct FloorObstacleObject : public FloorObstacle { std::string file; diff --git a/geo/GPCPolygon2.h b/geo/GPCPolygon2.h index 9111458..5729f03 100644 --- a/geo/GPCPolygon2.h +++ b/geo/GPCPolygon2.h @@ -2,6 +2,8 @@ #define GPCPOLYGON2_H #include "../lib/gpc/gpc.cpp.h" +#include "Polygon2.h" +#include "Triangle3.h" class GPCPolygon2 { @@ -44,6 +46,11 @@ public: ; } + void add(const Polygon2& poly) { + GPCPolygon cur; + toGPC(poly, cur); + gpc_polygon_clip(GPC_UNION, &state, &cur, &state); + } void add(const Floorplan::Polygon2& poly) { GPCPolygon cur; toGPC(poly, cur); @@ -55,6 +62,11 @@ public: toGPC(poly, cur); gpc_polygon_clip(GPC_DIFF, &state, &cur, &state); } + void remove(const Polygon2& poly) { + GPCPolygon cur; + toGPC(poly, cur); + gpc_polygon_clip(GPC_DIFF, &state, &cur, &state); + } std::vector> get() { @@ -88,6 +100,57 @@ public: } + std::vector getTriangles() { + + gpc_tristrip res; + res.num_strips = 0; + res.strip = nullptr; + + //res.strip = (gpc_vertex_list*) malloc(1024); + gpc_polygon_to_tristrip(&state, &res); + + std::vector trias; + + for (int i = 0; i < res.num_strips; ++i) { + gpc_vertex_list lst = res.strip[i]; + for (int j = 2; j < lst.num_vertices; ++j) { + + gpc_vertex& v1 = lst.vertex[j - 2]; + gpc_vertex& v2 = lst.vertex[j - 1]; + gpc_vertex& v3 = lst.vertex[j]; + + // https://en.wikipedia.org/wiki/Triangle_strip + // GL_TRIANGLE_STRIP + // Draws a series of triangles (three-sided polygons) using vertices v0, v1, v2, then v2, v1, v3 (note the order), then v2, v3, v4, and so on. The ordering is to ensure that the triangles are all drawn with the same orientation so that the strip can correctly form part of a surface. + // For odd n, vertices n, n+1, and n+2 define triangle n. For even n, vertices n+1, n, and n+2 define triangle n. N-2 triangles are drawn. + + if (j % 2 == 0) { + Triangle3 tria( + Point3(v1.x, v1.y, z), + Point3(v2.x, v2.y, z), + Point3(v3.x, v3.y, z) + ); + trias.push_back(tria); + } else { + Triangle3 tria( + Point3(v2.x, v2.y, z), + Point3(v1.x, v1.y, z), + Point3(v3.x, v3.y, z) + ); + trias.push_back(tria); + } + + + } + + } + + gpc_free_tristrip(&res); + + return std::move(trias); + + } + std::vector> get(float z) { gpc_tristrip res; @@ -118,7 +181,7 @@ public: private: - void toGPC(Floorplan::Polygon2 poly, GPCPolygon& result) { + void toGPC(const Floorplan::Polygon2& poly, GPCPolygon& result) { std::vector verts; for (Point2 p2 : poly.points) { @@ -133,5 +196,20 @@ private: } + void toGPC(const Polygon2& poly, GPCPolygon& result) { + + std::vector verts; + for (Point2 p2 : poly) { + gpc_vertex vert; vert.x = p2.x; vert.y = p2.y; + verts.push_back(vert); + } + + gpc_vertex_list list; + list.num_vertices = verts.size(); + list.vertex = verts.data(); + gpc_add_contour(&result, &list, 0); + + } + }; #endif diff --git a/geo/Line2.h b/geo/Line2.h index f063e32..dea9e45 100755 --- a/geo/Line2.h +++ b/geo/Line2.h @@ -232,7 +232,7 @@ public: }; -static inline bool intersects(const Line2& l1, const Line2& l2, bool limit, Point2& pos) { +static inline bool intersects(const Line2& l1, const Line2& l2, bool limit, Point2& pos, float* _u = nullptr) { // (sx1,sy1) + (dx1, dy1)*u = (sx2, sy2) + (dx2, dy2)*v // solve((c+d*v-a)/b = (g+h*v-e)/f, v) @@ -269,6 +269,7 @@ static inline bool intersects(const Line2& l1, const Line2& l2, bool limit, Poin if (!limit || (u >= 0 && v >= 0 && u <= 1 && v <= 1)) { pos = Point2(x,y); + if (_u) {*_u = u;} return true; } @@ -277,4 +278,6 @@ static inline bool intersects(const Line2& l1, const Line2& l2, bool limit, Poin } + + #endif // LINE2D_H diff --git a/geo/Point2.h b/geo/Point2.h index 587ee11..16f79e8 100644 --- a/geo/Point2.h +++ b/geo/Point2.h @@ -85,6 +85,7 @@ inline void swap(Point2& p1, Point2& p2) { } namespace std { + template <> struct hash { std::size_t operator()(const Point2& p) const { uint32_t x = *((uint32_t*)&(p.x)); @@ -92,6 +93,7 @@ namespace std { return std::hash()(x^y); } }; + } #endif // POINT2_H diff --git a/geo/Point3.h b/geo/Point3.h index 0e2f45a..9a59283 100644 --- a/geo/Point3.h +++ b/geo/Point3.h @@ -59,6 +59,7 @@ template struct _Point3 { Point2 xy() const {return Point2(x,y);} + Point2 xz() const {return Point2(x,z);} _Point3 rotX(const Scalar r) const { diff --git a/geo/Polygon2.h b/geo/Polygon2.h index f6d76ed..c92f2b4 100644 --- a/geo/Polygon2.h +++ b/geo/Polygon2.h @@ -18,6 +18,10 @@ public: pts.push_back(p); } + void add(std::initializer_list lst) { + for (const Point2 p : lst) {add(p);} + } + std::vector::iterator begin() {return pts.begin();} std::vector::iterator end() {return pts.end();} diff --git a/tests/geo/TestBVH2.cpp b/tests/geo/TestBVH2.cpp index d3245d6..b7d460a 100644 --- a/tests/geo/TestBVH2.cpp +++ b/tests/geo/TestBVH2.cpp @@ -10,7 +10,7 @@ #include "../../floorplan/v2/Floorplan.h" #include "../../floorplan/v2/FloorplanReader.h" -#include "../../wifi/estimate/ray3/ModelFactory.h" +//#include "../../wifi/estimate/ray3/ModelFactory.h" struct WrapperBBox2 { diff --git a/tests/geo/TestBVH3.cpp b/tests/geo/TestBVH3.cpp index 5108b9f..18d9eaf 100644 --- a/tests/geo/TestBVH3.cpp +++ b/tests/geo/TestBVH3.cpp @@ -8,9 +8,9 @@ #include "../../floorplan/v2/Floorplan.h" #include "../../floorplan/v2/FloorplanReader.h" -#include "../../wifi/estimate/ray3/ModelFactory.h" +#include "../../floorplan/3D/Builder.h" -using namespace Ray3D; +using namespace Floorplan3D; struct Wrapper { @@ -140,37 +140,37 @@ TEST(BVH, tree3) { } -TEST(BVH, treeMap) { +//TEST(BVH, treeMap) { - std::string file = "/apps/SHL39.xml"; - Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); +// std::string file = "/apps/SHL39.xml"; +// Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); - ModelFactory fac(map); - fac.setExportCeilings(false); - fac.setFloors({map->floors[3]}); - FloorplanMesh mesh = fac.getMesh(); - std::vector obs = mesh.elements; +// ModelFactory fac(map); +// fac.setExportCeilings(false); +// fac.setFloors({map->floors[3]}); +// FloorplanMesh mesh = fac.getMesh(); +// std::vector obs = mesh.elements; - BVH3Debug tree; +// BVH3Debug tree; - for (const Obstacle3D& o : obs) { - tree.add(o); - } +// for (const Obstacle3D& o : obs) { +// tree.add(o); +// } - //tree.show(150); +// //tree.show(150); - //int rounds = tree.optimize(); +// //int rounds = tree.optimize(); - for (int i = 0; i < 200; ++i) { - tree.optimize(1); - //if (i%3==0) { - tree.show(250, false); - //} - } +// for (int i = 0; i < 200; ++i) { +// tree.optimize(1); +// //if (i%3==0) { +// tree.show(250, false); +// //} +// } - int i = 0; (void) i; +// int i = 0; (void) i; -} +//} TEST(BVH, treeRandom) { diff --git a/tests/ray/TestModelFac.cpp b/tests/ray/TestModelFac.cpp index 2ca8667..15875c9 100644 --- a/tests/ray/TestModelFac.cpp +++ b/tests/ray/TestModelFac.cpp @@ -1,22 +1,21 @@ #ifdef WITH_TESTS #include "../Tests.h" -#include "../../wifi/estimate/ray3/ModelFactory.h" +#include "../../floorplan/3D/Builder.h" #include "../../floorplan/v2/FloorplanReader.h" #include -using namespace Ray3D; -TEST(Ray, ModelFac) { +TEST(Floorplan3D, ModelFac) { //std::string file = "/apps/paper/diss/data/maps/SHL42_nm.xml"; std::string file = "/mnt/vm/paper/diss/data/maps/SHL42_nm.xml"; Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); - ModelFactory fac(map); + Floorplan3D::Builder fac(map); //fac.triangulize(); - FloorplanMesh mesh = fac.getMesh(); + Floorplan3D::FloorplanMesh mesh = fac.getMesh(); std::ofstream out("/tmp/fhws.ply"); out << mesh.toPLY() << std::endl; diff --git a/tests/ray/TestRayTrace3.cpp b/tests/ray/TestRayTrace3.cpp index f38f12b..806110b 100644 --- a/tests/ray/TestRayTrace3.cpp +++ b/tests/ray/TestRayTrace3.cpp @@ -18,7 +18,7 @@ TEST(RayTrace3, test) { Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file); Floorplan::AccessPoint* ap = map->floors[0]->accesspoints[4]; - ModelFactory fac(map); + Builder fac(map); std::ofstream outOBJ("/tmp/vm/map.obj"); outOBJ << fac.getMesh().toOBJ("obj").obj; outOBJ.close();