Indoor/navMesh/NavMeshFactory.h

#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