From 4d740d6236a8ff92f49f33e03c14df72d6e5cd99 Mon Sep 17 00:00:00 2001 From: frank Date: Mon, 30 Jul 2018 20:59:54 +0200 Subject: [PATCH] changes to Floorplan 3D --- floorplan/3D/Builder.h | 15 +- floorplan/3D/Doors.h | 67 ++++++++ floorplan/3D/Obstacle3.h | 31 +++- floorplan/3D/WallsViaCuttedQuads.h | 259 +++++++++++++++++++++++++++-- geo/GPCPolygon2.h | 56 +------ geo/TriangleStrip3.h | 59 +++++++ navMesh/NavMeshFactory.h | 73 +++++++- 7 files changed, 484 insertions(+), 76 deletions(-) create mode 100644 floorplan/3D/Doors.h create mode 100644 geo/TriangleStrip3.h diff --git a/floorplan/3D/Builder.h b/floorplan/3D/Builder.h index ecab29a..fb9ce77 100644 --- a/floorplan/3D/Builder.h +++ b/floorplan/3D/Builder.h @@ -11,6 +11,7 @@ #include "Handrails.h" #include "Objects.h" #include "Pillars.h" +#include "Doors.h" #include "Walls.h" #include "WallsViaCubes.h" @@ -102,6 +103,8 @@ namespace Floorplan3D { res.insert(res.end(), tmp.begin(), tmp.end()); } + + if (exportHandrails) { Handrails rails; const std::vector tmp = rails.getHandrails(f); @@ -120,6 +123,12 @@ namespace Floorplan3D { res.insert(res.end(), tmp.begin(), tmp.end()); } + if (exportDoors) { + Doors doors; + const std::vector tmp = doors.getDoors(f); + res.insert(res.end(), tmp.begin(), tmp.end()); + } + // for (const Floorplan::FloorObstacle* fo : f->obstacles) { // std::vector tmp = getWalls(f); // res.insert(res.end(), tmp.begin(), tmp.end()); @@ -161,7 +170,11 @@ namespace Floorplan3D { for (const Floorplan::FloorObstacle* obs : f->obstacles) { const Floorplan::FloorObstacleLine* line = dynamic_cast(obs); - if (line) {walls.add(f, line, nullptr);} + if (line) { + if (line->type == Floorplan::ObstacleType::WALL) { + walls.add(f, line, nullptr); + } + } } return walls.get(); diff --git a/floorplan/3D/Doors.h b/floorplan/3D/Doors.h new file mode 100644 index 0000000..8537ffd --- /dev/null +++ b/floorplan/3D/Doors.h @@ -0,0 +1,67 @@ +#ifndef FLOORPLAN_3D_DOORS_H +#define FLOORPLAN_3D_DOORS_H + +#include "Walls.h" +#include "misc.h" +#include "primitives/Cube.h" + +namespace Floorplan3D { + + class Doors { + + Cube::Part cubeParts = (Cube::Part) 63; // leftright,topbottom,rearfront + + public: + + std::vector getDoors(const Floorplan::Floor* floor) { + std::vector res; + for (const Floorplan::FloorObstacle* o: floor->obstacles) { + const Floorplan::FloorObstacleWall* wall = dynamic_cast(o); + if (wall) { + for (const Floorplan::FloorObstacleWallDoor* door : wall->doors) { + res.push_back(getDoor(floor, wall, door)); + } + } + } + return res; + } + + Obstacle3D getDoor(const Floorplan::Floor* f, const Floorplan::FloorObstacleWall* wall, const Floorplan::FloorObstacleWallDoor* door) { + + FloorPos fpos(f); + + const float thickness_m = 0.1; + const Point2 from = door->getStart(wall); + const Point2 to = door->getEnd(wall); + 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 double height = door->height; + const double cenZ = (fpos.z1 + 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(Obstacle3D::Type::DOOR, door->material); + res.triangles = cube.getTriangles(); + return res; + + } + + }; + +} + +#endif // FLOORPLAN_3D_DOORS_H diff --git a/floorplan/3D/Obstacle3.h b/floorplan/3D/Obstacle3.h index 7af4d8f..2ef7ad1 100644 --- a/floorplan/3D/Obstacle3.h +++ b/floorplan/3D/Obstacle3.h @@ -4,6 +4,7 @@ #include #include "../../geo/Triangle3.h" #include "../../geo/Sphere3.h" +#include "../../geo/TriangleStrip3.h" #include "../v2/Floorplan.h" @@ -41,15 +42,35 @@ namespace Floorplan3D { /** 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)); + Triangle3& addTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const bool reverse) { + Triangle3 t(p1, p2, p3); + if (reverse) {t.reverse();} + triangles.push_back(t); return triangles.back(); } + void addTriangleStrip(std::initializer_list pts, bool reverse = false) { + TriangleStrip3 strip; + for (const Point3& p : pts) {strip.add(p);} + for (Triangle3 t : strip.toTriangles()) { + if (reverse) {t.reverse();} + triangles.push_back(t); + } + } + + void addTriangleStrip(const std::vector& pts, bool reverse = false) { + TriangleStrip3 strip; + for (const Point3& p : pts) {strip.add(p);} + for (Triangle3 t : strip.toTriangles()) { + if (reverse) {t.reverse();} + triangles.push_back(t); + } + } + /** 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); + void addQuad(const Point3 p1, const Point3 p2, const Point3 p3, const Point3 p4, const bool reverse = false) { + addTriangle(p1, p2, p3, reverse); + addTriangle(p1, p3, p4, reverse); } /** reverse all faces (CW<->CCW) */ diff --git a/floorplan/3D/WallsViaCuttedQuads.h b/floorplan/3D/WallsViaCuttedQuads.h index 4157366..fc5a4e7 100644 --- a/floorplan/3D/WallsViaCuttedQuads.h +++ b/floorplan/3D/WallsViaCuttedQuads.h @@ -167,26 +167,205 @@ namespace Floorplan3D { } - /** convert one wall into an obstacle */ - Obstacle3D toObstacle(const Wall& wall, const Floorplan::Floor* f) { + Obstacle3D toObstacle(const Wall& w, const Floorplan::Floor* f) { FloorPos fp(f); - Obstacle3D::Type type = (wall.error) ? (Obstacle3D::Type::ERROR) : (getType(wall.line)); - Obstacle3D obs(type, wall.line->material); + const Floorplan::FloorObstacleWall* wall = w.line; + Obstacle3D::Type type = (w.error) ? (Obstacle3D::Type::ERROR) : (getType(wall)); + Obstacle3D obs(type, wall->material); const float z1 = fp.z1; - const float z2 = (wall.line->height_m > 0) ? (fp.z1 + wall.line->height_m) : (fp.z2); + const float z2 = (wall->height_m > 0) ? (fp.z1 + wall->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(w.getP1().x, w.getP1().y, z1); // front + const Point3 p2 = Point3(w.getP2().x, w.getP2().y, z1); // front + const Point3 p3 = Point3(w.getP3().x, w.getP3().y, z1); // back + const Point3 p4 = Point3(w.getP4().x, w.getP4().y, z1); // back + + const Point3 p1u = Point3(w.getP1().x, w.getP1().y, z2); // front upper + const Point3 p2u = Point3(w.getP2().x, w.getP2().y, z2); // front upper + const Point3 p3u = Point3(w.getP3().x, w.getP3().y, z2); // back upper + const Point3 p4u = Point3(w.getP4().x, w.getP4().y, z2); // back upper + + const Point2 dir = (wall->to - wall->from).normalized(); + const Point2 perp = dir.perpendicular().normalized() * (wall->thickness_m/2); + + // move from wall's center line towards front (- half thickness) + auto toFront = [perp] (const Point2 pt, const float z) { + const Point2 tmp = pt + perp; + return Point3(tmp.x, tmp.y, z); + }; + + // move from wall's center line towards back (+ half thickness) + auto toBack = [perp] (const Point2 pt, const float z) { + const Point2 tmp = pt - perp; + return Point3(tmp.x, tmp.y, z); + }; + + // above window, below window, window frame + for (const Floorplan::FloorObstacleWallWindow* win : wall->windows) { + + // quad below the window (1,2,3,4) CCW + // quad above the window (5,6,7,8) CCW + // -> window = (4,3,6,5) CCW + + const float za = z1 + win->startsAtHeight; + const float zb = za + win->height; + + const Point3 p1f = toFront(win->getStart(wall), z1); + const Point3 p2f = toFront(win->getEnd(wall), z1); + const Point3 p3f = toFront(win->getEnd(wall), za); + const Point3 p4f = toFront(win->getStart(wall), za); + + const Point3 p1b = toBack(win->getStart(wall), z1); + const Point3 p2b = toBack(win->getEnd(wall), z1); + const Point3 p3b = toBack(win->getEnd(wall), za); + const Point3 p4b = toBack(win->getStart(wall), za); + + const Point3 p5f = toFront(win->getStart(wall), zb); + const Point3 p6f = toFront(win->getEnd(wall), zb); + const Point3 p7f = toFront(win->getEnd(wall), z2); + const Point3 p8f = toFront(win->getStart(wall), z2); + + const Point3 p5b = toBack(win->getStart(wall), zb); + const Point3 p6b = toBack(win->getEnd(wall), zb); + const Point3 p7b = toBack(win->getEnd(wall), z2); + const Point3 p8b = toBack(win->getStart(wall), z2); + + // quad below the window + obs.addQuad(p1f, p2f, p3f, p4f, true); // front + obs.addQuad(p1b, p2b, p3b, p4b, false); // back + + // quad above the window + obs.addQuad(p5f, p6f, p7f, p8f, true); // front + obs.addQuad(p5b, p6b, p7b, p8b, false); // back + + // window frame + obs.addTriangleStrip({p4f, p4b, p3f, p3b, p6f, p6b, p5f, p5b, p4f, p4b}); + + } + + // start mantle surface (zig-zag around the wall) + std::vector ptsLateralSurface; + ptsLateralSurface.push_back(p1); + ptsLateralSurface.push_back(p4); + + + // sort doors along the wall + std::vector doors = wall->doors; + std::sort(doors.begin(), doors.end(), [] (const Floorplan::FloorObstacleWallDoor* d1, const Floorplan::FloorObstacleWallDoor* d2) { + return d1->atLinePos < d2->atLinePos; + }); + + // above door, append lateral surface around wall + for (const Floorplan::FloorObstacleWallDoor* door : doors) { + + Point2 ps = door->getStart(wall); + Point2 pe = door->getEnd(wall); + if (door->leftRight) {std::swap(ps,pe);} + + const float zb = z1 + door->height; + + { + + const Point3 p1f = toFront(ps, zb); + const Point3 p2f = toFront(pe, zb); + const Point3 p3f = toFront(pe, z2); + const Point3 p4f = toFront(ps, z2); + + const Point3 p1b = toBack(ps, zb); + const Point3 p2b = toBack(pe, zb); + const Point3 p3b = toBack(pe, z2); + const Point3 p4b = toBack(ps, z2); + + // quad above the door (front) + obs.addQuad(p1f, p2f, p3f, p4f, true); + + // quad above the door (back) + obs.addQuad(p1b, p2b, p3b, p4b, false); + + } + + const Point3 p1f = toFront(ps, z1); + const Point3 p2f = toFront(pe, z1); + const Point3 p3f = toFront(pe, zb); + const Point3 p4f = toFront(ps, zb); + + const Point3 p1b = toBack(ps, z1); + const Point3 p2b = toBack(pe, z1); + const Point3 p3b = toBack(pe, zb); + const Point3 p4b = toBack(ps, zb); + + // continue mantle surface + ptsLateralSurface.push_back(p1f); + ptsLateralSurface.push_back(p1b); + ptsLateralSurface.push_back(p4f); + ptsLateralSurface.push_back(p4b); + ptsLateralSurface.push_back(p3f); + ptsLateralSurface.push_back(p3b); + ptsLateralSurface.push_back(p2f); + ptsLateralSurface.push_back(p2b); + + } + + // complete mantle surface + ptsLateralSurface.push_back(p2); + ptsLateralSurface.push_back(p3); + ptsLateralSurface.push_back(p2u); + ptsLateralSurface.push_back(p3u); + ptsLateralSurface.push_back(p1u); + ptsLateralSurface.push_back(p4u); + ptsLateralSurface.push_back(p1); + ptsLateralSurface.push_back(p4); + obs.addTriangleStrip(ptsLateralSurface, true); + + // get all points along the wall start, doorstart,doorend, doorstart,doorend, .., end + std::vector ptsFront; + ptsFront.push_back(p1); + ptsFront.push_back(p2); + + std::vector ptsBack; + ptsBack.push_back(p3); + ptsBack.push_back(p4); + + for (const Floorplan::FloorObstacleWallDoor* door : wall->doors) { + ptsFront.push_back(toFront(door->getStart(wall), z1)); + ptsFront.push_back(toFront(door->getEnd(wall), z1)); + ptsBack.push_back(toBack(door->getStart(wall), z1)); + ptsBack.push_back(toBack(door->getEnd(wall), z1)); + } + for (const Floorplan::FloorObstacleWallWindow* window : wall->windows) { + ptsFront.push_back(toFront(window->getStart(wall), z1)); + ptsFront.push_back(toFront(window->getEnd(wall), z1)); + ptsBack.push_back(toBack(window->getStart(wall), z1)); + ptsBack.push_back(toBack(window->getEnd(wall), z1)); + } + + // sort all points by distance from start (correct on-off-on-off-on order) + auto compFront = [p1] (const Point3 _p1, const Point3 _p2) { return p1.getDistance(_p1) < p1.getDistance(_p2); }; + std::sort(ptsFront.begin(), ptsFront.end(), compFront); + auto compBack = [p4] (const Point3 _p1, const Point3 _p2) { return p4.getDistance(_p1) < p4.getDistance(_p2); }; + std::sort(ptsBack.begin(), ptsBack.end(), compBack); + + // from wall segment to wall segment, excluding doors + + for (size_t i = 0; i < ptsFront.size(); i += 2) { + const Point3 p1 = ptsFront[i+0]; + const Point3 p2 = ptsFront[i+1]; + const Point3 p3(p2.x, p2.y, z2); + const Point3 p4(p1.x, p1.y, z2); + obs.addQuad(p1, p2, p3, p4, true); + } + + for (size_t i = 0; i < ptsBack.size(); i += 2) { + const Point3 p1 = ptsBack[i+0]; + const Point3 p2 = ptsBack[i+1]; + const Point3 p3(p2.x, p2.y, z2); + const Point3 p4(p1.x, p1.y, z2); + obs.addQuad(p1, p2, p3, p4, false); + } -// 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); @@ -198,7 +377,58 @@ namespace Floorplan3D { // obs.reverseFaces(); -// return obs; + return obs; + } + + /* + Obstacle3D toObstacle(const Wall& wall, const Floorplan::Floor* f) { + + FloorPos fp(f); + + Obstacle3D::Type type = (wall.error) ? (Obstacle3D::Type::ERROR) : (getType(wall.line)); + Obstacle3D obs(type, wall.line->material); + + const float z1 = fp.z1; + 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 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(); + + return obs; + } + */ + + + /** convert one wall into an obstacle */ + /* + Obstacle3D toObstacle(const Wall& wall, const Floorplan::Floor* f) { + + FloorPos fp(f); + + Obstacle3D::Type type = (wall.error) ? (Obstacle3D::Type::ERROR) : (getType(wall.line)); + Obstacle3D obs(type, wall.line->material); + + const float z1 = fp.z1; + const float z2 = (wall.line->height_m > 0) ? (fp.z1 + wall.line->height_m) : (fp.z2); + + Point3 p0 = Point3(wall.line->from.x, wall.line->from.y, z1); @@ -394,6 +624,7 @@ namespace Floorplan3D { return obs; } + */ void fixTria(Triangle3& t, std::function func) { t.p1 = func(t.p1); diff --git a/geo/GPCPolygon2.h b/geo/GPCPolygon2.h index cdbee8c..625b234 100644 --- a/geo/GPCPolygon2.h +++ b/geo/GPCPolygon2.h @@ -5,59 +5,7 @@ #include "Polygon2.h" #include "Triangle3.h" -class TriangleStrip { - -private: - std::vector pts; - -public: - - void add(const Point3 p) { - pts.push_back(p); - } - - void set(const std::vector& pts) { - this->pts = pts; - } - - void toTriangles(std::vector& trias) const { - - // 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. - - - - for (size_t j = 2; j < pts.size(); ++j) { - - - if (j % 2 == 0) { - Triangle3 tria( - pts[j-2], - pts[j-1], - pts[j] - ); - trias.push_back(tria); - } else { - Triangle3 tria( - pts[j-1], - pts[j-2], - pts[j] - ); - trias.push_back(tria); - } - } - - } - - std::vector toTriangles() const { - std::vector trias; - toTriangles(trias); - return trias; - } - -}; +#include "TriangleStrip3.h" class GPCPolygon2 { @@ -168,7 +116,7 @@ public: for (int i = 0; i < res.num_strips; ++i) { gpc_vertex_list lst = res.strip[i]; - TriangleStrip strip; + TriangleStrip3 strip; for (int j = 0; j < lst.num_vertices; ++j) { gpc_vertex& v = lst.vertex[j]; strip.add(Point3(v.x, v.y, z)); diff --git a/geo/TriangleStrip3.h b/geo/TriangleStrip3.h new file mode 100644 index 0000000..4bebb93 --- /dev/null +++ b/geo/TriangleStrip3.h @@ -0,0 +1,59 @@ +#ifndef TRIANGLESTRIP3_H +#define TRIANGLESTRIP3_H + +#include +#include "Point3.h" +#include "Triangle3.h" + +class TriangleStrip3 { + +private: + std::vector pts; + +public: + + void add(const Point3 p) { + pts.push_back(p); + } + + void set(const std::vector& pts) { + this->pts = pts; + } + + void toTriangles(std::vector& trias) const { + + // 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. + + for (size_t j = 2; j < pts.size(); ++j) { + + if (j % 2 == 0) { + Triangle3 tria( + pts[j-2], + pts[j-1], + pts[j] + ); + trias.push_back(tria); + } else { + Triangle3 tria( + pts[j-1], + pts[j-2], + pts[j] + ); + trias.push_back(tria); + } + } + + } + + std::vector toTriangles() const { + std::vector trias; + toTriangles(trias); + return trias; + } + +}; + +#endif // TRIANGLESTRIP3_H diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index d46454c..8e25a72 100644 --- a/navMesh/NavMeshFactory.h +++ b/navMesh/NavMeshFactory.h @@ -137,16 +137,24 @@ namespace NM { // get all obstacles of this floor and remove them from the polygon as well (many will be outside of the added polygon) for (Floorplan::FloorObstacle* obs : floor->obstacles) { + // wall-obstacles + Floorplan::FloorObstacleWall* wall = dynamic_cast(obs); + if (wall != nullptr) { + for (const Floorplan::Polygon2& poly : getPolygons(wall, false)) { + nmPoly.remove(poly); + } + } + // line-obstacles Floorplan::FloorObstacleLine* line = dynamic_cast(obs); if (line != nullptr) { - nmPoly.remove(getPolygon(line)); + nmPoly.remove(getPolygon(line)); } // object-obstacles Floorplan::FloorObstacleObject* obj = dynamic_cast(obs); if (obj != nullptr) { - nmPoly.remove(getPolygon(obj)); + nmPoly.remove(getPolygon(obj)); } } @@ -201,6 +209,15 @@ namespace NM { } } + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + Floorplan::FloorObstacleWall* wall = dynamic_cast(obs); + if (wall != nullptr) { + for (const Floorplan::Polygon2& poly : getPolygons(wall, true)) { + nmDoors.add(poly); + } + } + } + // construct and add triangles std::vector> tmp = nmDoors.get(); for (const std::vector& tria : tmp) { @@ -626,6 +643,58 @@ namespace NM { return res; } + /** create all polygons describing the walls floor outline, excluding doors */ + std::vector getPolygons(const Floorplan::FloorObstacleWall* wall, bool invert) const { + + // invert = true -> door polygons + // invert = false -> wall polygons + + const float thickness_m = std::max(wall->thickness_m, settings.maxQuality_m); // wall's thickness (make thin walls big enough to be detected) + + // get all points along the wall start, doorstart,doorend, doorstart,doorend, .., end + std::vector pts; + pts.push_back(wall->from); + pts.push_back(wall->to); + for (const Floorplan::FloorObstacleWallDoor* door : wall->doors) { + pts.push_back(door->getStart(wall)); + pts.push_back(door->getEnd(wall)); + } + + // sort all points by distance from start (correct on-off-on-off-on order) + auto comp = [wall] (const Point2 p1, const Point2 p2) { + return wall->from.getDistance(p1) < wall->from.getDistance(p2); + }; + std::sort(pts.begin(), pts.end(), comp); + + std::vector polys; + + const size_t start = (invert) ? (1) : (0); + + // from wall segment to wall segment, excluding doors + for (size_t i = start; i < pts.size()-start; i += 2) { + + const Point2 ps = pts[i+0]; + const Point2 pe = pts[i+1]; + + const Point2 dir = (pe - ps); // part's direction + const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree) + const Point2 p1 = ps + perp * thickness_m/2; // start-up + const Point2 p2 = ps - perp * thickness_m/2; // start-down + const Point2 p3 = pe + perp * thickness_m/2; // end-up + const Point2 p4 = pe - perp * thickness_m/2; // end-down + Floorplan::Polygon2 res; + res.points.push_back(p1); + res.points.push_back(p2); + res.points.push_back(p4); + res.points.push_back(p3); + polys.push_back(res); + + } + + return polys; + + } + /** convert the given 3D object to a polygon outline */ Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleObject* obj) const {