FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity
This repository has been archived on 2020-04-08. You can view files and clone it, but cannot push or open issues or pull requests.
Files
master
Indoor/navMesh/NavMeshFactory.h
kazu ac91eeaa3d changes
2018-10-25 11:50:12 +02:00

744 lines
27 KiB
C++
Raw Permalink Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

/*
* © 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 <vector>
#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 <string.h> // 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 <typename Tria> class NavMeshFactory {
private:
NavMesh<Tria>* dst = nullptr;
const NavMeshSettings& settings;
std::vector<TriangleIn> triangles;
public:
NavMeshFactory(NavMesh<Tria>* 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<Floorplan::FloorObstacleLine*>(obs);
// if (line != nullptr) {
// nmPoly.remove(getPolygon(line));
// }
// }
// std::vector<std::vector<Point3>> tmp = nmPoly.get();
// for (const std::vector<Point3>& 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<Floorplan::FloorObstacleWall*>(obs);
if (wall != nullptr) {
for (const Floorplan::Polygon2& poly : getPolygons(wall, false)) {
nmPoly.remove(poly);
}
}
// line-obstacles
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line != nullptr) {
nmPoly.remove(getPolygon(line));
}
// object-obstacles
Floorplan::FloorObstacleObject* obj = dynamic_cast<Floorplan::FloorObstacleObject*>(obs);
if (obj != nullptr) {
nmPoly.remove(getPolygon(obj));
}
}
// construct and add
std::vector<std::vector<Point3>> tmp = nmPoly.get();
int type = poly->outdoor ? (int) NavMeshType::FLOOR_OUTDOOR : (int) NavMeshType::FLOOR_INDOOR;
for (const std::vector<Point3>& 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<Floorplan::Quad3> 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<Floorplan::FloorObstacleDoor*>(obs);
if (door != nullptr) {
nmDoors.add(getPolygon(door));
}
}
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
Floorplan::FloorObstacleWall* wall = dynamic_cast<Floorplan::FloorObstacleWall*>(obs);
if (wall != nullptr) {
for (const Floorplan::Polygon2& poly : getPolygons(wall, true)) {
nmDoors.add(poly);
}
}
}
// construct and add triangles
std::vector<std::vector<Point3>> tmp = nmDoors.get();
for (const std::vector<Point3>& 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<int> tData;
std::vector<float> vData;
std::vector<uint8_t> 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<TriangleOut> 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 <tt>2 * #nvp</tt> 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<Floorplan::Polygon2> 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<Point2> 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<Floorplan::Polygon2> 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
Reference in New Issue View Git Blame Copy Permalink