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

minor changes to floorplan

Browse Source 
fixed some compile issues
worked on nav-meshes
added some tests
This commit is contained in:
FrankE
2018-01-16 12:41:05 +01:00
parent fee6cd3496
commit 55061ef0da
24 changed files with 1288 additions and 205 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
navMesh/NavMeshDebug.h
View File

@@ -7,6 +7,7 @@
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
namespace NM {
@@ -18,27 +19,61 @@ namespace NM {
public:
template <typename Tria> static void show(NavMesh<Tria>& nm) {
K::Gnuplot gp;
K::GnuplotSplot plot;
K::GnuplotSplotElementLines lines;
K::GnuplotSplotElementPoints border;
K::GnuplotSplotElementColorPoints particles;
K::GnuplotSplotElementLines pathEstimated;
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)
};
private:
K::GnuplotFill gFill[6] = {
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#0000ff"), 1), // unknown
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"), 1), // indoor
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#44ffee"), 1), // outdoor
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666699"), 1), // door
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#444444"), 1), // stairs_level
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"), 1) // stairs_skewed
};
public:
NavMeshDebug() {
gp << "set view equal xy\n";
plot.add(&lines); lines.setShowPoints(true);
plot.add(&border);
plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2);
plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00");
}
void draw() {
gp.draw(plot);
gp.flush();
}
template <typename T> void showParticles(const std::vector<T>& particles) {
this->particles.clear();
double min = +999;
double max = -999;
for (const T& p : particles) {
const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z);
const double prob = std::pow(p.weight, 0.25);
this->particles.add(p3, prob);
if (prob > max) {max = prob;}
if (prob < min) {min = prob;}
}
plot.getAxisCB().setRange(min, max + 0.000001);
}
template <typename Tria> void addMesh(NavMesh<Tria>& nm) {
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));
border.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z));
border.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));
@@ -47,7 +82,7 @@ namespace NM {
for (const Tria* tria : nm) {
const uint8_t type = tria->getType();
if (type < 0 || type > 2) {
if (type < 0 || type > 5) {
throw std::runtime_error("out of type-bounds");
}
K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke);
@@ -75,10 +110,15 @@ namespace NM {
plot.getObjects().reOrderByZIndex();
gp.draw(plot);
gp.flush();
sleep(1);
}
void setGT(const Point3 pt) {
gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.4) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n";
}
void setCurPos(const Point3 pt) {
gp << "set arrow 31338 from " << pt.x << "," << pt.y << "," << (pt.z+0.9) << " to " << pt.x << "," << pt.y << "," << pt.z << " lw 2 lc 'green' front \n";
}
};

348
navMesh/NavMeshFactory.h
View File

@@ -6,10 +6,14 @@
#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"
namespace NM {
@@ -119,11 +123,7 @@ namespace NM {
enum SamplePartitionType {
SAMPLE_PARTITION_WATERSHED,
SAMPLE_PARTITION_MONOTONE,
SAMPLE_PARTITION_LAYERS,
};
struct TriangleIn {
Point3 p1;
@@ -150,86 +150,172 @@ namespace NM {
};
#define NMF_STEPS 8
template <typename Tria> class NavMeshFactory {
private:
float maxQuality_m = 0.20f; // 25cm elements are the smallest to-be-detected
NavMesh<Tria>* dst = nullptr;
const NavMeshSettings& settings;
std::vector<TriangleIn> triangles;
public:
NavMeshFactory(NavMesh<Tria>* dst) : dst(dst) {
NavMeshFactory(NavMesh<Tria>* dst, const NavMeshSettings& settings) : dst(dst), settings(settings) {
}
void build(Floorplan::IndoorMap* map) {
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);
fire(bbox, listener);
}
/** get the smallest obstacle size that can be detected */
float getMaxQuality_m() const {
return maxQuality_m;
}
// /** 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) {
NavMeshPoly nmPoly(floor->atHeight);
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);
}
}
for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
if (poly->method == Floorplan::OutlineMethod::REMOVE) {
nmPoly.remove(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);
}
}
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line != nullptr) {
nmPoly.remove(getPolygon(line));
}
}
// 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) {
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line != nullptr) {
nmPoly.remove(getPolygon(line));
}
}
// 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);
}
}
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
// 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);
const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), floor); // slightly grow to ensure connection?!
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);
// 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) {
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) {
@@ -282,37 +368,37 @@ namespace NM {
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 = 2.0f;
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
// 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 = 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;
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?
@@ -338,15 +424,16 @@ namespace NM {
// 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)
{
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))
{
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;
}
@@ -366,8 +453,7 @@ namespace NM {
// 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))
{
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;
}
@@ -377,18 +463,9 @@ namespace NM {
bool m_filterLedgeSpans = false;
bool m_filterWalkableLowHeightSpans = false;
// std::vector!
// if (!m_keepInterResults)
// {
// delete [] m_triareas;
// m_triareas = 0;
// }
//
// 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
@@ -401,30 +478,27 @@ namespace NM {
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)
{
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))
{
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)
{
//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))
@@ -465,103 +539,107 @@ namespace NM {
// 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)
{
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))
{
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))
{
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)
{
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))
{
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
{
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))
{
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)
{
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))
{
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)
{
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))
{
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)
{
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))
{
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)
{
//if (!m_keepInterResults) {
rcFreeCompactHeightfield(m_chf);
m_chf = 0;
rcFreeContourSet(m_cset);
m_cset = 0;
}
//}
std::vector<TriangleOut> res;
// std::vector<TriangleOut> res;
const float* orig = m_pmesh->bmin;
@@ -628,8 +706,7 @@ namespace NM {
/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) const {
//const Line2 base(line->from*100, line->to*100);
const float thickness_m = std::max(line->thickness_m, maxQuality_m); // wall's thickness (make thin walls big enough to be detected)
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
@@ -644,6 +721,23 @@ namespace NM {
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;
}
};
}

20
navMesh/NavMeshFactoryListener.h Normal file
View File

@@ -0,0 +1,20 @@
#ifndef NAVMESHFACTORYLISTENER_H
#define NAVMESHFACTORYLISTENER_H
#include <string>
namespace NM {
/** listen for events during the build process */
class NavMeshFactoryListener {
public:
virtual void onNavMeshBuildUpdateMajor(const std::string& what) = 0;
virtual void onNavMeshBuildUpdateMajor(const int cnt, const int cur) = 0;
};
}
#endif // NAVMESHFACTORYLISTENER_H

58
navMesh/NavMeshRandom.h
View File

@@ -5,6 +5,7 @@
#include <vector>
#include "../math/DrawList.h"
#include "../geo/Point3.h"
#include "../misc/PerfCheck.h"
#include "NavMeshLocation.h"
@@ -21,6 +22,7 @@ namespace NM {
DrawList<size_t> lst;
std::minstd_rand gen;
std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
std::uniform_real_distribution<float> dHeading = std::uniform_real_distribution<float>(0, M_PI*2);
std::vector<const Tria*> triangles;
@@ -34,8 +36,8 @@ namespace NM {
/** ctor (const/non-const using T) */
template <typename T> NavMeshRandom(const std::vector<T*>& srcTriangles) : lst(nextSeed()), gen(nextSeed()) {
// almost always the same number?!
gen();
// 1st = almost always the same number?!
gen(); gen();
// construct a DrawList (probability = size[area] of the triangle
// bigger triangles must be choosen more often
@@ -49,16 +51,56 @@ namespace NM {
/** draw a random point */
NavMeshLocation<Tria> draw() {
PERF_REGION(3, "NavMeshRandom::draw()");
// pick a random triangle to draw from
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->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v);
return NavMeshLocation<Tria>(pos, tria);
// get random (u,v) on triangle
float u = dOnTriangle(gen);
float v = dOnTriangle(gen);
// if the (u,v) is outside of the triangle, mirror it so its inside the triangle again
if ((u+v) > 1) {
u = 1.0f - u;
v = 1.0f - v;
}
// done
const Point3 pos = tria->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v);
return NavMeshLocation<Tria>(pos, tria);
}
/** draw a random location within the given radius */
NavMeshLocation<Tria> drawWithin(const Point3 center, const float radius) {
std::uniform_real_distribution<float> dDistance(0.001, radius);
while(true) {
const float head = dHeading(gen);
const float dist = dDistance(gen);
const float ox = std::cos(head) * dist;
const float oy = std::sin(head) * dist;
// 2D destination (ignore z)
const Point2 dst(center.x + ox, center.y + oy);
for (const Tria* t : triangles) {
// if triangle contains 2D position
if (t->contains(dst)) {
// convert it to a 3D position
const Point3 p3 = t->toPoint3(dst);
const NavMeshLocation<Tria> loc(p3, t);
return loc;
}
}
}
}

63
navMesh/NavMeshSettings.h Normal file
View File

@@ -0,0 +1,63 @@
#ifndef NAVMESHSETTINGS_H
#define NAVMESHSETTINGS_H
namespace NM {
enum class SamplePartitionType {
SAMPLE_PARTITION_WATERSHED,
SAMPLE_PARTITION_MONOTONE,
SAMPLE_PARTITION_LAYERS,
};
struct NavMeshSettings {
/** maximum resolution for outputs. nothing below this size will be detected (walls, doors, ..) */
float maxQuality_m = 0.20f;
/** height of the walking person (used to delete regions below other regions) */
float agentHeight = 1.8f;
/** radius of the walking person (used to shrink the walkable area) */
float agentRadius = 0.2f;
/** the max angle (degree) the pedestrian is able to walk */
float agentMaxSlope = 45.0f; // elevator???
/** maximal size for one triangle. too high = too many samples when walking! */
float edgeMaxLen = 10.0f;
/** higher values allow joining some small triangles */
float edgeMaxError = 1.1f; //1.3f;
/** algorithm choice */
SamplePartitionType partitionType = SamplePartitionType::SAMPLE_PARTITION_WATERSHED;
const float regionMinSize = 2;//8; // (isolated) regions smaller than this will not be rendered?!
const float regionMergeSize = 20; //??
const int vertsPerPoly = 3;//6.0f;
const float detailSampleDist = 6.0f;
const float detailSampleMaxError = 1.0f;//1.0f;
float getCellSizeXY() const {
return maxQuality_m / 2.0f;
}
float getCellSizeZ() const {
return maxQuality_m / 2.0f;
}
/** allow jumping onto stairs from the side. usually we do not want this -> set it as low as possible */
float getMaxClimb() const {
return maxQuality_m; // prevent jumping onto stairs from the side of the stair. setting this below 2xgrid-size will fail!
}
};
}
#endif // NAVMESHSETTINGS_H

48
navMesh/NavMeshTriangle.h
View File

@@ -34,7 +34,7 @@ namespace NM {
float dot00;
float dot01;
float dot11;
float invDenom;
double invDenom;
float area;
float minZ;
@@ -66,6 +66,44 @@ namespace NM {
Point3 getP3() const {return p3;}
/** get the distance between the given point and the triangle using approximate tests */
float getDistanceApx(const Point3 pt) const {
// const float d1 = pt.getDistance(p1);
// const float d2 = pt.getDistance(p2);
// const float d3 = pt.getDistance(p3);
// const float d4 = pt.getDistance(center);
// const float d5 = pt.getDistance((p1-p2)/2);
// const float d6 = pt.getDistance((p2-p3)/2);
// const float d7 = pt.getDistance((p3-p1)/2);
// return std::min(d1, std::min(d2, std::min(d3, std::min(d4, std::min(d5, std::min(d6,d7))))));
// const float d1 = pt.getDistance(p1);
// const float d2 = pt.getDistance(p2);
// const float d3 = pt.getDistance(p3);
// const float d4 = pt.getDistance(center);
// return std::min(d1, std::min(d2, std::min(d3,d4)));
float bestD = 99999;
Point3 bestP;
Point3 dir12 = p2-p1;
Point3 dir13 = p3-p1;
Point3 dir23 = p3-p2;
for (float f = 0; f < 1; f += 0.05f) {
const Point3 pos1 = p1 + dir12 * f; const float dist1 = pos1.getDistance(pt);
const Point3 pos2 = p1 + dir13 * f; const float dist2 = pos2.getDistance(pt);
const Point3 pos3 = p2 + dir23 * f; const float dist3 = pos3.getDistance(pt);
if (dist1 < bestD) {bestP = pos1; bestD = dist1;}
if (dist2 < bestD) {bestP = pos2; bestD = dist2;}
if (dist3 < bestD) {bestP = pos3; bestD = dist3;}
}
return bestD;
}
bool operator == (const NavMeshTriangle& o) const {
return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3);
}
@@ -122,7 +160,11 @@ namespace NM {
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
const Point3 res = getPoint(v,u);
return res;
Assert::isNear(res.x, p.x, 1.0f, "TODO: high difference while mapping from 2D to 3D");
Assert::isNear(res.y, p.y, 1.0f, "TODO: high difference while mapping from 2D to 3D");
//return res;
return Point3(p.x, p.y, res.z); // only use the new z, keep input as-is
}
@@ -159,7 +201,7 @@ namespace NM {
dot11 = dot(v1, v1);
// Compute barycentric coordinates
invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)dot01);

22
navMesh/NavMeshType.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef NAVMESHTYPE_H
#define NAVMESHTYPE_H
namespace NM {
enum class NavMeshType {
UNWALKABLE, // needed by Recast
FLOOR_INDOOR,
FLOOR_OUTDOOR,
DOOR,
STAIR_LEVELED, // eben
STAIR_SKEWED, // schraeg
ELEVATOR,
};
}
#endif // NAVMESHTYPE_H

33
navMesh/walk/NavMeshSub.h
View File

@@ -28,6 +28,14 @@ namespace NM {
return false;
}
/** does this submesh contain the given point? */
bool contains(const Point3 p3) const {
for (const Tria* t : toVisit) {
if (t->contains(p3)) {return true;}
}
return false;
}
/** get the triangle that contains the given point (if any) */
const Tria* getContainingTriangle(const Point2 p2) const {
for (const Tria* t : toVisit) {
@@ -37,24 +45,36 @@ namespace NM {
}
/** perform random operations on the submesh */
NavMeshRandom<Tria> getRandom() {
NavMeshRandom<Tria> getRandom() const {
return NavMeshRandom<Tria>(toVisit);
}
/** allows for-each iteration over all included triangles */
decltype(toVisit.begin()) begin() {return toVisit.begin();}
/** allows for-each iteration over all included triangles */
decltype(toVisit.end()) end() {return toVisit.end();}
private:
void build(const NavMeshLocation<Tria>& loc, float radius_m) {
PERF_REGION(6, "NavMeshSub::build()");
std::unordered_set<const Tria*> visited;
// starting-triangle + all its (max 3) neighbors
toVisit.push_back(loc.tria);
visited.insert(loc.tria);
for (const auto* n : *loc.tria) {
toVisit.push_back( (const Tria*)n );
}
// for (const auto* n : *loc.tria) {
// toVisit.push_back( (const Tria*)n );
// }
// size_t next = 1; // start with the first neighbor (skip starting triangle itself)
size_t next = 1; // start with the first neighbor (skip starting triangle itself)
size_t next = 0;
while (next < toVisit.size()) {
// next triangle
@@ -63,7 +83,8 @@ namespace NM {
// neighbors
for (const auto* n : *cur) {
const Tria* t = (const Tria*) n;
const float dist = loc.pos.getDistance(n->getCenter());
//const float dist = loc.pos.getDistance(n->getCenter());
const float dist = n->getDistanceApx(loc.pos);
if (dist > radius_m) {continue;}
if (visited.find(t) != visited.end()) {continue;}
toVisit.push_back(t);

46
navMesh/walk/NavMeshWalkEval.h
View File

@@ -4,6 +4,7 @@
#include "NavMeshWalkParams.h"
#include "../NavMeshLocation.h"
#include "../../math/Distributions.h"
#include "../../misc/PerfCheck.h"
namespace NM {
@@ -13,6 +14,10 @@ namespace NM {
NavMeshLocation<Tria> end;
NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested) : requested(requested) {
;
}
NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested, const NavMeshLocation<Tria>& end) : requested(requested), end(end) {
;
}
@@ -57,10 +62,9 @@ namespace NM {
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
if (walk.requested.start.pos == walk.end.pos) {
std::cout << "warn! start-position == end-positon" << std::endl;
return 0;
}
PERF_REGION(4, "WalkEvalHeadingStartEnd");
Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position");
const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
const float diff = head.getDiffHalfRAD(walk.requested.heading);
@@ -72,6 +76,38 @@ namespace NM {
};
/**
* evaluate the difference between head(start,end) and the requested heading
*/
template <typename Tria> class WalkEvalHeadingStartEndNormal : public NavMeshWalkEval<Tria> {
const double sigma_rad;
Distribution::Normal<double> dist;
public:
WalkEvalHeadingStartEndNormal(const double sigma_rad = 0.04) :
sigma_rad(sigma_rad), dist(0, sigma_rad) {
;
}
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
PERF_REGION(4, "WalkEvalHeadingStartEnd");
Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position");
const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
const float diff = head.getDiffHalfRAD(walk.requested.heading);
//const float diff = Heading::getSignedDiff(params.heading, head);
//return Distribution::Normal<double>::getProbability(0, sigma, diff);
return dist.getProbability(diff);
}
};
/**
* evaluate the difference between distance(start, end) and the requested distance
*/
@@ -87,6 +123,8 @@ namespace NM {
virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
PERF_REGION(5, "WalkEvalDistance");
const float requestedDistance_m = walk.requested.getToBeWalkedDistance();
const float walkedDistance_m = walk.requested.start.pos.getDistance(walk.end.pos);
const float diff = walkedDistance_m - requestedDistance_m;

26
navMesh/walk/NavMeshWalkParams.h
View File

@@ -3,6 +3,7 @@
#include "../../geo/Heading.h"
#include "../NavMeshLocation.h"
#include "../NavMeshType.h"
namespace NM {
@@ -20,12 +21,18 @@ namespace NM {
Assert::isTrue(isValid(), "invalid step-sizes given");
if (start.tria->isPlain()) {
return stepSizeFloor_m * steps;
} else {
if (start.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {
return stepSizeStair_m * steps;
} else {
return stepSizeFloor_m * steps;
}
// if (start.tria->isPlain()) {
// return stepSizeFloor_m * steps;
// } else {
// return stepSizeStair_m * steps;
// }
}
};
@@ -36,9 +43,6 @@ namespace NM {
/** walk starts here (pos/tria) */
NavMeshLocation<Tria> start;
// /** to-be-walked distance */
// float distance_m;
/** direction to walk to */
Heading heading;
@@ -54,9 +58,17 @@ namespace NM {
/** get the to-be-walked distance (steps vs. current location [stair/floor/..]) */
float getToBeWalkedDistance() const {
return stepSizes.inMeter(numSteps, start);
if (_toBeWalkedDistance != _toBeWalkedDistance) {
_toBeWalkedDistance = stepSizes.inMeter(numSteps, start);
}
return _toBeWalkedDistance;
}
private:
// precalc
mutable float _toBeWalkedDistance = NAN;
};
}

146
navMesh/walk/NavMeshWalkRandom.h Normal file
View File

@@ -0,0 +1,146 @@
#ifndef NAVMESHWALKRANDOM_H
#define NAVMESHWALKRANDOM_H
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
namespace NM {
/**
* pick a truely random destination within the reachable area
* weight this area (evaluators)
* repeat this several times to find a robus destination
*/
template <typename Tria> class NavMeshWalkRandom {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
public:
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
struct ResultList : std::vector<ResultEntry> {};
public:
/** ctor */
NavMeshWalkRandom(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) const {
ResultEntry res;
res.probability = 0;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 1.0 + toBeWalkedDist * 1.1;
// construct reachable region
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
NavMeshRandom<Tria> rnd = reachable.getRandom();
NavMeshPotentialWalk<Tria> pwalk(params);
// improve quality (the higher, the better)
for (int i = 0; i < 25; ++i) {
PERF_REGION(1, "NavMeshWalkRandom::SampleLoop");
// draw a random destination
// is this destination within the reachable area? (triangles might be larger!)
pwalk.end = rnd.draw();
if (pwalk.end.pos.getDistance(params.start.pos) > toBeWalkedDistSafe) {
--i; continue;
}
// calculate the probability for this destination
const double p = eval(pwalk);
// better?
if (p > res.probability) {
res.location = pwalk.end;
res.probability = p;
}
}
// destination is known. update the heading
res.heading = Heading(params.start.pos.xy(), res.location.pos.xy());
return res;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) const {
ResultList res;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
const float toBeWalkedDistSafe = 1.0 + toBeWalkedDist * 1.1;
// construct reachable region
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
NavMeshRandom<Tria> rnd = reachable.getRandom();
NavMeshPotentialWalk<Tria> pwalk(params);
// improve quality (the higher, the better)
for (int i = 0; i < 25; ++i) {
PERF_REGION(1, "NavMeshWalkRandom::SampleLoop");
pwalk.end = rnd.drawWithin(params.start.pos, toBeWalkedDistSafe);
// calculate the probability for this destination
const double p = eval(pwalk);
ResultEntry re;
re.heading = Heading(params.start.pos.xy(), pwalk.end.pos.xy());
re.location = pwalk.end;
re.probability = p;
res.push_back(re);
}
return res;
}
double eval(const NM::NavMeshPotentialWalk<Tria>& pwalk) const {
PERF_REGION(2, "NavMeshWalkRandom::EvalLoop");
double p = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
p *= p1;
}
return p;
}
};
}
#endif // NAVMESHWALKRANDOM_H

169
navMesh/walk/NavMeshWalkSemiRandom.h Normal file
View File

@@ -0,0 +1,169 @@
#ifndef NAVMESHWALKSEMIRANDOM_H
#define NAVMESHWALKSEMIRANDOM_H
#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"
#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"
namespace NM {
/**
* similar to NavMeshWalkRandom but:
* pick a semi random destination within the reachable area (requested distance/heading + strong deviation)
* if this destination is reachable:
* weight this area (evaluators)
* repeat this some times to find a robus destination
*/
template <typename Tria> class NavMeshWalkSemiRandom {
private:
const NavMesh<Tria>& mesh;
std::vector<NavMeshWalkEval<Tria>*> evals;
public:
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
ResultEntry() : heading(0) {;}
};
struct ResultList : public std::vector<ResultEntry> {};
public:
/** ctor */
NavMeshWalkSemiRandom(const NavMesh<Tria>& mesh) : mesh(mesh) {
}
/** add a new evaluator to the walker */
void addEvaluator(NavMeshWalkEval<Tria>* eval) {
this->evals.push_back(eval);
}
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) const {
static Distribution::Normal<float> dDist(1.0, 0.4);
static Distribution::Normal<float> dHead(0.0, 1.0);
// construct reachable region
const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1;
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
ResultEntry re;
NavMeshPotentialWalk<Tria> pwalk(params);
pwalk.end = reachable.getRandom().draw(); // to have at least a non-start solution
re.probability = eval(pwalk);
re.location = pwalk.end;
for (int i = 0; i < 25; ++i) {
const float distance = params.getToBeWalkedDistance() * dDist.draw();
const Heading head = params.heading + dHead.draw();
// only forward!
if (distance < 0.01) {continue;}
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = head.asVector();
const Point2 dst = params.start.pos.xy() + (dir * distance);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// is above destination reachable?
if (dstTria) {
pwalk.end.pos = dstTria->toPoint3(dst);
pwalk.end.tria = dstTria;
const double p = eval(pwalk);
// better?
if (p > re.probability) {
re.location = pwalk.end;
re.probability = p;
re.heading = head;
}
}
}
return re;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) const {
static Distribution::Normal<float> dDist(1.0, 0.4);
static Distribution::Normal<float> dHead(0.0, 1.0);
ResultList res;
// construct reachable region
const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1;
const NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
NavMeshPotentialWalk<Tria> pwalk(params);
for (int i = 0; i < 25; ++i) {
const float distance = params.getToBeWalkedDistance() * dDist.draw();
const Heading head = params.heading + dHead.draw();
// only forward!
if (distance < 0.01) {continue;}
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = head.asVector();
const Point2 dst = params.start.pos.xy() + (dir * distance);
const Tria* dstTria = reachable.getContainingTriangle(dst);
// is above destination reachable?
if (dstTria) {
pwalk.end.pos = dstTria->toPoint3(dst);
pwalk.end.tria = dstTria;
const double p = eval(pwalk);
ResultEntry re;
re.location = pwalk.end;
re.probability = p;
re.heading = head;
res.push_back(re);
}
}
return res;
}
double eval(const NM::NavMeshPotentialWalk<Tria>& pwalk) const {
double p = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
p *= p1;
}
return p;
}
};
}
#endif // NAVMESHWALKSEMIRANDOM_H

46
navMesh/walk/NavMeshWalkSimple.h
View File

@@ -25,16 +25,17 @@ namespace NM {
public:
struct Result {
/** single result */
struct ResultEntry {
NavMeshLocation<Tria> location;
Heading heading;
double probability;
Result() : heading(0) {;}
ResultEntry() : heading(0) {;}
};
/** list of results */
using ResultList = std::vector<ResultEntry>;
public:
/** ctor */
@@ -47,10 +48,11 @@ namespace NM {
this->evals.push_back(eval);
}
Result getDestination(const NavMeshWalkParams<Tria>& params) {
Result res;
res.heading = params.heading;
ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
ResultEntry re;
// to-be-walked distance;
const float toBeWalkedDist = params.getToBeWalkedDistance();
@@ -60,7 +62,7 @@ namespace NM {
NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
// get the to-be-reached destination's position (using start+distance+heading)
const Point2 dir = res.heading.asVector();
const Point2 dir = params.heading.asVector();
const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
const Tria* dstTria = reachable.getContainingTriangle(dst);
@@ -68,16 +70,16 @@ namespace NM {
// is above destination reachable?
if (dstTria) {
res.location.pos = dstTria->toPoint3(dst);
res.location.tria = dstTria;
re.heading = params.heading; // heading was OK -> keep
re.location.pos = dstTria->toPoint3(dst); // new destination position
re.location.tria = dstTria; // new destination triangle
++hits;
} else {
NavMeshRandom<Tria> rnd = reachable.getRandom();
NavMeshLocation<Tria> rndLoc = rnd.draw();
res.location = rndLoc;
res.heading = Heading(params.start.pos.xy(), rndLoc.pos.xy()); // update the heading
NavMeshRandom<Tria> rnd = reachable.getRandom(); // random-helper
re.location = rnd.draw(); // get a random destianation
re.heading = Heading(params.start.pos.xy(), re.location.pos.xy()); // update the heading
++misses;
}
@@ -87,17 +89,23 @@ namespace NM {
std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
}
const NavMeshPotentialWalk<Tria> pwalk(params, res.location);
res.probability = 1.0;
// calculate probability
const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
re.probability = 1.0;
for (const NavMeshWalkEval<Tria>* eval : evals) {
const double p1 = eval->getProbability(pwalk);
res.probability *= p1;
re.probability *= p1;
}
return res;
// done
return re;
}
ResultList getMany(const NavMeshWalkParams<Tria>& params) {
return {getOne(params)};
}
};