newly arranged classes

makepanda/makepanda.vcproj

@@ -649,12 +649,14 @@
 				<File RelativePath="..\panda\src\vision\arToolKit.cxx"></File>
 			</Filter>
 			<Filter Name="navigation">
-				<File RelativePath="..\panda\src\navigation\InputGeom.cxx"></File>
-				<File RelativePath="..\panda\src\navigation\InputGeom.h"></File>
-				<File RelativePath="..\panda\src\navigation\MeshLoaderObj.cxx"></File>
-				<File RelativePath="..\panda\src\navigation\MeshLoaderObj.h"></File>
-				<File RelativePath="..\panda\src\navigation\NavMeshSample.cxx"></File>
-				<File RelativePath="..\panda\src\navigation\NavMeshSample.h"></File>
+				<File RelativePath="..\panda\src\navigation\navMesh.cxx"></File>
+				<File RelativePath="..\panda\src\navigation\navMesh.h"></File>
+				<File RelativePath="..\panda\src\navigation\navMeshBuilder.cxx"></File>
+				<File RelativePath="..\panda\src\navigation\navMeshBuilder.h"></File>
+				<File RelativePath="..\panda\src\navigation\navMeshNode.cxx"></File>
+				<File RelativePath="..\panda\src\navigation\navMeshNode.h"></File>
+				<File RelativePath="..\panda\src\navigation\navMeshQuery.cxx"></File>
+				<File RelativePath="..\panda\src\navigation\navMeshQuery.h"></File>
 				<File RelativePath="..\panda\src\navigation\Detour\Source\DetourAlloc.cpp"></File>
 				<File RelativePath="..\panda\src\navigation\Detour\Source\DetourAssert.cpp"></File>
 				<File RelativePath="..\panda\src\navigation\Detour\Source\DetourCommon.cpp"></File>

panda/src/navigation/InputGeom.cxx

@@ -1,596 +0,0 @@
-
-#define _USE_MATH_DEFINES
-
-#include <math.h>
-#include <stdio.h>
-#include <ctype.h>
-#include <string.h>
-#include <algorithm>
-
-#ifndef CPPPARSER
-#include "Recast.h"
-#include "DebugDraw.h"
-#include "RecastDebugDraw.h"
-#include "DetourNavMesh.h"
-#endif
-#include "MeshLoaderObj.h"
-#include "InputGeom.h"
-#include "NavMeshSample.h"
-#include "nodePath.h"
-
-static bool intersectSegmentTriangle(const float* sp, const float* sq,
-	const float* a, const float* b, const float* c,
-	float &t)
-{
-	float v, w;
-	float ab[3], ac[3], qp[3], ap[3], norm[3], e[3];
-	rcVsub(ab, b, a);
-	rcVsub(ac, c, a);
-	rcVsub(qp, sp, sq);
-
-	// Compute triangle normal. Can be precalculated or cached if
-	// intersecting multiple segments against the same triangle
-	rcVcross(norm, ab, ac);
-
-	// Compute denominator d. If d <= 0, segment is parallel to or points
-	// away from triangle, so exit early
-	float d = rcVdot(qp, norm);
-	if (d <= 0.0f) return false;
-
-	// Compute intersection t value of pq with plane of triangle. A ray
-	// intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
-	// dividing by d until intersection has been found to pierce triangle
-	rcVsub(ap, sp, a);
-	t = rcVdot(ap, norm);
-	if (t < 0.0f) return false;
-	if (t > d) return false; // For segment; exclude this code line for a ray test
-
-	// Compute barycentric coordinate components and test if within bounds
-	rcVcross(e, qp, ap);
-	v = rcVdot(ac, e);
-	if (v < 0.0f || v > d) return false;
-	w = -rcVdot(ab, e);
-	if (w < 0.0f || v + w > d) return false;
-
-	// Segment/ray intersects triangle. Perform delayed division
-	t /= d;
-
-	return true;
-}
-
-static char* parse_Row(char* buf, char* bufEnd, char* row, int len)
-{
-	bool start = true;
-	bool done = false;
-	int n = 0;
-	while (!done && buf < bufEnd)
-	{
-		char c = *buf;
-		buf++;
-		// multirow
-		switch (c)
-		{
-		case '\n':
-			if (start) break;
-			done = true;
-			break;
-		case '\r':
-			break;
-		case '\t':
-		case ' ':
-			if (start) break;
-			// else falls through
-		default:
-			start = false;
-			row[n++] = c;
-			if (n >= len - 1)
-				done = true;
-			break;
-		}
-	}
-	row[n] = '\0';
-	return buf;
-}
-
-
-
-InputGeom::InputGeom() :
-	m_mesh(0),
-	m_hasBuildSettings(false),
-	m_offMeshConCount(0),
-	m_volumeCount(0)
-{
-}
-
-InputGeom::~InputGeom()
-{
-	delete m_mesh;
-}
-
-bool InputGeom::loadMesh(rcContext* ctx, const std::string& filepath)
-{
-	if (m_mesh)
-	{
-		delete m_mesh;
-		m_mesh = 0;
-	}
-	m_offMeshConCount = 0;
-	m_volumeCount = 0;
-
-	m_mesh = new rcMeshLoaderObj;
-	if (!m_mesh)
-	{
-		ctx->log(RC_LOG_ERROR, "loadMesh: Out of memory 'm_mesh'.");
-		return false;
-	}
-	if (!m_mesh->load(filepath))
-	{
-		ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath.c_str());
-		return false;
-	}
-
-	rcCalcBounds(m_mesh->getVerts(), m_mesh->getVertCount(), m_meshBMin, m_meshBMax);
-
-	return true;
-}
-
-bool InputGeom::loadMesh(rcContext* ctx, NodePath node)
-{
-	if (m_mesh)
-	{
-		delete m_mesh;
-		m_mesh = 0;
-	}
-	m_offMeshConCount = 0;
-	m_volumeCount = 0;
-
-	m_mesh = new rcMeshLoaderObj;
-	if (!m_mesh)
-	{
-		ctx->log(RC_LOG_ERROR, "loadMesh: Out of memory 'm_mesh'.");
-		return false;
-	}
-	if (!m_mesh->load(node))
-	{
-		ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load 'NodePath'");
-		return false;
-	}
-
-	rcCalcBounds(m_mesh->getVerts(), m_mesh->getVertCount(), m_meshBMin, m_meshBMax);
-
-	return true;
-}
-
-bool InputGeom::loadGeomSet(rcContext* ctx, const std::string& filepath)
-{
-	char* buf = 0;
-	FILE* fp = fopen(filepath.c_str(), "rb");
-	if (!fp)
-	{
-		return false;
-	}
-	if (fseek(fp, 0, SEEK_END) != 0)
-	{
-		fclose(fp);
-		return false;
-	}
-
-	long bufSize = ftell(fp);
-	if (bufSize < 0)
-	{
-		fclose(fp);
-		return false;
-	}
-	if (fseek(fp, 0, SEEK_SET) != 0)
-	{
-		fclose(fp);
-		return false;
-	}
-	buf = new char[bufSize];
-	if (!buf)
-	{
-		fclose(fp);
-		return false;
-	}
-	size_t readLen = fread(buf, bufSize, 1, fp);
-	fclose(fp);
-	if (readLen != 1)
-	{
-		delete[] buf;
-		return false;
-	}
-
-	m_offMeshConCount = 0;
-	m_volumeCount = 0;
-	delete m_mesh;
-	m_mesh = 0;
-
-	char* src = buf;
-	char* srcEnd = buf + bufSize;
-	char row[512];
-	while (src < srcEnd)
-	{
-		// Parse one row
-		row[0] = '\0';
-		src = parse_Row(src, srcEnd, row, sizeof(row) / sizeof(char));
-		if (row[0] == 'f')
-		{
-			// File name.
-			const char* name = row + 1;
-			// Skip white spaces
-			while (*name && isspace(*name))
-				name++;
-			if (*name)
-			{
-				if (!loadMesh(ctx, name))
-				{
-					delete[] buf;
-					return false;
-				}
-			}
-		}
-		else if (row[0] == 'c')
-		{
-			// Off-mesh connection
-			if (m_offMeshConCount < MAX_OFFMESH_CONNECTIONS)
-			{
-				float* v = &m_offMeshConVerts[m_offMeshConCount * 3 * 2];
-				int bidir, area = 0, flags = 0;
-				float rad;
-				sscanf(row + 1, "%f %f %f  %f %f %f %f %d %d %d",
-					&v[0], &v[1], &v[2], &v[3], &v[4], &v[5], &rad, &bidir, &area, &flags);
-				m_offMeshConRads[m_offMeshConCount] = rad;
-				m_offMeshConDirs[m_offMeshConCount] = (unsigned char)bidir;
-				m_offMeshConAreas[m_offMeshConCount] = (unsigned char)area;
-				m_offMeshConFlags[m_offMeshConCount] = (unsigned short)flags;
-				m_offMeshConCount++;
-			}
-		}
-		else if (row[0] == 'v')
-		{
-			// Convex volumes
-			if (m_volumeCount < MAX_VOLUMES)
-			{
-				ConvexVolume* vol = &m_volumes[m_volumeCount++];
-				sscanf(row + 1, "%d %d %f %f", &vol->nverts, &vol->area, &vol->hmin, &vol->hmax);
-				for (int i = 0; i < vol->nverts; ++i)
-				{
-					row[0] = '\0';
-					src = parse_Row(src, srcEnd, row, sizeof(row) / sizeof(char));
-					sscanf(row, "%f %f %f", &vol->verts[i * 3 + 0], &vol->verts[i * 3 + 1], &vol->verts[i * 3 + 2]);
-				}
-			}
-		}
-		else if (row[0] == 's')
-		{
-			// Settings
-			m_hasBuildSettings = true;
-			sscanf(row + 1, "%f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f",
-				&m_buildSettings.cellSize,
-				&m_buildSettings.cellHeight,
-				&m_buildSettings.agentHeight,
-				&m_buildSettings.agentRadius,
-				&m_buildSettings.agentMaxClimb,
-				&m_buildSettings.agentMaxSlope,
-				&m_buildSettings.regionMinSize,
-				&m_buildSettings.regionMergeSize,
-				&m_buildSettings.edgeMaxLen,
-				&m_buildSettings.edgeMaxError,
-				&m_buildSettings.vertsPerPoly,
-				&m_buildSettings.detailSampleDist,
-				&m_buildSettings.detailSampleMaxError,
-				&m_buildSettings.partitionType,
-				&m_buildSettings.navMeshBMin[0],
-				&m_buildSettings.navMeshBMin[1],
-				&m_buildSettings.navMeshBMin[2],
-				&m_buildSettings.navMeshBMax[0],
-				&m_buildSettings.navMeshBMax[1],
-				&m_buildSettings.navMeshBMax[2],
-				&m_buildSettings.tileSize);
-		}
-	}
-
-	delete[] buf;
-
-	return true;
-}
-
-bool InputGeom::load(rcContext* ctx, NodePath node) 
-{
-	return loadMesh(ctx, node);
-}
-
-bool InputGeom::load(rcContext* ctx, const std::string& filepath)
-{
-	size_t extensionPos = filepath.find_last_of('.');
-	if (extensionPos == std::string::npos)
-		return false;
-
-	std::string extension = filepath.substr(extensionPos);
-	std::transform(extension.begin(), extension.end(), extension.begin(), tolower);
-
-	if (extension == ".gset")
-		return loadGeomSet(ctx, filepath);
-	if (extension == ".obj")
-		return loadMesh(ctx, filepath);
-
-	return false;
-}
-
-bool InputGeom::saveGeomSet(const BuildSettings* settings)
-{
-	if (!m_mesh) return false;
-
-	// Change extension
-	std::string filepath = m_mesh->getFileName();
-	size_t extPos = filepath.find_last_of('.');
-	if (extPos != std::string::npos)
-		filepath = filepath.substr(0, extPos);
-
-	filepath += ".gset";
-
-	FILE* fp = fopen(filepath.c_str(), "w");
-	if (!fp) return false;
-
-	// Store mesh filename.
-	fprintf(fp, "f %s\n", m_mesh->getFileName().c_str());
-
-	// Store settings if any
-	if (settings)
-	{
-		fprintf(fp,
-			"s %f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f\n",
-			settings->cellSize,
-			settings->cellHeight,
-			settings->agentHeight,
-			settings->agentRadius,
-			settings->agentMaxClimb,
-			settings->agentMaxSlope,
-			settings->regionMinSize,
-			settings->regionMergeSize,
-			settings->edgeMaxLen,
-			settings->edgeMaxError,
-			settings->vertsPerPoly,
-			settings->detailSampleDist,
-			settings->detailSampleMaxError,
-			settings->partitionType,
-			settings->navMeshBMin[0],
-			settings->navMeshBMin[1],
-			settings->navMeshBMin[2],
-			settings->navMeshBMax[0],
-			settings->navMeshBMax[1],
-			settings->navMeshBMax[2],
-			settings->tileSize);
-	}
-
-	// Store off-mesh links.
-	for (int i = 0; i < m_offMeshConCount; ++i)
-	{
-		const float* v = &m_offMeshConVerts[i * 3 * 2];
-		const float rad = m_offMeshConRads[i];
-		const int bidir = m_offMeshConDirs[i];
-		const int area = m_offMeshConAreas[i];
-		const int flags = m_offMeshConFlags[i];
-		fprintf(fp, "c %f %f %f  %f %f %f  %f %d %d %d\n",
-			v[0], v[1], v[2], v[3], v[4], v[5], rad, bidir, area, flags);
-	}
-
-	// Convex volumes
-	for (int i = 0; i < m_volumeCount; ++i)
-	{
-		ConvexVolume* vol = &m_volumes[i];
-		fprintf(fp, "v %d %d %f %f\n", vol->nverts, vol->area, vol->hmin, vol->hmax);
-		for (int j = 0; j < vol->nverts; ++j)
-			fprintf(fp, "%f %f %f\n", vol->verts[j * 3 + 0], vol->verts[j * 3 + 1], vol->verts[j * 3 + 2]);
-	}
-
-	fclose(fp);
-
-	return true;
-}
-
-static bool isectSegAABB(const float* sp, const float* sq,
-	const float* amin, const float* amax,
-	float& tmin, float& tmax)
-{
-	static const float EPS = 1e-6f;
-
-	float d[3];
-	d[0] = sq[0] - sp[0];
-	d[1] = sq[1] - sp[1];
-	d[2] = sq[2] - sp[2];
-	tmin = 0.0;
-	tmax = 1.0f;
-
-	for (int i = 0; i < 3; i++)
-	{
-		if (fabsf(d[i]) < EPS)
-		{
-			if (sp[i] < amin[i] || sp[i] > amax[i])
-				return false;
-		}
-		else
-		{
-			const float ood = 1.0f / d[i];
-			float t1 = (amin[i] - sp[i]) * ood;
-			float t2 = (amax[i] - sp[i]) * ood;
-			if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; }
-			if (t1 > tmin) tmin = t1;
-			if (t2 < tmax) tmax = t2;
-			if (tmin > tmax) return false;
-		}
-	}
-
-	return true;
-}
-
-
-bool InputGeom::raycastMesh(float* src, float* dst, float& tmin)
-{
-	float dir[3];
-	rcVsub(dir, dst, src);
-
-	// Prune hit ray.
-	float btmin, btmax;
-	if (!isectSegAABB(src, dst, m_meshBMin, m_meshBMax, btmin, btmax))
-		return false;
-	float p[2], q[2];
-	p[0] = src[0] + (dst[0] - src[0])*btmin;
-	p[1] = src[2] + (dst[2] - src[2])*btmin;
-	q[0] = src[0] + (dst[0] - src[0])*btmax;
-	q[1] = src[2] + (dst[2] - src[2])*btmax;
-
-	int cid[512];
-	
-	tmin = 1.0f;
-	bool hit = false;
-	const float* verts = m_mesh->getVerts();
-
-	return hit;
-}
-
-void InputGeom::addOffMeshConnection(const float* spos, const float* epos, const float rad,
-	unsigned char bidir, unsigned char area, unsigned short flags)
-{
-	if (m_offMeshConCount >= MAX_OFFMESH_CONNECTIONS) return;
-	float* v = &m_offMeshConVerts[m_offMeshConCount * 3 * 2];
-	m_offMeshConRads[m_offMeshConCount] = rad;
-	m_offMeshConDirs[m_offMeshConCount] = bidir;
-	m_offMeshConAreas[m_offMeshConCount] = area;
-	m_offMeshConFlags[m_offMeshConCount] = flags;
-	m_offMeshConId[m_offMeshConCount] = 1000 + m_offMeshConCount;
-	rcVcopy(&v[0], spos);
-	rcVcopy(&v[3], epos);
-	m_offMeshConCount++;
-}
-
-void InputGeom::deleteOffMeshConnection(int i)
-{
-	m_offMeshConCount--;
-	float* src = &m_offMeshConVerts[m_offMeshConCount * 3 * 2];
-	float* dst = &m_offMeshConVerts[i * 3 * 2];
-	rcVcopy(&dst[0], &src[0]);
-	rcVcopy(&dst[3], &src[3]);
-	m_offMeshConRads[i] = m_offMeshConRads[m_offMeshConCount];
-	m_offMeshConDirs[i] = m_offMeshConDirs[m_offMeshConCount];
-	m_offMeshConAreas[i] = m_offMeshConAreas[m_offMeshConCount];
-	m_offMeshConFlags[i] = m_offMeshConFlags[m_offMeshConCount];
-}
-
-void InputGeom::drawOffMeshConnections(duDebugDraw* dd, bool hilight)
-{
-	unsigned int conColor = duRGBA(192, 0, 128, 192);
-	unsigned int baseColor = duRGBA(0, 0, 0, 64);
-	dd->depthMask(false);
-
-	dd->begin(DU_DRAW_LINES, 2.0f);
-	for (int i = 0; i < m_offMeshConCount; ++i)
-	{
-		float* v = &m_offMeshConVerts[i * 3 * 2];
-
-		dd->vertex(v[0], v[1], v[2], baseColor);
-		dd->vertex(v[0], v[1] + 0.2f, v[2], baseColor);
-
-		dd->vertex(v[3], v[4], v[5], baseColor);
-		dd->vertex(v[3], v[4] + 0.2f, v[5], baseColor);
-
-		duAppendCircle(dd, v[0], v[1] + 0.1f, v[2], m_offMeshConRads[i], baseColor);
-		duAppendCircle(dd, v[3], v[4] + 0.1f, v[5], m_offMeshConRads[i], baseColor);
-
-		if (hilight)
-		{
-			duAppendArc(dd, v[0], v[1], v[2], v[3], v[4], v[5], 0.25f,
-				(m_offMeshConDirs[i] & 1) ? 0.6f : 0.0f, 0.6f, conColor);
-		}
-	}
-	dd->end();
-
-	dd->depthMask(true);
-}
-
-void InputGeom::addConvexVolume(const float* verts, const int nverts,
-	const float minh, const float maxh, unsigned char area)
-{
-	if (m_volumeCount >= MAX_VOLUMES) return;
-	ConvexVolume* vol = &m_volumes[m_volumeCount++];
-	memset(vol, 0, sizeof(ConvexVolume));
-	memcpy(vol->verts, verts, sizeof(float) * 3 * nverts);
-	vol->hmin = minh;
-	vol->hmax = maxh;
-	vol->nverts = nverts;
-	vol->area = area;
-}
-
-void InputGeom::deleteConvexVolume(int i)
-{
-	m_volumeCount--;
-	m_volumes[i] = m_volumes[m_volumeCount];
-}
-
-void InputGeom::drawConvexVolumes(struct duDebugDraw* dd, bool /*hilight*/)
-{
-	dd->depthMask(false);
-
-	dd->begin(DU_DRAW_TRIS);
-
-	for (int i = 0; i < m_volumeCount; ++i)
-	{
-		const ConvexVolume* vol = &m_volumes[i];
-		unsigned int col = duTransCol(dd->areaToCol(vol->area), 32);
-		for (int j = 0, k = vol->nverts - 1; j < vol->nverts; k = j++)
-		{
-			const float* va = &vol->verts[k * 3];
-			const float* vb = &vol->verts[j * 3];
-
-			dd->vertex(vol->verts[0], vol->hmax, vol->verts[2], col);
-			dd->vertex(vb[0], vol->hmax, vb[2], col);
-			dd->vertex(va[0], vol->hmax, va[2], col);
-
-			dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
-			dd->vertex(va[0], vol->hmax, va[2], col);
-			dd->vertex(vb[0], vol->hmax, vb[2], col);
-
-			dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
-			dd->vertex(vb[0], vol->hmax, vb[2], col);
-			dd->vertex(vb[0], vol->hmin, vb[2], duDarkenCol(col));
-		}
-	}
-
-	dd->end();
-
-	dd->begin(DU_DRAW_LINES, 2.0f);
-	for (int i = 0; i < m_volumeCount; ++i)
-	{
-		const ConvexVolume* vol = &m_volumes[i];
-		unsigned int col = duTransCol(dd->areaToCol(vol->area), 220);
-		for (int j = 0, k = vol->nverts - 1; j < vol->nverts; k = j++)
-		{
-			const float* va = &vol->verts[k * 3];
-			const float* vb = &vol->verts[j * 3];
-			dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
-			dd->vertex(vb[0], vol->hmin, vb[2], duDarkenCol(col));
-			dd->vertex(va[0], vol->hmax, va[2], col);
-			dd->vertex(vb[0], vol->hmax, vb[2], col);
-			dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
-			dd->vertex(va[0], vol->hmax, va[2], col);
-		}
-	}
-	dd->end();
-
-	dd->begin(DU_DRAW_POINTS, 3.0f);
-	for (int i = 0; i < m_volumeCount; ++i)
-	{
-		const ConvexVolume* vol = &m_volumes[i];
-		unsigned int col = duDarkenCol(duTransCol(dd->areaToCol(vol->area), 220));
-		for (int j = 0; j < vol->nverts; ++j)
-		{
-			dd->vertex(vol->verts[j * 3 + 0], vol->verts[j * 3 + 1] + 0.1f, vol->verts[j * 3 + 2], col);
-			dd->vertex(vol->verts[j * 3 + 0], vol->hmin, vol->verts[j * 3 + 2], col);
-			dd->vertex(vol->verts[j * 3 + 0], vol->hmax, vol->verts[j * 3 + 2], col);
-		}
-	}
-	dd->end();
-
-
-	dd->depthMask(true);
-}

panda/src/navigation/MeshLoaderObj.cxx

@@ -1,385 +0,0 @@
-
-#include "MeshLoaderObj.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <cstring>
-#define _USE_MATH_DEFINES
-#include <math.h>
-#include "geom.h"
-#include "nodePath.h"
-#include "geomVertexReader.h"
-#include "geomNode.h"
-#include "pmap.h"
-#include "pvector.h"
-
-
-std::map<LVector3,int> mp;
-int indexTemp = 0;
-
-rcMeshLoaderObj::rcMeshLoaderObj() :
-	m_scale(1.0f),
-	m_verts(0),
-	m_tris(0),
-	m_normals(0),
-	m_vertCount(0),
-	m_triCount(0),
-	temp_vcount(0)
-{
-}
-
-rcMeshLoaderObj::~rcMeshLoaderObj()
-{
-	delete[] m_verts;
-	delete[] m_normals;
-	delete[] m_tris;
-}
-
-void rcMeshLoaderObj::addVertex(float x, float y, float z, int& cap)
-{
-	if (m_vertCount + 1 > cap)
-	{
-		cap = !cap ? 8 : cap * 2;
-		float* nv = new float[cap * 3];
-		if (m_vertCount)
-			memcpy(nv, m_verts, m_vertCount * 3 * sizeof(float));
-		delete[] m_verts;
-		m_verts = nv;
-	}
-	float* dst = &m_verts[m_vertCount * 3];
-	*dst++ = x * m_scale;
-	*dst++ = y * m_scale;
-	*dst++ = z * m_scale;
-	m_vertCount++;
-}
-
-void rcMeshLoaderObj::addTriangle(int a, int b, int c, int& cap)
-{
-	if (m_triCount + 1 > cap)
-	{
-		cap = !cap ? 8 : cap * 2;
-		int* nv = new int[cap * 3];
-		if (m_triCount)
-			memcpy(nv, m_tris, m_triCount * 3 * sizeof(int));
-		delete[] m_tris;
-		m_tris = nv;
-	}
-	int* dst = &m_tris[m_triCount * 3];
-	*dst++ = a;
-	*dst++ = b;
-	*dst++ = c;
-	m_triCount++;
-}
-
-
-static char* parseRow(char* buf, char* bufEnd, char* row, int len)
-{
-	bool start = true;
-	bool done = false;
-	int n = 0;
-	while (!done && buf < bufEnd)
-	{
-		char c = *buf;
-		buf++;
-		// multirow
-		switch (c)
-		{
-		case '\\':
-			break;
-		case '\n':
-			if (start) break;
-			done = true;
-			break;
-		case '\r':
-			break;
-		case '\t':
-		case ' ':
-			if (start) break;
-			// else falls through
-		default:
-			start = false;
-			row[n++] = c;
-			if (n >= len - 1)
-				done = true;
-			break;
-		}
-	}
-	row[n] = '\0';
-	return buf;
-}
-
-static int parseFace(char* row, int* data, int n, int vcnt)
-{
-	int j = 0;
-	while (*row != '\0')
-	{
-		// Skip initial white space
-		while (*row != '\0' && (*row == ' ' || *row == '\t'))
-			row++;
-		char* s = row;
-		// Find vertex delimiter and terminated the string there for conversion.
-		while (*row != '\0' && *row != ' ' && *row != '\t')
-		{
-			if (*row == '/') *row = '\0';
-			row++;
-		}
-		if (*s == '\0')
-			continue;
-		int vi = atoi(s);
-		data[j++] = vi < 0 ? vi + vcnt : vi - 1;
-		if (j >= n) return j;
-	}
-	return j;
-}
-
-bool rcMeshLoaderObj::load(const std::string& filename)
-{
-	char* buf = 0;
-	FILE* fp = fopen(filename.c_str(), "rb");
-	if (!fp)
-		return false;
-	if (fseek(fp, 0, SEEK_END) != 0)
-	{
-		fclose(fp);
-		return false;
-	}
-	long bufSize = ftell(fp);
-	if (bufSize < 0)
-	{
-		fclose(fp);
-		return false;
-	}
-	if (fseek(fp, 0, SEEK_SET) != 0)
-	{
-		fclose(fp);
-		return false;
-	}
-	buf = new char[bufSize];
-	if (!buf)
-	{
-		fclose(fp);
-		return false;
-	}
-	size_t readLen = fread(buf, bufSize, 1, fp);
-	fclose(fp);
-
-	if (readLen != 1)
-	{
-		delete[] buf;
-		return false;
-	}
-
-	char* src = buf;
-	char* srcEnd = buf + bufSize;
-	char row[512];
-	int face[32];
-	float x, y, z;
-	int nv;
-	int vcap = 0;
-	int tcap = 0;
-
-	while (src < srcEnd)
-	{
-		// Parse one row
-		row[0] = '\0';
-		src = parseRow(src, srcEnd, row, sizeof(row) / sizeof(char));
-		// Skip comments
-		if (row[0] == '#') continue;
-		if (row[0] == 'v' && row[1] != 'n' && row[1] != 't')
-		{
-			// Vertex pos
-			sscanf(row + 1, "%f %f %f", &x, &y, &z);
-			addVertex(x, z, -y, vcap);		//exchanged y and z to match with panda3d's coordinate system
-		}
-		if (row[0] == 'f')
-		{
-			// Faces
-			nv = parseFace(row + 1, face, 32, m_vertCount);
-			for (int i = 2; i < nv; ++i)
-			{
-				const int a = face[0];
-				const int b = face[i - 1];
-				const int c = face[i];
-				if (a < 0 || a >= m_vertCount || b < 0 || b >= m_vertCount || c < 0 || c >= m_vertCount)
-					continue;
-				addTriangle(a, b, c, tcap);
-			}
-		}
-	}
-
-	delete[] buf;
-
-	// Calculate normals.
-	m_normals = new float[m_triCount * 3];
-	for (int i = 0; i < m_triCount * 3; i += 3)
-	{
-		const float* v0 = &m_verts[m_tris[i] * 3];
-		const float* v1 = &m_verts[m_tris[i + 1] * 3];
-		const float* v2 = &m_verts[m_tris[i + 2] * 3];
-		float e0[3], e1[3];
-		for (int j = 0; j < 3; ++j)
-		{
-			e0[j] = v1[j] - v0[j];
-			e1[j] = v2[j] - v0[j];
-		}
-		float* n = &m_normals[i];
-		n[0] = e0[1] * e1[2] - e0[2] * e1[1];
-		n[1] = e0[2] * e1[0] - e0[0] * e1[2];
-		n[2] = e0[0] * e1[1] - e0[1] * e1[0];
-		float d = sqrtf(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]);
-		if (d > 0)
-		{
-			d = 1.0f / d;
-			n[0] *= d;
-			n[1] *= d;
-			n[2] *= d;
-		}
-	}
-
-	m_filename = filename;
-	return true;
-}
-std::vector<LVector3> vc,fc;
-bool rcMeshLoaderObj::load(NodePath node)
-{
-	NodePathCollection geomNodeCollection = node.find_all_matches("**/+GeomNode");
-	//std::cout << "Number of geomNode_paths: " << geomNodeCollection.get_num_paths() << std::endl;
-	int vcap = 0;
-	int tcap = 0;
-
-
-	for (size_t i = 0; i < geomNodeCollection.get_num_paths(); ++i)
-	{
-		temp_vcount = m_vertCount;
-		//std::cout << "Vertices count: " << m_vertCount << std::endl;
-		PT(GeomNode) g = DCAST(GeomNode, geomNodeCollection.get_path(i).node());
-		//std::cout << "GeomPath i = " << i << std::endl;
-		processGeomNode(g, vcap, tcap);
-	}
-	for (int i = 0;i < vc.size();i++) {
-		//std::cout << "v " << vc[i][0] << " " << vc[i][1] << " " << vc[i][2] << "\n";
-	}
-	for (int i = 0;i < fc.size();i++) {
-		//std::cout << "f " << fc[i][0] << " " << fc[i][1] << " " << fc[i][2] << "\n";
-	}
-	return true;
-}
-
-void rcMeshLoaderObj::processPrimitive(const GeomPrimitive *orig_prim, const GeomVertexData *vdata, int& tcap) {
-
-	GeomVertexReader vertex(vdata, "vertex");
-
-	CPT(GeomPrimitive) prim = orig_prim->decompose();
-	//std::cout << "prim->get_num_primitives(): " << prim->get_num_primitives() << std::endl;
-	for (size_t k = 0; k < prim->get_num_primitives(); ++k) {
-		int s = prim->get_primitive_start(k);
-		int e = prim->get_primitive_end(k);
-		//std::cout << "(s, e): " << s << "\t" << e << "\n";
-		LVector3 v;
-		if (e - s == 3)
-		{
-			int a = prim->get_vertex(s);
-			vertex.set_row(a);
-			v = vertex.get_data3();
-			a = mp[v];
-
-			int b = prim->get_vertex(s + 1);
-			vertex.set_row(b);
-			v = vertex.get_data3();
-			b = mp[v];
-			
-			int c = prim->get_vertex(s + 2);
-			vertex.set_row(c);
-			v = vertex.get_data3();
-			c = mp[v];
-
-			//if (a < 0 || a >= m_vertCount || b < 0 || b >= m_vertCount || c < 0 || c >= m_vertCount)
-				//continue;
-			//std::cout <<"f: " << a << "\t" << b << "\t" << c << "\ntcap: "<<tcap<<"\n";
-			LVector3 xvx = { float(a + 1), float(b + 1), float(c + 1) };
-			fc.push_back(xvx);
-			addTriangle(a, b, c, tcap);
-		}
-		else if(e - s > 3 ) {
-			//std::cout << "More than 3 vertices\n";
-			for (int i = s+2; i < e; ++i)
-			{
-				int a = prim->get_vertex(s);
-				vertex.set_row(a);
-				v = vertex.get_data3();
-				a = mp[v];
-
-				int b = prim->get_vertex(i-1);
-				vertex.set_row(b);
-				v = vertex.get_data3();
-				b = mp[v];
-
-				int c = prim->get_vertex(i);
-				vertex.set_row(c);
-				v = vertex.get_data3();
-				c = mp[v];
-
-				//if (a < 0 || a >= m_vertCount || b < 0 || b >= m_vertCount || c < 0 || c >= m_vertCount)
-					//continue;
-				//std::cout << "f: " << a << "\t" << b << "\t" << c << "\ntcap: " << tcap << "\n";
-				LVector3 xvx = { float(a + 1), float(b + 1), float(c + 1) };
-				fc.push_back(xvx);
-				addTriangle(a, b, c, tcap);
-			}
-		}
-		else continue;
-		
-	}
-	return;
-}
-
-void rcMeshLoaderObj::processVertexData(const GeomVertexData *vdata, int& vcap) {
-	GeomVertexReader vertex(vdata, "vertex");
-	float x, y, z;
-	
-	while (!vertex.is_at_end()) {
-		//std::cout << vcap++ << std::endl;
-		LVector3 v = vertex.get_data3();
-		x = v[0];
-		y = v[1];
-		z = v[2];
-		if (mp.find(v) == mp.end())
-		{
-			addVertex(x, z, -y, vcap);
-			mp[v] = indexTemp++;
-			LVector3 xvx = { v[0],v[2],-v[1] };
-			vc.push_back(xvx);
-		}
-		
-		//nout << "vcount: " << m_vertCount << "\tvcap: "<< vcap <<"\n";
-		//nout << "V = " << v << std::endl;
-	}
-	return;
-}
-void rcMeshLoaderObj::processGeom(CPT(Geom) geom, int& vcap, int& tcap) {
-	
-	CPT(GeomVertexData) vdata = geom->get_vertex_data();
-	
-	processVertexData(vdata, vcap);
-	//std::cout << "geom->get_num_primitives(): " << geom->get_num_primitives() << std::endl;
-	for (size_t i = 0; i < geom->get_num_primitives(); ++i) {
-		
-		CPT(GeomPrimitive) prim = geom->get_primitive(i);
-		processPrimitive(prim, vdata, tcap);
-	}
-	return;
-}
-void rcMeshLoaderObj::processGeomNode(GeomNode *geomnode, int& vcap, int& tcap) {
-
-	//std::cout << "Number of geoms: " << geomnode->get_num_geoms() << std::endl;
-	if (geomnode->get_num_geoms() > 1)
-	{
-		std::cout << "Cannot proceed: Make sure number of geoms = 1\n";
-		return;
-	}
-	for (size_t j = 0; j < geomnode->get_num_geoms(); ++j) {
-		
-		CPT(Geom) geom = geomnode->get_geom(j);
-		processGeom(geom, vcap, tcap);
-	}
-	return;
-}

panda/src/navigation/NavMeshSample.cxx

@@ -1,482 +0,0 @@
-#include "NavMeshSample.h"
-
-#ifndef CPPPARSER
-#include "Recast.h"
-#include "DetourNavMesh.h"
-#include "DetourNavMeshQuery.h"
-#include "DetourCrowd.h"
-#endif
-
-#include "InputGeom.h"
-#define _USE_MATH_DEFINES
-#include <math.h>
-#include <stdio.h>
-#include <string>
-#include <iostream>
-#include "geom.h"
-#include "geomVertexFormat.h"
-#include "geomVertexWriter.h"
-#include "geomTrifans.h"
-
-
-#ifdef WIN32
-#	define snprintf _snprintf
-#endif
-
-NavMeshSample::NavMeshSample():
-	m_geom(0),
-	m_navMesh(0),
-	m_navQuery(0),
-	m_crowd(0),
-	m_filterLowHangingObstacles(true),
-	m_filterLedgeSpans(true),
-	m_filterWalkableLowHeightSpans(true),
-	m_ctx(0),
-	m_triareas(0),
-	m_solid(0),
-	m_chf(0),
-	m_cset(0),
-	m_pmesh(0),
-	m_dmesh(0)
-{
-	m_ctx = new rcContext;
-	resetCommonSettings();
-	m_navQuery = dtAllocNavMeshQuery();
-	m_crowd = dtAllocCrowd();
-}
-
-NavMeshSample::~NavMeshSample()
-{
-	dtFreeNavMeshQuery(m_navQuery);
-	dtFreeNavMesh(m_navMesh);
-	dtFreeCrowd(m_crowd);
-
-	cleanup();
-
-}
-
-void NavMeshSample::cleanup()
-{
-	delete[] m_triareas;
-	m_triareas = 0;
-	rcFreeHeightField(m_solid);
-	m_solid = 0;
-	rcFreeCompactHeightfield(m_chf);
-	m_chf = 0;
-	rcFreeContourSet(m_cset);
-	m_cset = 0;
-	rcFreePolyMesh(m_pmesh);
-	m_pmesh = 0;
-	rcFreePolyMeshDetail(m_dmesh);
-	m_dmesh = 0;
-	dtFreeNavMesh(m_navMesh);
-	m_navMesh = 0;
-}
-
-void NavMeshSample::collectSettings(BuildSettings& settings)
-{
-	settings.cellSize = m_cellSize;
-	settings.cellHeight = m_cellHeight;
-	settings.agentHeight = m_agentHeight;
-	settings.agentRadius = m_agentRadius;
-	settings.agentMaxClimb = m_agentMaxClimb;
-	settings.agentMaxSlope = m_agentMaxSlope;
-	settings.regionMinSize = m_regionMinSize;
-	settings.regionMergeSize = m_regionMergeSize;
-	settings.edgeMaxLen = m_edgeMaxLen;
-	settings.edgeMaxError = m_edgeMaxError;
-	settings.vertsPerPoly = m_vertsPerPoly;
-	settings.detailSampleDist = m_detailSampleDist;
-	settings.detailSampleMaxError = m_detailSampleMaxError;
-	settings.partitionType = m_partitionType;
-}
-
-void NavMeshSample::resetCommonSettings()
-{
-	m_cellSize = 0.3f;
-	m_cellHeight = 0.2f;
-	m_agentHeight = 2.0f;
-	m_agentRadius = 0.6f;
-	m_agentMaxClimb = 0.9f;
-	m_agentMaxSlope = 45.0f;
-	m_regionMinSize = 8;
-	m_regionMergeSize = 20;
-	m_edgeMaxLen = 12.0f;
-	m_edgeMaxError = 1.3f;
-	m_vertsPerPoly = 6.0f;
-	m_detailSampleDist = 6.0f;
-	m_detailSampleMaxError = 1.0f;
-	m_partitionType = SAMPLE_PARTITION_WATERSHED;
-}
-
-static const int NAVMESHSET_MAGIC = 'M' << 24 | 'S' << 16 | 'E' << 8 | 'T'; //'MSET';
-static const int NAVMESHSET_VERSION = 1;
-
-
-bool NavMeshSample::handleBuild() {
-	if (!m_geom || !m_geom->getMesh())
-	{
-		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Input mesh is not specified.");
-		std::cout << "\nbuildNavigation: Input mesh is not specified.\n";
-		return false;
-	}
-
-	cleanup();
-
-	const float* bmin = m_geom->getNavMeshBoundsMin();
-	const float* bmax = m_geom->getNavMeshBoundsMax();
-	const float* verts = m_geom->getMesh()->getVerts();
-	const int nverts = m_geom->getMesh()->getVertCount();
-	const int* tris = m_geom->getMesh()->getTris();
-	const int ntris = m_geom->getMesh()->getTriCount();
-	std::cout << "BMIN: " << bmin[0] << " " << bmin[1] << std::endl;
-	std::cout << "BMAX: " << bmax[0] << " " << bmax[1] << std::endl;
-	//
-	// Step 1. Initialize build config.
-	//
-	// 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;
-
-	// 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:");
-	std::cout << "\nBuilding navigation:\n";
-	m_ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", m_cfg.width, m_cfg.height);
-	std::cout << "\n - " << m_cfg.width << " x " << m_cfg.height << " cells\n";
-	m_ctx->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", nverts / 1000.0f, ntris / 1000.0f);
-	std::cout << "\n - " << nverts / 1000.0f << "K vetrs, " << ntris / 1000.0f << "K tris\n";
-
-	//
-	// 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'.");
-		std::cout << "\nbuildNavigation: Out of memory 'solid'.\n";
-		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.");
-		std::cout << "\nbuildNavigation: Could not create solid heightfield.\n";
-		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);
-		std::cout << "\nbuildNavigation: Out of memory 'm_triareas' (" << ntris << ").\n";
-		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.");
-		std::cout << "\nbuildNavigation: Could not rasterize triangles.\n";
-		return false;
-	}
-
-
-	//
-	// 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'.");
-		std::cout << "\nbuildNavigation: Out of memory 'chf'.\n";
-		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.");
-		std::cout << "\nbuildNavigation: Could not build compact data.\n";
-		return false;
-	}
-
-
-	// 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.");
-		std::cout << "\nbuildNavigation: Could not erode.\n";
-		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.");
-			std::cout << "\nbuildNavigation: Could not build distance field.\n";
-			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.");
-			std::cout << "\nbuildNavigation: Could not build watershed regions.\n";
-			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.");
-			std::cout << "\nbuildNavigation: Could not build monotone regions.\n";
-			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.");
-			std::cout << "\nbuildNavigation: Could not build layer regions.\n";
-			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'.");
-		std::cout << "\nbuildNavigation: Out of memory 'cset'.\n";
-		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.");
-		std::cout << "\nbuildNavigation: Could not create contours.\n";
-		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'.");
-		std::cout << "\nbuildNavigation: Out of memory 'pmesh'.\n";
-		return false;
-	}
-	if (!rcBuildPolyMesh(m_ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh))
-	{
-		m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours.");
-		std::cout << "\nbuildNavigation: Could not triangulate contours.\n";
-		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'.");
-		std::cout << "\nbuildNavigation: Out of memory 'pmdt1'.\n";
-		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.");
-		std::cout << "\nbuildNavigation: Could not build detail mesh.\n";
-		return false;
-	}
-	std::cout << "Number of vertices: " << m_pmesh->nverts << std::endl;
-	std::cout << "Number of polygons: " << m_pmesh->npolys << std::endl;
-	std::cout << "Number of allocated polygons: " << m_pmesh->maxpolys << std::endl;
-	return true;
-	// At this point the navigation mesh data is ready, you can access it from m_pmesh.
-	// See duDebugDrawPolyMesh or dtCreateNavMeshData as examples how to access the data.
-
-}
-
-
-
-PT(GeomNode) NavMeshSample::getPolyMeshGeom() {
-
-	PT(GeomVertexData) vdata;
-	vdata = new GeomVertexData("vertexInfo", GeomVertexFormat::get_v3(), Geom::UH_static);
-	vdata->set_num_rows(m_pmesh->nverts);
-
-	GeomVertexWriter vertex(vdata, "vertex");
-
-	const int nvp = m_pmesh->nvp;
-	std::cout << "nvp: " << nvp << std::endl;
-	const float cs = m_pmesh->cs;
-	std::cout << "cs: " << cs << std::endl;
-	const float ch = m_pmesh->ch;
-	std::cout << "ch: " << ch << std::endl;
-	const float* orig = m_pmesh->bmin;
-	std::cout << "orig: " << orig[0] << "\t" << orig[1] << "\t" << orig[2] << std::endl;
-
-	std::cout << "m_pmesh->npolys: " << m_pmesh->npolys << std::endl;
-	std::cout << "m_pmesh->nverts: " << m_pmesh->nverts << std::endl;
-
-	for (int i = 0;i < m_pmesh->nverts*3;i += 3) {
-
-		const unsigned short* v = &m_pmesh->verts[i];
-
-		//convert to world space
-		const float x = orig[0] + v[0] * cs;
-		const float y = orig[1] + v[1] * ch;
-		const float z = orig[2] + v[2] * cs;
-		
-		vertex.add_data3(x, -z, y);
-		std::cout << "index: " << i / 3 << "\t" << x << "\t" << y << "\t" << z << "\n";
-
-	}
-
-	PT(GeomNode) node;
-	node = new GeomNode("gnode");
-
-	PT(GeomTrifans) prim;
-	prim = new GeomTrifans(Geom::UH_static);
-
-	for (int i = 0; i < m_pmesh->npolys; ++i)
-	{
-		
-		
-		const unsigned short* p = &m_pmesh->polys[i*nvp * 2];
-
-		// Iterate the vertices.
-		//unsigned short vi[3];  // The vertex indices.
-		for (int j = 0; j < nvp; ++j)
-		{
-			if (p[j] == RC_MESH_NULL_IDX)
-			{
-				break;// End of vertices.
-			}
-			if (p[j + nvp] == RC_MESH_NULL_IDX)
-			{
-				prim->add_vertex(p[j]);
-				// The edge beginning with this vertex is a solid border.
-			}
-			else
-			{
-				prim->add_vertex(p[j]);
-				// The edge beginning with this vertex connects to 
-				// polygon p[j + nvp].
-			}
-			std::cout << "p[j]: " << p[j] << std::endl;
-			
-		}
-		prim->close_primitive();
-		
-	}
-	PT(Geom) polymeshgeom;
-	polymeshgeom = new Geom(vdata);
-	polymeshgeom->add_primitive(prim);
-
-	node->add_geom(polymeshgeom);
-	std::cout << "Number of Polygons: " << m_pmesh->npolys << std::endl;
-
-	return node;
-}
-
-
-

panda/src/navigation/config_navigation.cxx

@@ -4,9 +4,10 @@
 
 #include "pandaSystem.h"
 #include "dconfig.h"
-#include "InputGeom.h"
-#include "MeshLoaderObj.h"
-#include "NavMeshSample.h"
+#include "navMesh.h"
+#include "navMeshBuilder.h"
+#include "navMeshNode.h"
+#include "navMeshQuery.h"
 
 #if !defined(CPPPARSER) && !defined(LINK_ALL_STATIC) && !defined(BUILDING_RECASTDETOUR)
   #error Buildsystem error: BUILDING_NAVIGATION not defined

panda/src/navigation/navMesh.cxx

@@ -0,0 +1,97 @@
+#include "navMesh.h"
+#include "geom.h"
+#include "geomTrifans.h"
+
+NavMesh::NavMesh() {
+
+}
+
+NavMesh::~NavMesh() {
+  
+}
+
+NavMesh::NavMesh(dtNavMesh *nav_mesh, rcPolyMesh *pmesh, rcPolyMeshDetail *dmesh) {
+  _nav_mesh = nav_mesh;
+  _pmesh = pmesh;
+  _dmesh = dmesh;
+}
+
+PT(GeomNode) NavMesh::draw_poly_mesh_geom() {
+
+  PT(GeomVertexData) vdata;
+  vdata = new GeomVertexData("vertexInfo", GeomVertexFormat::get_v3c4(), Geom::UH_static);
+  vdata->set_num_rows(_pmesh->nverts);
+
+  GeomVertexWriter vertex(vdata, "vertex");
+  GeomVertexWriter colour(vdata, "color");
+
+  const int nvp = _pmesh->nvp;
+  std::cout << "nvp: " << nvp << std::endl;
+  const float cs = _pmesh->cs;
+  std::cout << "cs: " << cs << std::endl;
+  const float ch = _pmesh->ch;
+  std::cout << "ch: " << ch << std::endl;
+  const float* orig = _pmesh->bmin;
+  std::cout << "orig: " << orig[0] << "\t" << orig[1] << "\t" << orig[2] << std::endl;
+
+  std::cout << "_pmesh->npolys: " << _pmesh->npolys << std::endl;
+  std::cout << "_pmesh->nverts: " << _pmesh->nverts << std::endl;
+
+  for (int i = 0;i < _pmesh->nverts * 3;i += 3) {
+
+    const unsigned short* v = &_pmesh->verts[i];
+
+    //convert to world space
+    const float x = orig[0] + v[0] * cs;
+    const float y = orig[1] + v[1] * ch;
+    const float z = orig[2] + v[2] * cs;
+
+    //vertex.add_data3(x, -z, y); //if origingally model is z-up
+    vertex.add_data3(x, y, z); //if originally model is y-up
+    colour.add_data4((float)rand() / RAND_MAX, (float)rand() / RAND_MAX, (float)rand() / RAND_MAX, 1);
+    std::cout << "index: " << i / 3 << "\t" << x << "\t" << y << "\t" << z << "\n";
+
+  }
+
+  PT(GeomNode) node;
+  node = new GeomNode("gnode");
+
+  PT(GeomTrifans) prim;
+  prim = new GeomTrifans(Geom::UH_static);
+
+  for (int i = 0; i < _pmesh->npolys; ++i) {
+
+
+    const unsigned short* p = &_pmesh->polys[i*nvp * 2];
+
+    // Iterate the vertices.
+    //unsigned short vi[3];  // The vertex indices.
+    for (int j = 0; j < nvp; ++j) {
+      if (p[j] == RC_MESH_NULL_IDX) {
+        break;// End of vertices.
+      }
+      if (p[j + nvp] == RC_MESH_NULL_IDX) {
+        prim->add_vertex(p[j]);
+        // The edge beginning with this vertex is a solid border.
+      }
+      else {
+        prim->add_vertex(p[j]);
+        // The edge beginning with this vertex connects to 
+        // polygon p[j + nvp].
+      }
+      std::cout << "p[j]: " << p[j] << std::endl;
+
+    }
+    prim->close_primitive();
+
+  }
+  PT(Geom) polymeshgeom;
+  polymeshgeom = new Geom(vdata);
+  polymeshgeom->add_primitive(prim);
+
+  node->add_geom(polymeshgeom);
+  std::cout << "Number of Polygons: " << _pmesh->npolys << std::endl;
+
+
+  return node;
+}

panda/src/navigation/navMesh.h

@@ -0,0 +1,26 @@
+#ifndef NAVMESH_H
+#define NAVMESH_H
+
+#include "Recast.h"
+#include "DetourNavMesh.h"
+#include "pandaFramework.h"
+
+class NavMesh
+{
+private:
+  dtNavMesh *_nav_mesh;
+  rcPolyMesh *_pmesh;
+  rcPolyMeshDetail *_dmesh;
+  
+public:
+  NavMesh();
+  NavMesh(dtNavMesh *nav_mesh, rcPolyMesh *pmesh, rcPolyMeshDetail *dmesh);
+  ~NavMesh();
+  PT(GeomNode) draw_poly_mesh_geom();
+  void set_nav_mesh(dtNavMesh *m) { _nav_mesh = m; }
+  void set_poly_mesh(rcPolyMesh *p) { _pmesh = p; }
+  void set_detail_poly_mesh(rcPolyMeshDetail *d) { _dmesh = d; }
+  //has draw functions
+};
+
+#endif // NAVMESH_H

panda/src/navigation/navMeshBuilder.cxx

@@ -0,0 +1,661 @@
+#include "navMeshBuilder.h"
+#include "Recast.h"
+#include "DetourNavMesh.h"
+#include "DetourNavMeshQuery.h"
+#include "DetourNavMeshBuilder.h"
+#include "DetourCrowd.h"
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <stdio.h>
+#include <string>
+#include <iostream>
+#include "geom.h"
+#include "geomVertexFormat.h"
+#include "geomVertexWriter.h"
+#include "geomTrifans.h"
+
+
+#ifdef WIN32
+# define snprintf _snprintf
+#endif
+
+NavMeshBuilder::NavMeshBuilder() :
+  _nav_mesh(0),
+  _nav_query(0),
+  _crowd(0),
+  _filter_low_hanging_obstacles(true),
+  _filter_ledge_spans(true),
+  _filter_walkable_low_height_spans(true),
+  _ctx(0),
+  _triareas(0),
+  _solid(0),
+  _chf(0),
+  _cset(0),
+  _pmesh(0),
+  _dmesh(0),
+  _scale(1.0f),
+  _verts(0),
+  _tris(0),
+  _normals(0),
+  _vert_count(0),
+  _tri_count(0) {
+  mp.clear();
+  vc.clear();
+  fc.clear();
+  _ctx = new rcContext;
+  reset_common_settings();
+  _nav_query = dtAllocNavMeshQuery();
+  _crowd = dtAllocCrowd();
+}
+
+NavMeshBuilder::~NavMeshBuilder() {
+  delete[] _verts;
+  delete[] _normals;
+  delete[] _tris;
+
+  dtFreeNavMeshQuery(_nav_query);
+  dtFreeCrowd(_crowd);
+
+  cleanup();
+
+}
+
+
+void NavMeshBuilder::add_vertex(float x, float y, float z, int &cap) {
+  if (_vert_count + 1 > cap) {
+    cap = !cap ? 8 : cap * 2;
+    float *nv = new float[cap * 3];
+    if (_vert_count) {
+      memcpy(nv, _verts, _vert_count * 3 * sizeof(float));
+    }
+    delete[] _verts;
+    _verts = nv;
+  }
+  float *dst = &_verts[_vert_count * 3];
+  *dst++ = x * _scale;
+  *dst++ = y * _scale;
+  *dst++ = z * _scale;
+  _vert_count++;
+}
+
+void NavMeshBuilder::add_triangle(int a, int b, int c, int &cap) {
+  if (_tri_count + 1 > cap) {
+    cap = !cap ? 8 : cap * 2;
+    int *nv = new int[cap * 3];
+    if (_tri_count) {
+      memcpy(nv, _tris, _tri_count * 3 * sizeof(int));
+    }
+    delete[] _tris;
+    _tris = nv;
+  }
+  int *dst = &_tris[_tri_count * 3];
+  *dst++ = a;
+  *dst++ = b;
+  *dst++ = c;
+  _tri_count++;
+}
+
+bool NavMeshBuilder::from_node_path(NodePath node) {
+  NodePathCollection geom_node_collection = node.find_all_matches("**/+GeomNode");
+
+  int vcap = 0;
+  int tcap = 0;
+
+  for (size_t i = 0; i < geom_node_collection.get_num_paths(); ++i) {
+
+    PT(GeomNode) g = DCAST(GeomNode, geom_node_collection.get_path(i).node());
+    process_geom_node(g, vcap, tcap);
+
+  }
+  loaded = true;
+  return true;
+}
+
+void NavMeshBuilder::process_primitive(const GeomPrimitive *orig_prim, const GeomVertexData *vdata, int &tcap) {
+
+  GeomVertexReader vertex(vdata, "vertex");
+
+  CPT(GeomPrimitive) prim = orig_prim->decompose();
+
+  for (size_t k = 0; k < prim->get_num_primitives(); ++k) {
+
+    int s = prim->get_primitive_start(k);
+    int e = prim->get_primitive_end(k);
+
+    LVector3 v;
+    if (e - s == 3) {
+      int a = prim->get_vertex(s);
+      vertex.set_row(a);
+      v = vertex.get_data3();
+      a = mp[v];
+      int b = prim->get_vertex(s + 1);
+      vertex.set_row(b);
+      v = vertex.get_data3();
+      b = mp[v];
+
+      int c = prim->get_vertex(s + 2);
+      vertex.set_row(c);
+      v = vertex.get_data3();
+      c = mp[v];
+
+
+      LVector3 xvx = { float(a + 1), float(b + 1), float(c + 1) };
+      fc.push_back(xvx);
+      add_triangle(a, b, c, tcap);
+    }
+    else if (e - s > 3) {
+
+      for (int i = s + 2; i < e; ++i) {
+        int a = prim->get_vertex(s);
+        vertex.set_row(a);
+        v = vertex.get_data3();
+        a = mp[v];
+
+        int b = prim->get_vertex(i - 1);
+        vertex.set_row(b);
+        v = vertex.get_data3();
+        b = mp[v];
+
+        int c = prim->get_vertex(i);
+        vertex.set_row(c);
+        v = vertex.get_data3();
+        c = mp[v];
+
+        LVector3 xvx = { float(a + 1), float(b + 1), float(c + 1) };
+        fc.push_back(xvx);
+        add_triangle(a, b, c, tcap);
+      }
+    }
+    else continue;
+
+  }
+  return;
+}
+
+void NavMeshBuilder::process_vertex_data(const GeomVertexData *vdata, int &vcap) {
+  GeomVertexReader vertex(vdata, "vertex");
+  float x, y, z;
+
+  while (!vertex.is_at_end()) {
+
+    LVector3 v = vertex.get_data3();
+    x = v[0];
+    y = v[1];
+    z = v[2];
+    if (mp.find(v) == mp.end()) {
+      //add_vertex(x, z, -y, vcap); //if input model is originally z-up
+      add_vertex(x, y, z, vcap); //if input model is originally y-up
+      mp[v] = index_temp++;
+      LVector3 xvx = { v[0],v[2],-v[1] };
+      vc.push_back(xvx);
+    }
+
+  }
+  return;
+}
+
+void NavMeshBuilder::process_geom(CPT(Geom) geom, int& vcap, int& tcap) {
+
+  CPT(GeomVertexData) vdata = geom->get_vertex_data();
+
+  process_vertex_data(vdata, vcap);
+
+  for (size_t i = 0; i < geom->get_num_primitives(); ++i) {
+    CPT(GeomPrimitive) prim = geom->get_primitive(i);
+    process_primitive(prim, vdata, tcap);
+  }
+  return;
+}
+
+void NavMeshBuilder::process_geom_node(GeomNode *geomnode, int &vcap, int &tcap) {
+
+
+  if (geomnode->get_num_geoms() > 1) {
+    std::cout << "Cannot proceed: Make sure number of geoms = 1\n";
+    return;
+  }
+  for (size_t j = 0; j < geomnode->get_num_geoms(); ++j) {
+    CPT(Geom) geom = geomnode->get_geom(j);
+    process_geom(geom, vcap, tcap);
+  }
+  return;
+}
+
+
+
+
+
+void NavMeshBuilder::cleanup()
+{
+  delete[] _triareas;
+  _triareas = 0;
+  rcFreeHeightField(_solid);
+  _solid = 0;
+  rcFreeCompactHeightfield(_chf);
+  _chf = 0;
+  rcFreeContourSet(_cset);
+  _cset = 0;
+  rcFreePolyMesh(_pmesh);
+  _pmesh = 0;
+  rcFreePolyMeshDetail(_dmesh);
+  _dmesh = 0;
+  dtFreeNavMesh(_nav_mesh);
+  _nav_mesh = 0;
+}
+
+void NavMeshBuilder::set_partition_type(std::string p) {
+  if (p == "watershed" || p == "Watershed" || p == "WATERSHED") {
+    _partition_type = SAMPLE_PARTITION_WATERSHED;
+    return;
+  }
+  if (p == "monotone" || p == "Monotone" || p == "MONOTONE") {
+    _partition_type = SAMPLE_PARTITION_MONOTONE;
+    return;
+  }
+  if (p == "layer" || p == "Layer" || p == "LAYER") {
+    _partition_type = SAMPLE_PARTITION_LAYERS;
+    return;
+  }
+}
+
+void NavMeshBuilder::collect_settings(BuildSettings& settings)
+{
+  settings.cell_size = _cell_size;
+  settings.cell_height = _cell_height;
+  settings.agent_height = _agent_height;
+  settings.agent_radius = _agent_radius;
+  settings.agent_max_climb = _agent_max_climb;
+  settings.agent_max_slope = _agent_max_slope;
+  settings.region_min_size = _region_min_size;
+  settings.region_merge_size = _region_merge_size;
+  settings.edge_max_len = _edge_max_len;
+  settings.edge_max_error = _edge_max_error;
+  settings.verts_per_poly = _verts_per_poly;
+  settings.detail_sample_dist = _detail_sample_dist;
+  settings.detail_sample_max_error = _detail_sample_max_error;
+  settings.partition_type = _partition_type;
+}
+
+void NavMeshBuilder::reset_common_settings()
+{
+  _cell_size = 0.3f;
+  _cell_height = 0.2f;
+  _agent_height = 2.0f;
+  _agent_radius = 0.6f;
+  _agent_max_climb = 0.9f;
+  _agent_max_slope = 45.0f;
+  _region_min_size = 8;
+  _region_merge_size = 20;
+  _edge_max_len = 12.0f;
+  _edge_max_error = 1.3f;
+  _verts_per_poly = 6.0f;
+  _detail_sample_dist = 6.0f;
+  _detail_sample_max_error = 1.0f;
+  _partition_type = SAMPLE_PARTITION_WATERSHED;
+}
+
+static const int NAVMESHSET_MAGIC = 'M' << 24 | 'S' << 16 | 'E' << 8 | 'T'; //'MSET';
+static const int NAVMESHSET_VERSION = 1;
+
+bool NavMeshBuilder::build() {
+  if (!loaded_geom()) {
+
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Input mesh is not specified.");
+    std::cout << "\nbuildNavigation: Input mesh is not specified.\n";
+
+    return false;
+  }
+
+  cleanup();
+
+  rcCalcBounds(get_verts(), get_vert_count(), _mesh_bMin, _mesh_bMax);
+
+  const float *bmin = _mesh_bMin;
+  const float *bmax = _mesh_bMax;
+  const float *verts = get_verts();
+  const int nverts = get_vert_count();
+  const int *tris = get_tris();
+  const int ntris = get_tri_count();
+  std::cout << "BMIN: " << bmin[0] << " " << bmin[1] << std::endl;
+  std::cout << "BMAX: " << bmax[0] << " " << bmax[1] << std::endl;
+  //
+  // Step 1. Initialize build config.
+  //
+  // Init build configuration from GUI
+  memset(&_cfg, 0, sizeof(_cfg));
+  _cfg.cs = _cell_size;
+  _cfg.ch = _cell_height;
+  _cfg.walkableSlopeAngle = _agent_max_slope;
+  _cfg.walkableHeight = (int)ceilf(_agent_height / _cfg.ch);
+  _cfg.walkableClimb = (int)floorf(_agent_max_climb / _cfg.ch);
+  _cfg.walkableRadius = (int)ceilf(_agent_radius / _cfg.cs);
+  _cfg.maxEdgeLen = (int)(_edge_max_len / _cell_size);
+  _cfg.maxSimplificationError = _edge_max_error;
+  _cfg.minRegionArea = (int)rcSqr(_region_min_size);    // Note: area = size*size
+  _cfg.mergeRegionArea = (int)rcSqr(_region_merge_size);  // Note: area = size*size
+  _cfg.maxVertsPerPoly = (int)_verts_per_poly;
+  _cfg.detailSampleDist = _detail_sample_dist < 0.9f ? 0 : _cell_size * _detail_sample_dist;
+  _cfg.detailSampleMaxError = _cell_height * _detail_sample_max_error;
+
+  // 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(_cfg.bmin, bmin);
+  rcVcopy(_cfg.bmax, bmax);
+  rcCalcGridSize(_cfg.bmin, _cfg.bmax, _cfg.cs, &_cfg.width, &_cfg.height);
+
+  // Reset build times gathering.
+  _ctx->resetTimers();
+
+  // Start the build process. 
+  _ctx->startTimer(RC_TIMER_TOTAL);
+
+  _ctx->log(RC_LOG_PROGRESS, "Building navigation:");
+  std::cout << "\nBuilding navigation:\n";
+  _ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", _cfg.width, _cfg.height);
+  std::cout << "\n - " << _cfg.width << " x " << _cfg.height << " cells\n";
+  _ctx->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", nverts / 1000.0f, ntris / 1000.0f);
+  std::cout << "\n - " << nverts / 1000.0f << "K vetrs, " << ntris / 1000.0f << "K tris\n";
+
+  //
+  // Step 2. Rasterize input polygon soup.
+  //
+
+  // Allocate voxel heightfield where we rasterize our input data to.
+  _solid = rcAllocHeightfield();
+  if (!_solid) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'.");
+    std::cout << "\nbuildNavigation: Out of memory 'solid'.\n";
+    return false;
+  }
+  if (!rcCreateHeightfield(_ctx, *_solid, _cfg.width, _cfg.height, _cfg.bmin, _cfg.bmax, _cfg.cs, _cfg.ch)) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield.");
+    std::cout << "\nbuildNavigation: Could not create solid heightfield.\n";
+    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.
+  _triareas = new unsigned char[ntris];
+  if (!_triareas) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory '_triareas' (%d).", ntris);
+    std::cout << "\nbuildNavigation: Out of memory '_triareas' (" << ntris << ").\n";
+    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(_triareas, 0, ntris * sizeof(unsigned char));
+  rcMarkWalkableTriangles(_ctx, _cfg.walkableSlopeAngle, verts, nverts, tris, ntris, _triareas);
+  if (!rcRasterizeTriangles(_ctx, verts, nverts, tris, _triareas, ntris, *_solid, _cfg.walkableClimb)) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not rasterize triangles.");
+    std::cout << "\nbuildNavigation: Could not rasterize triangles.\n";
+    return false;
+  }
+
+
+  //
+  // 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 (_filter_low_hanging_obstacles)
+    rcFilterLowHangingWalkableObstacles(_ctx, _cfg.walkableClimb, *_solid);
+  if (_filter_ledge_spans)
+    rcFilterLedgeSpans(_ctx, _cfg.walkableHeight, _cfg.walkableClimb, *_solid);
+  if (_filter_walkable_low_height_spans)
+    rcFilterWalkableLowHeightSpans(_ctx, _cfg.walkableHeight, *_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.
+  _chf = rcAllocCompactHeightfield();
+  if (!_chf) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'.");
+    std::cout << "\nbuildNavigation: Out of memory 'chf'.\n";
+    return false;
+  }
+  if (!rcBuildCompactHeightfield(_ctx, _cfg.walkableHeight, _cfg.walkableClimb, *_solid, *_chf)) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data.");
+    std::cout << "\nbuildNavigation: Could not build compact data.\n";
+    return false;
+  }
+
+
+  // Erode the walkable area by agent radius.
+  if (!rcErodeWalkableArea(_ctx, _cfg.walkableRadius, *_chf)) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode.");
+    std::cout << "\nbuildNavigation: Could not erode.\n";
+    return false;
+  }
+
+
+  // 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 (_partition_type == SAMPLE_PARTITION_WATERSHED) {
+    // Prepare for region partitioning, by calculating distance field along the walkable surface.
+    if (!rcBuildDistanceField(_ctx, *_chf)) {
+      _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field.");
+      std::cout << "\nbuildNavigation: Could not build distance field.\n";
+      return false;
+    }
+
+    // Partition the walkable surface into simple regions without holes.
+    if (!rcBuildRegions(_ctx, *_chf, 0, _cfg.minRegionArea, _cfg.mergeRegionArea)) {
+      _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build watershed regions.");
+      std::cout << "\nbuildNavigation: Could not build watershed regions.\n";
+      return false;
+    }
+  }
+  else if (_partition_type == SAMPLE_PARTITION_MONOTONE) {
+    // Partition the walkable surface into simple regions without holes.
+    // Monotone partitioning does not need distancefield.
+    if (!rcBuildRegionsMonotone(_ctx, *_chf, 0, _cfg.minRegionArea, _cfg.mergeRegionArea)) {
+      _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build monotone regions.");
+      std::cout << "\nbuildNavigation: Could not build monotone regions.\n";
+      return false;
+    }
+  }
+  else { // SAMPLE_PARTITION_LAYERS
+    // Partition the walkable surface into simple regions without holes.
+    if (!rcBuildLayerRegions(_ctx, *_chf, 0, _cfg.minRegionArea)) {
+      _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build layer regions.");
+      std::cout << "\nbuildNavigation: Could not build layer regions.\n";
+      return false;
+    }
+  }
+
+  //
+  // Step 5. Trace and simplify region contours.
+  //
+
+  // Create contours.
+  _cset = rcAllocContourSet();
+  if (!_cset) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'.");
+    std::cout << "\nbuildNavigation: Out of memory 'cset'.\n";
+    return false;
+  }
+  if (!rcBuildContours(_ctx, *_chf, _cfg.maxSimplificationError, _cfg.maxEdgeLen, *_cset)) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create contours.");
+    std::cout << "\nbuildNavigation: Could not create contours.\n";
+    return false;
+  }
+
+  //
+  // Step 6. Build polygons mesh from contours.
+  //
+
+  // Build polygon navmesh from the contours.
+  _pmesh = rcAllocPolyMesh();
+  if (!_pmesh) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'.");
+    std::cout << "\nbuildNavigation: Out of memory 'pmesh'.\n";
+    return false;
+  }
+  if (!rcBuildPolyMesh(_ctx, *_cset, _cfg.maxVertsPerPoly, *_pmesh)) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours.");
+    std::cout << "\nbuildNavigation: Could not triangulate contours.\n";
+    return false;
+  }
+
+  //
+  // Step 7. Create detail mesh which allows to access approximate height on each polygon.
+  //
+
+  _dmesh = rcAllocPolyMeshDetail();
+  if (!_dmesh) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmdtl'.");
+    std::cout << "\nbuildNavigation: Out of memory 'pmdt1'.\n";
+    return false;
+  }
+
+  if (!rcBuildPolyMeshDetail(_ctx, *_pmesh, *_chf, _cfg.detailSampleDist, _cfg.detailSampleMaxError, *_dmesh)) {
+    _ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build detail mesh.");
+    std::cout << "\nbuildNavigation: Could not build detail mesh.\n";
+    return false;
+  }
+  std::cout << "Number of vertices: " << _pmesh->nverts << std::endl;
+  std::cout << "Number of polygons: " << _pmesh->npolys << std::endl;
+  std::cout << "Number of allocated polygons: " << _pmesh->maxpolys << std::endl;
+
+  // At this point the navigation mesh data is ready, you can access it from _pmesh.
+  // See duDebugDrawPolyMesh or dtCreateNavMeshData as examples how to access the data.
+
+
+  //
+  // (Optional) Step 8. Create Detour data from Recast poly mesh.
+  //
+  std::cout << "\nSample_SoloMesh::handleBuild() : (Optional) Step 8. Create Detour data from Recast poly mesh.\n";
+  // The GUI may allow more max points per polygon than Detour can handle.
+  // Only build the detour navmesh if we do not exceed the limit.
+  if (_cfg.maxVertsPerPoly <= DT_VERTS_PER_POLYGON) {
+    unsigned char *nav_data = 0;
+    int nav_data_size = 0;
+
+    // Update poly flags from areas.
+    for (int i = 0; i < _pmesh->npolys; ++i) {
+      if (_pmesh->areas[i] == RC_WALKABLE_AREA)
+        _pmesh->areas[i] = SAMPLE_POLYAREA_GROUND;
+
+      if (_pmesh->areas[i] == SAMPLE_POLYAREA_GROUND || _pmesh->areas[i] == SAMPLE_POLYAREA_GRASS || _pmesh->areas[i] == SAMPLE_POLYAREA_ROAD) {
+        _pmesh->flags[i] = SAMPLE_POLYFLAGS_WALK;
+      }
+      else if (_pmesh->areas[i] == SAMPLE_POLYAREA_WATER) {
+        _pmesh->flags[i] = SAMPLE_POLYFLAGS_SWIM;
+      }
+      else if (_pmesh->areas[i] == SAMPLE_POLYAREA_DOOR) {
+        _pmesh->flags[i] = SAMPLE_POLYFLAGS_WALK | SAMPLE_POLYFLAGS_DOOR;
+      }
+    }
+
+
+    dtNavMeshCreateParams params;
+    memset(&params, 0, sizeof(params));
+    params.verts = _pmesh->verts;
+    params.vertCount = _pmesh->nverts;
+    params.polys = _pmesh->polys;
+    params.polyAreas = _pmesh->areas;
+    params.polyFlags = _pmesh->flags;
+    params.polyCount = _pmesh->npolys;
+    params.nvp = _pmesh->nvp;
+    params.detailMeshes = _dmesh->meshes;
+    params.detailVerts = _dmesh->verts;
+    params.detailVertsCount = _dmesh->nverts;
+    params.detailTris = _dmesh->tris;
+    params.detailTriCount = _dmesh->ntris;
+
+
+    params.walkableHeight = _agent_height;
+    params.walkableRadius = _agent_radius;
+    params.walkableClimb = _agent_max_climb;
+    rcVcopy(params.bmin, _pmesh->bmin);
+    rcVcopy(params.bmax, _pmesh->bmax);
+    params.cs = _cfg.cs;
+    params.ch = _cfg.ch;
+    params.buildBvTree = true;
+
+    if (!dtCreateNavMeshData(&params, &nav_data, &nav_data_size)) {
+      _ctx->log(RC_LOG_ERROR, "Could not build Detour navmesh.");
+      return false;
+    }
+
+    _nav_mesh = dtAllocNavMesh();
+
+    if (!_nav_mesh) {
+      dtFree(nav_data);
+      _ctx->log(RC_LOG_ERROR, "Could not create Detour navmesh");
+      return false;
+    }
+
+    dtStatus status;
+
+    status = _nav_mesh->init(nav_data, nav_data_size, DT_TILE_FREE_DATA);
+    if (dtStatusFailed(status)) {
+      dtFree(nav_data);
+      _ctx->log(RC_LOG_ERROR, "Could not init Detour navmesh");
+      return false;
+    }
+
+    status = _nav_query->init(_nav_mesh, 2048);
+    if (dtStatusFailed(status)) {
+      _ctx->log(RC_LOG_ERROR, "Could not init Detour navmesh query");
+      return false;
+    }
+
+  }
+
+
+  _ctx->stopTimer(RC_TIMER_TOTAL);
+
+  // Show performance stats.
+  _ctx->log(RC_LOG_PROGRESS, ">> Polymesh: %d vertices  %d polygons", _pmesh->nverts, _pmesh->npolys);
+
+  _total_build_time_ms = _ctx->getAccumulatedTime(RC_TIMER_TOTAL) / 1000.0f;
+
+  //if (_tool)
+    //_tool->init(this);
+  //initToolStates(this);
+  //std::cout << "\nExiting Sample_SoloMesh::handleBuild()\n";
+  return true;
+}
+
+
+NavMesh NavMeshBuilder::get_navmesh() {
+  NavMesh nav(_nav_mesh, _pmesh, _dmesh);
+  return nav;
+}

panda/src/navigation/navMeshBuilder.h

@@ -0,0 +1,164 @@
+#ifndef NAVMESHBUILDER_H
+#define NAVMESHBUILDER_H
+
+#include "Recast.h"
+#include <string>
+#include "geom.h"
+#include "geomNode.h"
+#include "nodePath.h"
+#include "pandaFramework.h"
+#include "pandaSystem.h"
+#include "navMesh.h"
+
+enum SamplePolyAreas {
+  SAMPLE_POLYAREA_GROUND,
+  SAMPLE_POLYAREA_WATER,
+  SAMPLE_POLYAREA_ROAD,
+  SAMPLE_POLYAREA_DOOR,
+  SAMPLE_POLYAREA_GRASS,
+  SAMPLE_POLYAREA_JUMP,
+};
+enum SamplePolyFlags {
+  SAMPLE_POLYFLAGS_WALK = 0x01,		// Ability to walk (ground, grass, road)
+  SAMPLE_POLYFLAGS_SWIM = 0x02,		// Ability to swim (water).
+  SAMPLE_POLYFLAGS_DOOR = 0x04,		// Ability to move through doors.
+  SAMPLE_POLYFLAGS_JUMP = 0x08,		// Ability to jump.
+  SAMPLE_POLYFLAGS_DISABLED = 0x10,		// Disabled polygon
+  SAMPLE_POLYFLAGS_ALL = 0xffff	// All abilities.
+};
+struct BuildSettings {
+  
+  float cell_size;
+  float cell_height;
+  float agent_height;
+  float agent_radius;
+  float agent_max_climb;
+  float agent_max_slope;
+  float region_min_size;
+  float region_merge_size;
+  float edge_max_len;
+  float edge_max_error;
+  float verts_per_poly;
+  float detail_sample_dist;
+  float detail_sample_max_error;
+  int partition_type;
+  float nav_mesh_bMin[3];
+  float nav_mesh_bMax[3];
+  float tile_size;
+};
+
+
+class NavMeshBuilder {
+private:
+  std::string _filename;
+  float _scale;
+  float *_verts;
+  int *_tris;
+  float *_normals;
+  int _vert_count;
+  int _tri_count;
+
+  void add_vertex(float x, float y, float z, int &cap);
+  void add_triangle(int a, int b, int c, int &cap);
+
+
+  void process_geom_node(GeomNode *geomnode, int &vcap, int &tcap);
+  void process_geom(CPT(Geom) geom, int &vcap, int &tcap);
+  void process_vertex_data(const GeomVertexData *vdata, int &vcap);
+  void process_primitive(const GeomPrimitive *orig_prim, const GeomVertexData *vdata, int &tcap);
+
+  std::map<LVector3, int> mp;
+  std::vector<LVector3> vc, fc;
+  bool loaded;
+  int index_temp;
+protected:
+  class dtNavMesh *_nav_mesh;
+  class dtNavMeshQuery *_nav_query;
+  class dtCrowd *_crowd;
+
+  unsigned char _nav_mesh_draw_flags;
+
+  float _cell_size;
+  float _cell_height;
+  float _agent_height;
+  float _agent_radius;
+  float _agent_max_climb;
+  float _agent_max_slope;
+  float _region_min_size;
+  float _region_merge_size;
+  float _edge_max_len;
+  float _edge_max_error;
+  float _verts_per_poly;
+  float _detail_sample_dist;
+  float _detail_sample_max_error;
+  int _partition_type;
+
+  bool _filter_low_hanging_obstacles;
+  bool _filter_ledge_spans;
+  bool _filter_walkable_low_height_spans;
+  float _mesh_bMin[3], _mesh_bMax[3];
+
+  rcContext *_ctx;
+
+  float _total_build_time_ms;
+
+  unsigned char *_triareas;
+  rcHeightfield *_solid;
+  rcCompactHeightfield *_chf;
+  rcContourSet *_cset;
+  rcPolyMesh *_pmesh;
+  rcConfig _cfg;
+  rcPolyMeshDetail *_dmesh;
+
+  void cleanup();
+
+public:
+
+  NavMeshBuilder();
+  ~NavMeshBuilder();
+  void set_context(rcContext *ctx) { _ctx = ctx; }
+
+  virtual void collect_settings(struct BuildSettings &settings);
+
+  float get_agent_radius() { return _agent_radius; }
+  float get_agent_height() { return _agent_height; }
+  float get_agent_climb() { return _agent_max_climb; }
+  dtNavMeshQuery *get_nav_query() { return _nav_query; }
+
+  void set_actor_height(float h) { _agent_height = h; }
+  void set_actor_radius(float r) { _agent_radius = r; }
+  void set_actor_clinb(float c) { _agent_max_climb = c; }
+  void set_partition_type(std::string p);
+
+  void reset_common_settings();
+
+  bool build();
+  NavMesh get_navmesh();
+
+  unsigned char getNavMeshDrawFlags() const { return _nav_mesh_draw_flags; }
+
+  //shifted from NavMeshNode
+  bool from_node_path(NodePath node);
+  //future functions:
+  //bool from_geom(Geom g);
+  //bool from_egg(string filename);
+
+  const float *get_verts() const { return _verts; }
+  const float *get_normals() const { return _normals; }
+  const int *get_tris() const { return _tris; }
+  int get_vert_count() const { return _vert_count; }
+  int get_tri_count() const { return _tri_count; }
+  const std::string& get_file_name() const { return _filename; }
+  bool loaded_geom() { return loaded; }
+  
+
+};
+
+enum SamplePartitionType {
+  SAMPLE_PARTITION_WATERSHED,
+  SAMPLE_PARTITION_MONOTONE,
+  SAMPLE_PARTITION_LAYERS,
+};
+
+
+#endif // NAVMESHBUILDER_H

panda/src/navigation/navMeshNode.cxx

@@ -0,0 +1,14 @@
+#include "navMeshNode.h"
+
+
+NavMeshNode::NavMeshNode(std::string &name, NavMesh nav_mesh):
+  PandaNode(name)
+{
+  _nav_mesh = nav_mesh;
+}
+
+NavMeshNode::~NavMeshNode() {
+  
+}
+
+

panda/src/navigation/navMeshNode.h

@@ -0,0 +1,21 @@
+#ifndef NAVMESHNODE_H
+#define NAVMESHNODE_H
+
+#include <string>
+#include "geom.h"
+#include "pandaFramework.h"
+#include "pandaSystem.h"
+#include "navMesh.h"
+#include <string>
+
+class NavMeshNode: public PandaNode
+{
+private:
+  NavMesh _nav_mesh;
+
+public:
+  NavMeshNode(std::string &name, NavMesh nav_mesh);
+  ~NavMeshNode();
+};
+
+#endif // NAVMESHNODE_H

panda/src/navigation/navMeshQuery.cxx

@@ -0,0 +1,30 @@
+#include "navMeshQuery.h"
+#include "DetourNavMeshQuery.h"
+
+bool NavMeshQuery::nearest_point(LPoint3 &p) {
+  if (!_nav_query) {
+    std::cout << "\nNavMeshQuery not created!\n";
+    return false;
+  }
+
+  const float center[3] = { p[0], p[1], p[2] };
+  float *nearest_p = new float[3];
+  const float extents[3] = { 2 , 4 , 2 };
+
+  dtQueryFilter filter;
+  filter.setIncludeFlags(RC_WALKABLE_AREA);
+  filter.setExcludeFlags(RC_NULL_AREA);
+  dtPolyRef nearest_poly_ref_id = 0;
+
+  dtStatus status = _nav_query->findNearestPoly(center, extents, &filter, &nearest_poly_ref_id, nearest_p);
+
+  if (dtStatusFailed(status)) {
+    std::cout << "\nCannot find nearest point on polymesh.\n";
+    return false;
+  }
+  p = LPoint3(nearest_p[0], nearest_p[1], nearest_p[2]);
+
+  return true;
+}
+
+

panda/src/navigation/navMeshQuery.h

@@ -0,0 +1,18 @@
+#ifndef NAVMESHQUERY_H
+#define NAVMESHQUERY_H
+
+#include "DetourNavMeshQuery.h"
+#include "navMeshBuilder.h"
+
+class NavMeshQuery
+{
+private:
+  class dtNavMeshQuery *_nav_query;
+  
+public:
+  void set_nav_query(NavMeshBuilder *nav_mesh) { _nav_query = nav_mesh->get_nav_query(); }
+  bool nearest_point(LPoint3 &p);
+  
+};
+
+#endif // NAVMESHQUERY_H

panda/src/navigation/p3navigation_composite1.cxx

@@ -1,4 +1,4 @@
-#include "config_navigation.cxx"
-#include "InputGeom.cxx"
-#include "MeshLoaderObj.cxx"
-#include "NavMeshSample.cxx"
+#include "navMesh.cxx"
+#include "navMeshBuilder.cxx"
+#include "navMeshNode.cxx"
+#include "navMeshQuery.cxx"