/* * © Copyright 2014 – Urheberrechtshinweis * Alle Rechte vorbehalten / All Rights Reserved * * Programmcode ist urheberrechtlich geschuetzt. * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner. * Keine Verwendung ohne explizite Genehmigung. * (vgl. § 106 ff UrhG / § 97 UrhG) */ #ifndef NAV_MESH_FACTORY_H #define NAV_MESH_FACTORY_H #include #include "../floorplan/v2/Floorplan.h" #include "../floorplan/v2/FloorplanHelper.h" #include "../geo/ConvexHull2.h" #include "../geo/GPCPolygon2.h" #include "../floorplan/3D/objects/OBJPool.h" #include "NavMesh.h" #include "NavMeshTriangle.h" #include "NavMeshFactoryListener.h" #include "NavMeshType.h" #include "NavMeshSettings.h" #include "../lib/Recast/Recast.h" #include // memset namespace NM { struct TriangleIn { Point3 p1; Point3 p2; Point3 p3; uint8_t type; TriangleIn(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : p1(p1), p2(p2), p3(p3), type(type) {;} }; struct TriangleOut { Point3 p1; Point3 p2; Point3 p3; int numNeighbors = 0; int neighbors[3]; // each triangle has max 3 neighbors TriangleOut(const Point3 p1, const Point3 p2, const Point3 p3) : p1(p1), p2(p2), p3(p3), neighbors() {;} Point3 center() const { return (p1+p2+p3) / 3; } }; #define NMF_STEPS 8 template class NavMeshFactory { private: NavMesh* dst = nullptr; const NavMeshSettings& settings; std::vector triangles; public: NavMeshFactory(NavMesh* dst, const NavMeshSettings& settings) : dst(dst), settings(settings) { } void build(Floorplan::IndoorMap* map, NavMeshFactoryListener* listener = nullptr) { if (listener) {listener->onNavMeshBuildUpdateMajor("preparing");} if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 0);} const BBox3 bbox = FloorplanHelper::getBBox(map); for (const Floorplan::Floor* floor : map->floors) { add(floor); } fire(bbox, listener); } // /** get the smallest obstacle size that can be detected */ // float getMaxQuality_m() const { // return maxQuality_m; // } private: /** add one floor */ void add(const Floorplan::Floor* floor) { if (!floor->enabled) {return;} // NavMeshPoly nmPoly(floor->atHeight); // for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { // if (poly->method == Floorplan::OutlineMethod::ADD) { // nmPoly.add(poly->poly); // } // } // for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { // if (poly->method == Floorplan::OutlineMethod::REMOVE) { // nmPoly.remove(poly->poly); // } // } // for (Floorplan::FloorObstacle* obs : floor->obstacles) { // Floorplan::FloorObstacleLine* line = dynamic_cast(obs); // if (line != nullptr) { // nmPoly.remove(getPolygon(line)); // } // } // std::vector> tmp = nmPoly.get(); // for (const std::vector& tria : tmp) { // const TriangleIn t(tria[0], tria[1], tria[2], 1; // TODO outdoor // triangles.push_back(t); // } // we need this strange loop, as we need to distinguish between indoor and outdoor regions/polygons // adding all "add" polygons first and removing "remove" polygons / obstacles afterwards is more performant // but does not allow for tagging the "add" polygons (indoor/outdoor/...) // thats why we have to tread each "add" polygon on its own (and remove all potential elements from it) for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { // if this is a to-be-added polygon, add it if (poly->method == Floorplan::OutlineMethod::ADD) { GPCPolygon2 nmPoly(floor->atHeight); nmPoly.add(poly->poly); // get all other polygons of this floor, that are tagged as "remove" and remove them (many will be outside of the added polygon) for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { if (poly->method == Floorplan::OutlineMethod::REMOVE) { nmPoly.remove(poly->poly); } } // 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)); } // object-obstacles Floorplan::FloorObstacleObject* obj = dynamic_cast(obs); if (obj != nullptr) { nmPoly.remove(getPolygon(obj)); } } // construct and add std::vector> tmp = nmPoly.get(); int type = poly->outdoor ? (int) NavMeshType::FLOOR_OUTDOOR : (int) NavMeshType::FLOOR_INDOOR; for (const std::vector& tria : tmp) { const TriangleIn t(tria[0], tria[1], tria[2], type); triangles.push_back(t); } } } // add all stairs // those must be DIRECTLY connected to the ending floor (stair's ending edge connected to an edge of the floor) // otherwise the stair ends UNDER a floor polygon and is thus not added (higher polygons always win) for (const Floorplan::Stair* stair : floor->stairs) { const std::vector quads = Floorplan::getQuads(stair->getParts(), floor); // slightly grow to ensure connection?! for (const Floorplan::Quad3& quad : quads) { // stair has two options: either leveled parts (no steps) and skewed parts (steps) // as those affect the pedestrian's step-length, we tag them differently const int type = quad.isLeveled() ? (int) NavMeshType::STAIR_LEVELED : (int) NavMeshType::STAIR_SKEWED; const TriangleIn t1(quad.p1, quad.p2, quad.p3, type); const TriangleIn t2(quad.p1, quad.p3, quad.p4, type); triangles.push_back(t1); triangles.push_back(t2); // sanity check. should never happen. just to be ultra sure const Point3 norm1 = cross((t1.p2-t1.p1), (t1.p3-t1.p1)); const Point3 norm2 = cross((t2.p2-t2.p1), (t2.p3-t2.p1)); Assert::isTrue(norm1.z > 0, "detected invalid culling for stair-quad. normal points downwards"); Assert::isTrue(norm2.z > 0, "detected invalid culling for stair-quad. normal points downwards"); } } // finally create additional triangles for the doors to tag doors differently (tagging also seems to improve the triangulation result) // note: door-regions are already walkable as doors are NOT removed from the outline // however: adding them again here seems to work.. triangles at the end of the list seem to overwrite (tagging) previous ones -> fine { // add (overlay) all doors for tagging them within the plan GPCPolygon2 nmDoors(floor->atHeight); for (Floorplan::FloorObstacle* obs : floor->obstacles) { Floorplan::FloorObstacleDoor* door = dynamic_cast(obs); if (door != nullptr) { nmDoors.add(getPolygon(door)); } } 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) { const TriangleIn t(tria[0], tria[1], tria[2], (int) NavMeshType::DOOR); triangles.push_back(t); } } } bool fire(BBox3 bbox, NavMeshFactoryListener* listener) { std::vector tData; std::vector vData; std::vector typeData; if (listener) {listener->onNavMeshBuildUpdateMajor("building polygons");} if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 1);} // floor outlines for (const TriangleIn& t : triangles) { // swap YZ and polygon order int startVert = vData.size() / 3; // invert triangle ? (CW vs CCW) // ensure normal points UP const Point3 norm = cross((t.p2-t.p1), (t.p3-t.p1)); if (norm.z > 0) { tData.push_back(startVert + 0); tData.push_back(startVert + 2); tData.push_back(startVert + 1); } else { tData.push_back(startVert + 0); tData.push_back(startVert + 1); tData.push_back(startVert + 2); } typeData.push_back(t.type); vData.push_back(t.p1.x); vData.push_back(t.p1.z); vData.push_back(t.p1.y); vData.push_back(t.p2.x); vData.push_back(t.p2.z); vData.push_back(t.p2.y); vData.push_back(t.p3.x); vData.push_back(t.p3.z); vData.push_back(t.p3.y); } unsigned char* m_triareas = typeData.data(); const float* verts = vData.data(); const int* tris = tData.data(); int ntris = tData.size() / 3; int nverts = vData.size() / 3; //unsigned char* m_triareas; rcHeightfield* m_solid; rcCompactHeightfield* m_chf; rcContourSet* m_cset; rcPolyMesh* m_pmesh; rcConfig m_cfg; rcPolyMeshDetail* m_dmesh; rcContext* m_ctx = new rcContext(); // float m_cellSize = maxQuality_m/2.0f; //0.3f; // ensure quality is enough to fit maxQuality_m // float m_cellHeight = maxQuality_m/2.0f; //0.2f; // float m_agentHeight = 1.8f; // float m_agentRadius = 0.2f;//0.6f; // float m_agentMaxClimb = maxQuality_m; // 0.9f; // prevent jumping onto stairs from the side of the stair. setting this below 2xgrid-size will fail! // float m_agentMaxSlope = 45.0f; // elevator??? // float m_regionMinSize = 2;//8; // float m_regionMergeSize = 20; // float m_edgeMaxLen = 10.0f; // maximal size for one triangle. too high = too many samples when walking! // float m_edgeMaxError = 1.1f; //1.3f; // higher values allow joining some small triangles // float m_vertsPerPoly = 3;//6.0f; // float m_detailSampleDist = 6.0f; // float m_detailSampleMaxError = 1.0f;//1.0f; // int m_partitionType = SAMPLE_PARTITION_WATERSHED; // SAMPLE_PARTITION_WATERSHED SAMPLE_PARTITION_MONOTONE SAMPLE_PARTITION_LAYERS // Init build configuration from GUI memset(&m_cfg, 0, sizeof(m_cfg)); m_cfg.cs = settings.getCellSizeXY(); m_cfg.ch = settings.getCellSizeZ(); m_cfg.walkableSlopeAngle = settings.agentMaxSlope; m_cfg.walkableHeight = (int)ceilf(settings.agentHeight / m_cfg.ch); m_cfg.walkableClimb = (int)floorf(settings.getMaxClimb() / m_cfg.ch); m_cfg.walkableRadius = (int)ceilf(settings.agentRadius / m_cfg.cs); m_cfg.maxEdgeLen = (int)(settings.edgeMaxLen / settings.getCellSizeXY()); m_cfg.maxSimplificationError = settings.edgeMaxError; m_cfg.minRegionArea = (int)rcSqr(settings.regionMinSize); // Note: area = size*size m_cfg.mergeRegionArea = (int)rcSqr(settings.regionMergeSize); // Note: area = size*size m_cfg.maxVertsPerPoly = settings.vertsPerPoly; m_cfg.detailSampleDist = settings.detailSampleDist < 0.9f ? 0 : settings.getCellSizeXY() * settings.detailSampleDist; m_cfg.detailSampleMaxError = settings.getCellSizeZ() * settings.detailSampleMaxError; float bmin[3] = {bbox.getMin().x, bbox.getMin().z, bbox.getMin().y}; float bmax[3] = {bbox.getMax().x, bbox.getMax().z, bbox.getMax().y};// x/z swapped? // Set the area where the navigation will be build. // Here the bounds of the input mesh are used, but the // area could be specified by an user defined box, etc. rcVcopy(m_cfg.bmin, bmin); rcVcopy(m_cfg.bmax, bmax); rcCalcGridSize(m_cfg.bmin, m_cfg.bmax, m_cfg.cs, &m_cfg.width, &m_cfg.height); // Reset build times gathering. m_ctx->resetTimers(); // Start the build process. m_ctx->startTimer(RC_TIMER_TOTAL); m_ctx->log(RC_LOG_PROGRESS, "Building navigation:"); m_ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", m_cfg.width, m_cfg.height); m_ctx->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", nverts/1000.0f, ntris/1000.0f); // // Step 2. Rasterize input polygon soup. // if (listener) {listener->onNavMeshBuildUpdateMajor("rasterizing polygons");} if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 2);} // Allocate voxel heightfield where we rasterize our input data to. m_solid = rcAllocHeightfield(); if (!m_solid) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'."); return false; } if (!rcCreateHeightfield(m_ctx, *m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield."); return false; } // Allocate array that can hold triangle area types. // If you have multiple meshes you need to process, allocate // and array which can hold the max number of triangles you need to process. // m_triareas = new unsigned char[ntris]; // if (!m_triareas) // { // m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", ntris); // return false; // } // Find triangles which are walkable based on their slope and rasterize them. // If your input data is multiple meshes, you can transform them here, calculate // the are type for each of the meshes and rasterize them. //memset(m_triareas, 0, ntris*sizeof(unsigned char)); //rcMarkWalkableTriangles(m_ctx, m_cfg.walkableSlopeAngle, verts, nverts, tris, ntris, m_triareas); if (!rcRasterizeTriangles(m_ctx, verts, nverts, tris, m_triareas, ntris, *m_solid, m_cfg.walkableClimb)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not rasterize triangles."); return false; } // bool m_keepInterResults = false; bool m_filterLowHangingObstacles = false; bool m_filterLedgeSpans = false; bool m_filterWalkableLowHeightSpans = false; // Step 3. Filter walkables surfaces. if (listener) {listener->onNavMeshBuildUpdateMajor("filtering");} if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 3);} // Once all geoemtry is rasterized, we do initial pass of filtering to // remove unwanted overhangs caused by the conservative rasterization // as well as filter spans where the character cannot possibly stand. if (m_filterLowHangingObstacles) rcFilterLowHangingWalkableObstacles(m_ctx, m_cfg.walkableClimb, *m_solid); if (m_filterLedgeSpans) rcFilterLedgeSpans(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid); if (m_filterWalkableLowHeightSpans) rcFilterWalkableLowHeightSpans(m_ctx, m_cfg.walkableHeight, *m_solid); // Step 4. Partition walkable surface to simple regions. if (listener) {listener->onNavMeshBuildUpdateMajor("partitioning");} if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 4);} // Compact the heightfield so that it is faster to handle from now on. // This will result more cache coherent data as well as the neighbours // between walkable cells will be calculated. m_chf = rcAllocCompactHeightfield(); if (!m_chf) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'."); return false; } if (!rcBuildCompactHeightfield(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data."); return false; } //if (!m_keepInterResults) { rcFreeHeightField(m_solid); m_solid = 0; //} // Erode the walkable area by agent radius. if (!rcErodeWalkableArea(m_ctx, m_cfg.walkableRadius, *m_chf)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode."); return false; } // (Optional) Mark areas. // const ConvexVolume* vols = m_geom->getConvexVolumes(); // for (int i = 0; i < m_geom->getConvexVolumeCount(); ++i) // rcMarkConvexPolyArea(m_ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf); // Partition the heightfield so that we can use simple algorithm later to triangulate the walkable areas. // There are 3 martitioning methods, each with some pros and cons: // 1) Watershed partitioning // - the classic Recast partitioning // - creates the nicest tessellation // - usually slowest // - partitions the heightfield into nice regions without holes or overlaps // - the are some corner cases where this method creates produces holes and overlaps // - holes may appear when a small obstacles is close to large open area (triangulation can handle this) // - overlaps may occur if you have narrow spiral corridors (i.e stairs), this make triangulation to fail // * generally the best choice if you precompute the nacmesh, use this if you have large open areas // 2) Monotone partioning // - fastest // - partitions the heightfield into regions without holes and overlaps (guaranteed) // - creates long thin polygons, which sometimes causes paths with detours // * use this if you want fast navmesh generation // 3) Layer partitoining // - quite fast // - partitions the heighfield into non-overlapping regions // - relies on the triangulation code to cope with holes (thus slower than monotone partitioning) // - produces better triangles than monotone partitioning // - does not have the corner cases of watershed partitioning // - can be slow and create a bit ugly tessellation (still better than monotone) // if you have large open areas with small obstacles (not a problem if you use tiles) // * good choice to use for tiled navmesh with medium and small sized tiles switch (settings.partitionType) { case SamplePartitionType::SAMPLE_PARTITION_WATERSHED: // Prepare for region partitioning, by calculating distance field along the walkable surface. if (!rcBuildDistanceField(m_ctx, *m_chf)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field."); return false; } // Partition the walkable surface into simple regions without holes. if (!rcBuildRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build watershed regions."); return false; } break; case SamplePartitionType::SAMPLE_PARTITION_MONOTONE: // Partition the walkable surface into simple regions without holes. // Monotone partitioning does not need distancefield. if (!rcBuildRegionsMonotone(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build monotone regions."); return false; } break; case SamplePartitionType::SAMPLE_PARTITION_LAYERS: // Partition the walkable surface into simple regions without holes. if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build layer regions."); return false; } break; default: throw Exception("unsupported SamplePartitionType"); } // Step 5. Trace and simplify region contours. if (listener) {listener->onNavMeshBuildUpdateMajor("tracing");} if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 5);} // Create contours. m_cset = rcAllocContourSet(); if (!m_cset) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'."); return false; } if (!rcBuildContours(m_ctx, *m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create contours."); return false; } // // Step 6. Build polygons mesh from contours. if (listener) {listener->onNavMeshBuildUpdateMajor("building triangles");} if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 6);} // Build polygon navmesh from the contours. m_pmesh = rcAllocPolyMesh(); if (!m_pmesh) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'."); return false; } if (!rcBuildPolyMesh(m_ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours."); return false; } // // Step 7. Create detail mesh which allows to access approximate height on each polygon. if (listener) {listener->onNavMeshBuildUpdateMajor("building details");} if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 7);} m_dmesh = rcAllocPolyMeshDetail(); if (!m_dmesh) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmdtl'."); return false; } if (!rcBuildPolyMeshDetail(m_ctx, *m_pmesh, *m_chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *m_dmesh)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build detail mesh."); return false; } //if (!m_keepInterResults) { rcFreeCompactHeightfield(m_chf); m_chf = 0; rcFreeContourSet(m_cset); m_cset = 0; //} // std::vector res; const float* orig = m_pmesh->bmin; // https://github.com/recastnavigation/recastnavigation/blob/master/Docs/Extern/Recast_api.txt for (int i = 0; i < m_pmesh->npolys; ++i) { const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2]; const uint8_t type = m_pmesh->areas[i]; // Each entry is 2 * #nvp in length. The first half of the entry // contains the indices of the polygon. The first instance of #RC_MESH_NULL_IDX // indicates the end of the indices for the entry. The second half contains // indices to neighbor polygons. A value of #RC_MESH_NULL_IDX indicates no // connection for the associated edge. (I.e. The edge is a solid border.) // we only use exactly 3 vertices per polygon, no iteration needed // for (int j = 0; j < m_pmesh->nvp; ++j) { // if (p[j] == RC_MESH_NULL_IDX) {break;} // const unsigned short* v = &m_pmesh->verts[p[j]*3]; // const float x = orig[0] + v[0]*m_pmesh->cs; // const float z = orig[1] + v[1]*m_pmesh->ch; // const float y = orig[2] + v[2]*m_pmesh->cs; // pol->add(K::GnuplotCoordinate3(x, y, z, K::GnuplotCoordinateSystem::FIRST)); // } // un-swap Y/Z const unsigned short* v0 = &m_pmesh->verts[p[0]*3]; const Point3 p0(orig[0] + v0[0]*m_pmesh->cs, orig[2] + v0[2]*m_pmesh->cs, orig[1] + v0[1]*m_pmesh->ch); const unsigned short* v1 = &m_pmesh->verts[p[1]*3]; const Point3 p1(orig[0] + v1[0]*m_pmesh->cs, orig[2] + v1[2]*m_pmesh->cs, orig[1] + v1[1]*m_pmesh->ch); const unsigned short* v2 = &m_pmesh->verts[p[2]*3]; const Point3 p2(orig[0] + v2[0]*m_pmesh->cs, orig[2] + v2[2]*m_pmesh->cs, orig[1] + v2[1]*m_pmesh->ch); dst->add(p0,p1,p2,type); } // now, connect neighbors for (int i = 0; i < m_pmesh->npolys; ++i) { const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2]; // find all neighbor polygons using their index for (int j = 0; j < m_pmesh->nvp; ++j) { int jj = j + m_pmesh->nvp; // offset, 2nd half of the array [size: 2*nvp] if (p[jj] == RC_MESH_NULL_IDX) {continue;} // no neighbor for the current edge! const int idx = p[jj]; dst->connectUniDir(i, idx); } } return true; } /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) const { const float thickness_m = std::max(line->thickness_m, settings.maxQuality_m); // wall's thickness (make thin walls big enough to be detected) 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 Floorplan::Polygon2 res; res.points.push_back(p1); res.points.push_back(p2); res.points.push_back(p4); res.points.push_back(p3); 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 { Floorplan::Polygon2 res; // fetch object from pool const Floorplan3D::Obstacle3D obs = Floorplan3D::OBJPool::get().getObject(obj->file).scaled(obj->scale).rotated_deg(obj->rot).translated(obj->pos); // construct 2D convex hull res.points = ConvexHull2::get(obs.getPoints2D()); return res; } /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleDoor* door) const { const float thickness_m = std::max(0.3f, settings.maxQuality_m); // wall's thickness (make thin walls big enough to be detected) const Point2 dir = (door->to - door->from); // obstacle's direction const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree) const Point2 p1 = door->from + perp * thickness_m/2; // start-up const Point2 p2 = door->from - perp * thickness_m/2; // start-down const Point2 p3 = door->to + perp * thickness_m/2; // end-up const Point2 p4 = door->to - 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); return res; } }; } #endif