FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

worked on grid-walking

Browse Source 
worked on grid-generation
added helper library for nav-meshes
started working on nav meshes
This commit is contained in:
FrankE
2018-01-08 20:55:50 +01:00
parent c346b7f222
commit ca6fed5371
33 changed files with 12991 additions and 1913 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
navMesh/NavMesh.h Normal file
View File

@@ -0,0 +1,99 @@
#ifndef NAV_MESH_H
#define NAV_MESH_H
#include "NavMeshTriangle.h"
#include <vector>
#include "../geo/BBox3.h"
#include <random>
#include "../math/DrawList.h"
#include "NavMeshRandom.h"
template <typename Tria> class NavMesh {
/** all triangles within the mesh */
std::vector<Tria> triangles;
BBox3 bbox;
public:
NavMesh() {
}
/** the overall bounding-box */
const BBox3 getBBox() const {
return bbox;
}
/** add a new triangle */
void add(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) {
triangles.push_back(Tria(p1,p2,p3,type));
bbox.add(p1);
bbox.add(p2);
bbox.add(p3);
}
/** connect both triangles */
void connectBiDir(int idx1, int idx2) {
connectUniDir(idx1,idx2);
connectUniDir(idx2,idx1);
}
/** connect both triangles */
void connectUniDir(int idxFrom, int idxTo) {
NavMeshTriangle& tria = triangles[idxFrom];
tria._neighbors[tria._numNeighbors] = idxTo;
}
/** allows for-each iteration over all included triangles */
decltype(triangles.begin()) begin() {return triangles.begin();}
/** allows for-each iteration over all included triangles */
decltype(triangles.end()) end() {return triangles.end();}
/** array access */
Tria& operator [] (const size_t idx) {
Assert::isBetween(idx, (size_t)0, getNumTriangles()-1, "index out of bounds");
return triangles[idx];
}
/** get the number of triangles used */
size_t getNumTriangles() const {
return triangles.size();
}
/** ---------------- MISC ---------------- */
NavMeshRandom<Tria> getRandomizer() {
return NavMeshRandom<Tria>(triangles);
}
/** ---------------- NEIGHBORS ---------------- */
/** get the number of neighbors for the given element */
int getNumNeighbors(const size_t idx) const {
return getNumNeighbors(triangles[idx]);
}
/** get the number of neighbors for the given element */
int getNumNeighbors(const Tria& e) const {
return e._numNeighbors;
}
/** get the n-th neighbor for the given node */
Tria& getNeighbor(const size_t nodeIdx, const size_t nth) const {
const Tria& node = triangles[nodeIdx];
return getNeighbor(node, nth);
}
/** get the n-th neighbor for the given node */
Tria& getNeighbor(const Tria& tria, const size_t nth) const {
const Tria& neighbor = triangles[tria._neighbors[nth]];
return (Tria&) neighbor;
}
};
#endif

75
navMesh/NavMeshDebug.h Normal file
View File

@@ -0,0 +1,75 @@
#ifndef NAVMESHDEBUG_H
#define NAVMESHDEBUG_H
#include "NavMesh.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
class NavMeshDebug {
public:
template <typename Tria> static void show(NavMesh<Tria>& nm) {
K::GnuplotFill gFill[3] = {
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#111111"), 1),
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaaa"), 1),
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaff"), 1)
};
K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600"));
K::Gnuplot gp;
gp << "set view equal xy\n";
K::GnuplotSplot plot;
K::GnuplotSplotElementLines lines; plot.add(&lines); lines.setShowPoints(true);
K::GnuplotSplotElementPoints points; plot.add(&points);
const BBox3 bbox = nm.getBBox();
points.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z));
points.add(K::GnuplotPoint3(bbox.getMax().x,bbox.getMax().y,bbox.getMax().z));
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(bbox.getMax().x, 0, 0));
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,bbox.getMax().y,0));
// lines.addSegment(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,0,bbox.getMax().z));
//stairs in eigene group? vlt gehen dann auch die dellen weg?
for (const Tria& tria : nm) {
uint8_t type = tria.type;
if (type < 0 || type > 2) {
throw std::runtime_error("out of type-bounds");
}
K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke);
pol->add(K::GnuplotCoordinate3(tria.p1.x, tria.p1.y, tria.p1.z, K::GnuplotCoordinateSystem::FIRST));
pol->add(K::GnuplotCoordinate3(tria.p2.x, tria.p2.y, tria.p2.z, K::GnuplotCoordinateSystem::FIRST));
pol->add(K::GnuplotCoordinate3(tria.p3.x, tria.p3.y, tria.p3.z, K::GnuplotCoordinateSystem::FIRST));
pol->close();
pol->setZIndex(tria.p3.z);
plot.getObjects().add(pol);
for (int i = 0; i < nm.getNumNeighbors(tria); ++i) {
const Tria& o = nm.getNeighbor(tria, i);
const Point3 p1 = tria.getCenter();
const Point3 p2 = o.getCenter();
//lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
}
}
plot.getObjects().reOrderByZIndex();
gp.draw(plot);
gp.flush();
sleep(1);
}
};
#endif // NAVMESHDEBUG_H

573
navMesh/NavMeshFactory.h Normal file
View File

@@ -0,0 +1,573 @@
#ifndef NAV_MESH_FACTORY_H
#define NAV_MESH_FACTORY_H
#include "../floorplan/v2/Floorplan.h"
#include "../floorplan/v2/FloorplanHelper.h"
#include "NavMesh.h"
#include "NavMeshPoly.h"
#include "NavMeshTriangle.h"
#include "../lib/Recast/Recast.h"
enum SamplePartitionType {
SAMPLE_PARTITION_WATERSHED,
SAMPLE_PARTITION_MONOTONE,
SAMPLE_PARTITION_LAYERS,
};
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;
}
};
template <typename Tria> class NavMeshFactory {
private:
NavMesh<Tria>* dst = nullptr;
std::vector<TriangleIn> triangles;
public:
NavMeshFactory(NavMesh<Tria>* dst) : dst(dst) {
}
void build(Floorplan::IndoorMap* map) {
const BBox3 bbox = FloorplanHelper::getBBox(map);
for (const Floorplan::Floor* floor : map->floors) {
add(floor);
}
fire(bbox);
}
private:
/** add one floor */
void add(const Floorplan::Floor* floor) {
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);
}
// add all stairs
for (const Floorplan::Stair* stair : floor->stairs) {
const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), floor);
for (const Floorplan::Quad3& quad : quads) {
const TriangleIn t1(quad.p1, quad.p2, quad.p3, 2); // TODO type
const TriangleIn t2(quad.p1, quad.p3, quad.p4, 2);
triangles.push_back(t1);
triangles.push_back(t2);
}
}
}
bool fire(BBox3 bbox) {
std::vector<int> tData;
std::vector<float> vData;
std::vector<uint8_t> typeData;
// 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 = 0.1f; //0.3f; // needed for 20cm walls to work!
float m_cellHeight = 0.1f; //0.2f;
float m_agentHeight = 2.0f;
float m_agentRadius = 0.1f;//0.6f;
float m_agentMaxClimb = 0.5f; // 0.9f;
float m_agentMaxSlope = 45.0f;
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.0f; //1.3f;
float m_vertsPerPoly = 3;//6.0f;
float m_detailSampleDist = 6.0f;
float m_detailSampleMaxError = 1.0f;//1.0f;
int m_partitionType = SAMPLE_PARTITION_WATERSHED;
// Init build configuration from GUI
memset(&m_cfg, 0, sizeof(m_cfg));
m_cfg.cs = m_cellSize;
m_cfg.ch = m_cellHeight;
m_cfg.walkableSlopeAngle = m_agentMaxSlope;
m_cfg.walkableHeight = (int)ceilf(m_agentHeight / m_cfg.ch);
m_cfg.walkableClimb = (int)floorf(m_agentMaxClimb / m_cfg.ch);
m_cfg.walkableRadius = (int)ceilf(m_agentRadius / m_cfg.cs);
m_cfg.maxEdgeLen = (int)(m_edgeMaxLen / m_cellSize);
m_cfg.maxSimplificationError = m_edgeMaxError;
m_cfg.minRegionArea = (int)rcSqr(m_regionMinSize); // Note: area = size*size
m_cfg.mergeRegionArea = (int)rcSqr(m_regionMergeSize); // Note: area = size*size
m_cfg.maxVertsPerPoly = (int)m_vertsPerPoly;
m_cfg.detailSampleDist = m_detailSampleDist < 0.9f ? 0 : m_cellSize * m_detailSampleDist;
m_cfg.detailSampleMaxError = m_cellHeight * m_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.
//
// 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;
// std::vector!
// if (!m_keepInterResults)
// {
// delete [] m_triareas;
// m_triareas = 0;
// }
//
// Step 3. Filter walkables surfaces.
//
// 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.
//
// 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
if (m_partitionType == 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;
}
}
else if (m_partitionType == 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;
}
}
else // 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;
}
}
//
// Step 5. Trace and simplify region contours.
//
// 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.
//
// 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.
//
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;
}
// void dump() {
// std::ofstream out("/tmp/1.dat");
// for (const std::vector<Point3> tria : mesh.get(0)) {
// for (int i = 0; i < 4; ++i) {
// const Point3 p = tria[i%3];
// out << p.x << " " << p.y << " " << p.z << "\r\n";
// }
// out << "\r\n";
// out << "\r\n";
// }
// out.close();
// K::Gnuplot gp;
// gp << "set view equal xyz\n";
// K::GnuplotSplot plot;
// K::GnuplotSplotElementLines lines; plot.add(&lines);
// lines.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(20,0,0));
// lines.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(0,20,0));
// for (const std::vector<Point3> tria : mesh.get(0)) {
// K::GnuplotFill gFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#888888"), 1);
// K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#000000"));
// K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill, gStroke);
// for (const Point3 p : tria) {
// K::GnuplotCoordinate3 coord(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST);
// pol->add(coord);
// }
// pol->close();
// plot.getObjects().add(pol);
// }
// gp.draw(plot);
// gp.flush();
// sleep(1000);
// }
/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
static Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) {
//const Line2 base(line->from*100, line->to*100);
const float thickness_m = line->thickness_m;
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;
}
};
#endif

123
navMesh/NavMeshPoly.h Normal file
View File

@@ -0,0 +1,123 @@
#ifndef POLYGON_H
#define POLYGON_H
#include <Indoor/floorplan/v2/Floorplan.h>
#include "../lib/gpc/gpc.cpp.h"
class NavMeshPoly {
struct GPCPolygon : gpc_polygon {
GPCPolygon() {
// contour = (gpc_vertex_list*) calloc(0, 1024);
// contour->num_vertices = 0;
// contour->vertex = (gpc_vertex*) calloc(0, 1024);
// hole = (int*) calloc(0, 1024);
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);
//GPCPolygon out;
gpc_polygon_clip(GPC_UNION, &state, &cur, &state);
//state = out;
}
void remove(const Floorplan::Polygon2& poly) {
GPCPolygon cur = toGPC(poly);
//GPCPolygon out;
gpc_polygon_clip(GPC_DIFF, &state, &cur, &state);
//state = out;
}
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 = 0; j < lst.num_vertices; ++j) {
// gpc_vertex& vert = lst.vertex[j];
// Point3 p3(vert.x, vert.y, z);
// tria.push_back(p3);
// }
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;
}
};
#endif // POLYGON_H

49
navMesh/NavMeshRandom.h Normal file
View File

@@ -0,0 +1,49 @@
#ifndef NAVMESHRANDOM_H
#define NAVMESHRANDOM_H
#include <random>
#include <vector>
#include "../math/DrawList.h"
#include "../geo/Point3.h"
template <typename Tria> class NavMeshRandom {
std::minstd_rand gen;
std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
const std::vector<Tria>& triangles;
DrawList<size_t> lst;
public:
struct Result {
Point3 pos;
size_t triaIdx;
Result(const Point3 pos, const size_t triaIdx) : pos(pos), triaIdx(triaIdx) {;}
};
/** ctor */
NavMeshRandom(const std::vector<Tria>& triangles) : triangles(triangles) {
for (size_t idx = 0; idx < triangles.size(); ++idx) {
lst.add(idx, triangles[idx].getArea());
}
}
/** draw a random point within the map */
Result draw() {
const size_t idx = lst.get();
const Tria& tria = triangles[idx];
while (true) {
const float u = dOnTriangle(gen);
const float v = dOnTriangle(gen);
if (u+v > 1) {continue;}
const Point3 pos = tria.getA() + (tria.getAB() * u) + (tria.getAC() * v);
return Result(pos, idx);
}
}
};
#endif // NAVMESHRANDOM_H

126
navMesh/NavMeshTriangle.h Normal file
View File

@@ -0,0 +1,126 @@
#ifndef NAVMESHTRIANGLE_H
#define NAVMESHTRIANGLE_H
#include "../geo/Point3.h"
#include "../geo/Point2.h"
class NavMeshTriangle {
public:
Point3 p1;
Point3 p2;
Point3 p3;
uint8_t type;
private:
template<typename> friend class NavMesh;
int _neighbors[3];
int _numNeighbors;
/** precalculated stuff */
private:
Point2 v0;
Point2 v1;
float dot00;
float dot01;
float dot11;
float invDenom;
float area;
const Point3 center;
const Point3 v12;
const Point3 v13;
public:
/** ctor */
NavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) :
p1(p1), p2(p2), p3(p3), type(type),
_neighbors(), _numNeighbors(0),
center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1) {
precompute();
}
bool operator == (const NavMeshTriangle& o) const {
return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3);
}
Point3 getA() const {
return p1;
}
Point3 getAB() const {
return v12;
}
Point3 getAC() const {
return v13;
}
bool contains(const Point3 p) const {
const Point2 v2 = p.xy() - p1.xy();
// Compute dot products
float dot02 = dot(v0, v2);
float dot12 = dot(v1, v2);
// Compute barycentric coordinates
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// Check if point is in triangle
return (u >= 0) && (v >= 0) && (u + v <= 1);
}
/** get the triangle's size */
float getArea() const {
return area;
}
/** get the triangle's center-point */
Point3 getCenter() const {
return center;
}
private:
/** perform some pre-calculations to speed things up */
void precompute() {
// Compute vectors
v0 = p3.xy() - p1.xy();
v1 = p2.xy() - p1.xy();
// Compute dot products
dot00 = dot(v0, v0);
dot01 = dot(v0, v1);
dot11 = dot(v1, v1);
// Compute barycentric coordinates
invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
const float a = (p2-p1).length();
const float b = (p3-p1).length();
const float c = (p2-p3).length();
const float s = 0.5f * (a+b+c);
area = std::sqrt( s * (s-a) * (s-b) * (s-c) );
}
};
#endif // NAVMESHTRIANGLE_H

43
navMesh/walk/NavMeshWalkSimple.h Normal file
View File

@@ -0,0 +1,43 @@
#ifndef NAVMESHWALKSIMPLE_H
#define NAVMESHWALKSIMPLE_H
#include "../NavMesh.h"
template <typename Tria> class NavMeshWalkSimpel {
private:
const NavMesh<Tria>& mesh;
public:
struct Location {
size_t idx;
Point3 pos;
};
struct Result {
Location loc;
};
struct Params {
Location loc;
float distance_m;
float heading_rad;
};
public:
/** ctor */
NavMeshWalkSimpel(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
Result walk(const Params& params) {
}
}
#endif // NAVMESHWALKSIMPLE_H