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
1c2081d4060649f05fce73add0b943b0bebdb86b
Indoor/navMesh/NavMeshFactory.h
frank 1c2081d406 worked on 3d models within map 
adjusted grid factory
adjusted nav mesh factory
minoor changes/fixes
new helper classes
refactoring
2018-04-03 14:55:59 +02:00

774 lines
27 KiB
C++
Raw Blame History

#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 "../wifi/estimate/ray3/OBJPool.h"
#include "NavMesh.h"
#include "NavMeshTriangle.h"
#include "NavMeshFactoryListener.h"
#include "NavMeshType.h"
#include "NavMeshSettings.h"
#include "../lib/gpc/gpc.cpp.h"
#include "../lib/Recast/Recast.h"
#include <string.h> // memset
namespace NM {
class NavMeshPoly {
struct GPCPolygon : gpc_polygon {
GPCPolygon() {
num_contours = 0;
contour = nullptr;
hole = nullptr;
}
~GPCPolygon() {
if (contour) {
gpc_free_polygon(this);
//free(contour->vertex); contour->vertex = nullptr;
}
free(contour); contour = nullptr;
free(hole); hole = nullptr;
}
GPCPolygon& operator = (const GPCPolygon& o) = delete;
GPCPolygon& operator = (GPCPolygon& o) {
this->contour = o.contour;
this->hole = o.hole;
this->num_contours = o.num_contours;
o.contour = nullptr;
o.hole = nullptr;
return *this;
}
};
private:
GPCPolygon state;
float z;
public:
NavMeshPoly(float z) : z(z) {
;
}
void add(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
gpc_polygon_clip(GPC_UNION, &state, &cur, &state);
}
void remove(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
gpc_polygon_clip(GPC_DIFF, &state, &cur, &state);
}
std::vector<std::vector<Point3>> get() {
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<std::vector<Point3>> 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) {
std::vector<Point3> tria;
gpc_vertex& v1 = lst.vertex[j-2];
gpc_vertex& v2 = lst.vertex[j-1];
gpc_vertex& v3 = lst.vertex[j];
tria.push_back(Point3(v1.x, v1.y, z));
tria.push_back(Point3(v2.x, v2.y, z));
tria.push_back(Point3(v3.x, v3.y, z));
trias.push_back(tria);
}
}
gpc_free_tristrip(&res);
return std::move(trias);
}
private:
GPCPolygon toGPC(Floorplan::Polygon2 poly) {
std::vector<gpc_vertex> verts;
for (Point2 p2 : poly.points) {
gpc_vertex vert; vert.x = p2.x; vert.y = p2.y;
verts.push_back(vert);
}
GPCPolygon gpol;
gpc_vertex_list list;
list.num_vertices = verts.size();
list.vertex = verts.data();
gpc_add_contour(&gpol, &list, 0);
return gpol;
}
};
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) {
NavMeshPoly 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) {
// 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
NavMeshPoly nmDoors(floor->atHeight);
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
Floorplan::FloorObstacleDoor* door = dynamic_cast<Floorplan::FloorObstacleDoor*>(obs);
if (door != nullptr) {
nmDoors.add(getPolygon(door));
}
}
// 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;
}
/** 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 Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(obj->file).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