/**
@par

Behavior:

- The movement is constrained to the surface of the navigation mesh. 
- The corridor is automatically adjusted (shorted or lengthened) in order to remain valid. 
- The new target will be located in the adjusted corridor's last polygon.

The expected use case is that the desired target will be 'near' the current corridor. What is considered 'near' depends on local polygon density, query search extents, etc.

The resulting target will differ from the desired target if the desired target is not on the navigation mesh, or it can't be reached using a local search.
*/
bool dtPathCorridor::moveTargetPosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
	dtAssert(m_path);
	dtAssert(m_npath);
	
	// Move along navmesh and update new position.
	float result[3];
	static const int MAX_VISITED = 16;
	dtPolyRef visited[MAX_VISITED];
	int nvisited = 0;
	dtStatus status = navquery->moveAlongSurface(m_path[m_npath-1], m_target, npos, filter,
												 result, visited, &nvisited, MAX_VISITED);
	if (dtStatusSucceed(status))
	{
		m_npath = dtMergeCorridorEndMoved(m_path, m_npath, m_maxPath, visited, nvisited);
		// TODO: should we do that?
		// Adjust the position to stay on top of the navmesh.
		/*	float h = m_target[1];
		 navquery->getPolyHeight(m_path[m_npath-1], result, &h);
		 result[1] = h;*/
		
		dtVcopy(m_target, result);
		
		return true;
	}
	return false;
}
bool dtProximityGrid::init(const int poolSize, const float cellSize)
{
    dtAssert(poolSize > 0);
    dtAssert(cellSize > 0.0f);

    m_cellSize = cellSize;
    m_invCellSize = 1.0f / m_cellSize;

    // Allocate hashs buckets
    m_bucketsSize = dtNextPow2(poolSize);
    m_buckets = (unsigned short*)dtAlloc(sizeof(unsigned short)*m_bucketsSize, DT_ALLOC_PERM);
    if (!m_buckets)
        return false;

    // Allocate pool of items.
    m_poolSize = poolSize;
    m_poolHead = 0;
    m_pool = (Item*)dtAlloc(sizeof(Item)*m_poolSize, DT_ALLOC_PERM);
    if (!m_pool)
        return false;

    clear();

    return true;
}
/**
@par

Inaccurate locomotion or dynamic obstacle avoidance can force the agent position significantly outside the 
original corridor. Over time this can result in the formation of a non-optimal corridor. This function will use a 
local area path search to try to re-optimize the corridor.

The more inaccurate the agent movement, the more beneficial this function becomes. Simply adjust the frequency of 
the call to match the needs to the agent.
*/
bool dtPathCorridor::optimizePathTopology(dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
	dtAssert(navquery);
	dtAssert(filter);
	dtAssert(m_path);
	
	if (m_npath < 3)
		return false;
	
	static const int MAX_ITER = 32;
	static const int MAX_RES = 32;
	
	dtPolyRef res[MAX_RES];
	int nres = 0;
	navquery->initSlicedFindPath(m_path[0], m_path[m_npath-1], m_pos, m_target, filter);
	navquery->updateSlicedFindPath(MAX_ITER, 0);
	dtStatus status = navquery->finalizeSlicedFindPathPartial(m_path, m_npath, res, &nres, MAX_RES);
	
	if (dtStatusSucceed(status) && nres > 0)
	{
		m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres);
		return true;
	}
	
	return false;
}
 inline unsigned char getFlags(dtPolyRef ref)
 {
     dtAssert(m_nav);
     dtAssert(m_ntiles);
     // Assume the ref is valid, no bounds checks.
     unsigned int salt, it, ip;
     m_nav->decodePolyId(ref, salt, it, ip);
     return m_tiles[it].flags[ip];
 }
 inline void setFlags(dtPolyRef ref, unsigned char flags)
 {
     dtAssert(m_nav);
     dtAssert(m_ntiles);
     // Assume the ref is valid, no bounds checks.
     unsigned int salt, it, ip;
     m_nav->decodePolyId(ref, salt, it, ip);
     m_tiles[it].flags[ip] = flags;
 }
/// @par
///
/// The current corridor position is expected to be within the first polygon in the path. The target 
/// is expected to be in the last polygon. 
/// 
/// @warning The size of the path must not exceed the size of corridor's path buffer set during #init().
void dtPathCorridor::setCorridor(const float* target, const dtPolyRef* path, const int npath)
{
	dtAssert(m_path);
	dtAssert(npath > 0);
	dtAssert(npath < m_maxPath);
	
	dtVcopy(m_target, target);
	memcpy(m_path, path, sizeof(dtPolyRef)*npath);
	m_npath = npath;
}
Beispiel #7
0
dtNodeQueue::dtNodeQueue(int n) :
	m_heap(0),
	m_capacity(n),
	m_size(0)
{
	dtAssert(m_capacity > 0);
	
	m_heap = (dtNode**)dtAlloc(sizeof(dtNode*)*(m_capacity+1), DT_ALLOC_PERM);
	dtAssert(m_heap);
}
dtStatus dtBuildTileCacheDistanceField(dtTileCacheAlloc* alloc, dtTileCacheLayer& layer, dtTileCacheDistanceField& dfield)
{
    dtAssert(alloc);

    const int w = (int)layer.header->width;
    const int h = (int)layer.header->height;

    dfield.data = (unsigned short*)alloc->alloc(w*h*sizeof(unsigned short));
    if (!dfield.data)
    {
        return DT_FAILURE | DT_OUT_OF_MEMORY;
    }

    dtTileCacheDistanceField tmpField;
    tmpField.data = (unsigned short*)alloc->alloc(w*h*sizeof(unsigned short));
    if (!tmpField.data)
    {
        return DT_FAILURE | DT_OUT_OF_MEMORY;
    }

    calculateDistanceField(layer, dfield.data, dfield.maxDist);
    if (boxBlur(layer, 1, dfield.data, tmpField.data) != dfield.data)
    {
        dtSwap(dfield.data, tmpField.data);
    }

    alloc->free(tmpField.data);
    return DT_SUCCESS;
}
static int addToPathQueue(dtCrowdAgent* newag, dtCrowdAgent** agents, const int nagents, const int maxAgents)
{
	// Insert neighbour based on greatest time.
	int slot = 0;
	if (!nagents)
	{
		slot = nagents;
	}
	else if (newag->targetReplanTime <= agents[nagents-1]->targetReplanTime)
	{
		if (nagents >= maxAgents)
			return nagents;
		slot = nagents;
	}
	else
	{
		int i;
		for (i = 0; i < nagents; ++i)
			if (newag->targetReplanTime >= agents[i]->targetReplanTime)
				break;
		
		const int tgt = i+1;
		const int n = dtMin(nagents-i, maxAgents-tgt);
		
		dtAssert(tgt+n <= maxAgents);
		
		if (n > 0)
			memmove(&agents[tgt], &agents[i], sizeof(dtCrowdAgent*)*n);
		slot = i;
	}
	
	agents[slot] = newag;
	
	return dtMin(nagents+1, maxAgents);
}
Beispiel #10
0
const dtOffMeshConnection* dtNavMesh::getOffMeshConnectionByRef(dtPolyRef ref) const
{
    unsigned int salt, it, ip;

    if (!ref)
        return 0;

    // Get current polygon
    decodePolyId(ref, salt, it, ip);
    if (it >= (unsigned int)m_maxTiles)
        return 0;

    if (m_tiles[it].salt != salt || m_tiles[it].header == 0)
        return 0;

    const dtMeshTile* tile = &m_tiles[it];
    if (ip >= (unsigned int)tile->header->polyCount)
        return 0;
	if (ip >= (unsigned int)tile->header->polyCount) return 0;
	const dtPoly* poly = &tile->polys[ip];
	
	// Make sure that the current poly is indeed off-mesh link.
	if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION)
		return 0;

	const unsigned int idx =  ip - tile->header->offMeshBase;
	dtAssert(idx < (unsigned int)tile->header->offMeshConCount);
	return &tile->offMeshCons[idx];
}
bool dtObstacleAvoidanceDebugData::init(const int maxSamples)
{
	dtAssert(maxSamples);
	m_maxSamples = maxSamples;

	m_vel = (float*)dtAlloc(sizeof(float)*3*m_maxSamples, DT_ALLOC_PERM);
	if (!m_vel)
		return false;
	m_pen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
	if (!m_pen)
		return false;
	m_ssize = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
	if (!m_ssize)
		return false;
	m_vpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
	if (!m_vpen)
		return false;
	m_vcpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
	if (!m_vcpen)
		return false;
	m_spen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
	if (!m_spen)
		return false;
	m_tpen = (float*)dtAlloc(sizeof(float)*m_maxSamples, DT_ALLOC_PERM);
	if (!m_tpen)
		return false;
	
	return true;
}
Beispiel #12
0
/** 
@par

Inaccurate locomotion or dynamic obstacle avoidance can force the argent position significantly outside the 
original corridor. Over time this can result in the formation of a non-optimal corridor. Non-optimal paths can 
also form near the corners of tiles.

This function uses an efficient local visibility search to try to optimize the corridor 
between the current position and @p next.

The corridor will change only if @p next is visible from the current position and moving directly toward the point 
is better than following the existing path.

The more inaccurate the agent movement, the more beneficial this function becomes. Simply adjust the frequency 
of the call to match the needs to the agent.

This function is not suitable for long distance searches.
*/
void dtPathCorridor::optimizePathVisibility(const float* next, const float pathOptimizationRange,
										  dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
	dtAssert(m_path);
	
	// Clamp the ray to max distance.
	float goal[3];
	dtVcopy(goal, next);
	float dist = dtVdist2D(m_pos, goal);
	
	// If too close to the goal, do not try to optimize.
	if (dist < 0.01f)
		return;
	
	// Overshoot a little. This helps to optimize open fields in tiled meshes.
	dist = dtMin(dist+0.01f, pathOptimizationRange);
	
	// Adjust ray length.
	float delta[3];
	dtVsub(delta, goal, m_pos);
	dtVmad(goal, m_pos, delta, pathOptimizationRange/dist);
	
	static const int MAX_RES = 32;
	dtPolyRef res[MAX_RES];
	float t, norm[3];
	int nres = 0;
	navquery->raycast(m_path[0], m_pos, goal, filter, &t, norm, res, &nres, MAX_RES);
	if (nres > 1 && t > 0.99f)
	{
		m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres);
	}
}
Beispiel #13
0
/// @par
///
/// Essentially, the corridor is set of one polygon in size with the target
/// equal to the position.
void dtPathCorridor::reset(dtPolyRef ref, const float* pos)
{
	dtAssert(m_path);
	dtVcopy(m_pos, pos);
	dtVcopy(m_target, pos);
	m_path[0] = ref;
	m_npath = 1;
}
Beispiel #14
0
/// @par
///
/// @warning Cannot be called more than once.
bool dtPathCorridor::init(const int maxPath)
{
	dtAssert(!m_path);
	m_path = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*maxPath, DT_ALLOC_PERM);
	if (!m_path)
		return false;
	m_npath = 0;
	m_maxPath = maxPath;
	return true;
}
Beispiel #15
0
bool dtPathCorridor::moveOverOffmeshConnection(dtPolyRef offMeshConRef, dtPolyRef* refs,
											   float* startPos, float* endPos,
											   dtNavMeshQuery* navquery)
{
	dtAssert(navquery);
	dtAssert(m_path);
	dtAssert(m_npath);

	// Advance the path up to and over the off-mesh connection.
	dtPolyRef prevRef = 0, polyRef = m_path[0];
	int npos = 0;
	while (npos < m_npath && polyRef != offMeshConRef)
	{
		prevRef = polyRef;
		polyRef = m_path[npos];
		npos++;
	}
	if (npos == m_npath)
	{
		// Could not find offMeshConRef
		return false;
	}
	
	// Prune path
	for (int i = npos; i < m_npath; ++i)
		m_path[i-npos] = m_path[i];
	m_npath -= npos;

	refs[0] = prevRef;
	refs[1] = polyRef;
	
	const dtNavMesh* nav = navquery->getAttachedNavMesh();
	dtAssert(nav);

	dtStatus status = nav->getOffMeshConnectionPolyEndPoints(refs[0], refs[1], startPos, endPos);
	if (dtStatusSucceed(status))
	{
		dtVcopy(m_pos, endPos);
		return true;
	}

	return false;
}
Beispiel #16
0
/**
@par

Behavior:

- The movement is constrained to the surface of the navigation mesh. 
- The corridor is automatically adjusted (shorted or lengthened) in order to remain valid. 
- The new position will be located in the adjusted corridor's first polygon.

The expected use case is that the desired position will be 'near' the current corridor. What is considered 'near' 
depends on local polygon density, query search extents, etc.

The resulting position will differ from the desired position if the desired position is not on the navigation mesh, 
or it can't be reached using a local search.
*/
void dtPathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
	dtAssert(m_path);
	dtAssert(m_npath);
	
	// Move along navmesh and update new position.
	float result[3];
	static const int MAX_VISITED = 16;
	dtPolyRef visited[MAX_VISITED];
	int nvisited = 0;
	navquery->moveAlongSurface(m_path[0], m_pos, npos, filter,
							   result, visited, &nvisited, MAX_VISITED);
	m_npath = dtMergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited);
	
	// Adjust the position to stay on top of the navmesh.
	float h = m_pos[1];
	navquery->getPolyHeight(m_path[0], result, &h);
	result[1] = h;
	dtVcopy(m_pos, result);
}
/**
@par

This is the function used to plan local movement within the corridor. One or more corners can be 
detected in order to plan movement. It performs essentially the same function as #dtNavMeshQuery::findStraightPath.

Due to internal optimizations, the maximum number of corners returned will be (@p maxCorners - 1) 
For example: If the buffers are sized to hold 10 corners, the function will never return more than 9 corners. 
So if 10 corners are needed, the buffers should be sized for 11 corners.

If the target is within range, it will be the last corner and have a polygon reference id of zero.
*/
int dtPathCorridor::findCorners(float* cornerVerts, unsigned char* cornerFlags,
							  dtPolyRef* cornerPolys, const int maxCorners,
							  dtNavMeshQuery* navquery, const dtQueryFilter* /*filter*/,
							  int options )
{
	dtAssert(m_path);
	dtAssert(m_npath);
	
	static const float MIN_TARGET_DIST = 0.01f;
	
	int ncorners = 0;
	navquery->findStraightPath(m_pos, m_target, m_path, m_npath,
		cornerVerts, cornerFlags, cornerPolys, &ncorners, maxCorners, options);
	
	// Prune points in the beginning of the path which are too close.
	while (ncorners)
	{
		if ((cornerFlags[0] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
			dtVdist2DSqr(&cornerVerts[0], m_pos) > dtSqr(MIN_TARGET_DIST))
			break;
		ncorners--;
		if (ncorners)
		{
			memmove(cornerFlags, cornerFlags+1, sizeof(unsigned char)*ncorners);
			memmove(cornerPolys, cornerPolys+1, sizeof(dtPolyRef)*ncorners);
			memmove(cornerVerts, cornerVerts+3, sizeof(float)*3*ncorners);
		}
	}
	
	// Prune points after an off-mesh connection.
	/*for (int i = 0; i < ncorners; ++i)
	{
		if (cornerFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
		{
			ncorners = i+1;
			break;
		}
	}*/
	
	return ncorners;
}
Beispiel #18
0
void dtPathCorridor::pruneOffmeshConenction(dtPolyRef offMeshConRef)
{
	dtAssert(m_path);
	dtAssert(m_npath);

	// Advance the path up to and over the off-mesh connection.
	dtPolyRef polyRef = m_path[0];
	int npos = 0;
	while (npos < m_npath && polyRef != offMeshConRef)
	{
		polyRef = m_path[npos];
		npos++;
	}
	
	// Prune path
	if (npos != m_npath)
	{
		for (int i = npos; i < m_npath; ++i)
			m_path[i - npos] = m_path[i];
		m_npath -= npos;
	}
}
Beispiel #19
0
dtNodePool::dtNodePool(int maxNodes, int hashSize) :
	m_nodes(0),
	m_first(0),
	m_next(0),
	m_maxNodes(maxNodes),
	m_hashSize(hashSize),
	m_nodeCount(0)
{
	dtAssert(dtNextPow2(m_hashSize) == (unsigned int)m_hashSize);
	dtAssert(m_maxNodes > 0);

	m_nodes = (dtNode*)dtAlloc(sizeof(dtNode)*m_maxNodes, DT_ALLOC_PERM);
	m_next = (dtNodeRef*)dtAlloc(sizeof(dtNodeRef)*m_maxNodes, DT_ALLOC_PERM);
	m_first = (dtNodeRef*)dtAlloc(sizeof(dtNodeRef)*hashSize, DT_ALLOC_PERM);

	dtAssert(m_nodes);
	dtAssert(m_next);
	dtAssert(m_first);

	memset(m_first, 0xff, sizeof(dtNodeRef)*m_hashSize);
	memset(m_next, 0xff, sizeof(dtNodeRef)*m_maxNodes);
}
Beispiel #20
0
bool dtPathCorridor::trimInvalidPath(dtPolyRef safeRef, const float* safePos,
									 dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
	dtAssert(navquery);
	dtAssert(filter);
	dtAssert(m_path);
	
	// Keep valid path as far as possible.
	int n = 0;
	while (n < m_npath && navquery->isValidPolyRef(m_path[n], filter)) {
		n++;
	}
	
	if (n == m_npath)
	{
		// All valid, no need to fix.
		return true;
	}
	else if (n == 0)
	{
		// The first polyref is bad, use current safe values.
		dtVcopy(m_pos, safePos);
		m_path[0] = safeRef;
		m_npath = 1;
	}
	else
	{
		// The path is partially usable.
		m_npath = n;
	}
	
	// Clamp target pos to last poly
	float tgt[3];
	dtVcopy(tgt, m_target);
	navquery->closestPointOnPolyBoundary(m_path[m_npath-1], tgt, m_target);
	
	return true;
}
Beispiel #21
0
bool dtPathCorridor::fixPathStart(dtPolyRef safeRef, const float* safePos)
{
	dtAssert(m_path);

	dtVcopy(m_pos, safePos);
	if (m_npath < 3 && m_npath > 0)
	{
		m_path[2] = m_path[m_npath-1];
		m_path[0] = safeRef;
		m_path[1] = 0;
		m_npath = 3;
	}
	else
	{
		m_path[0] = safeRef;
		m_path[1] = 0;
	}
	
	return true;
}
static int addNeighbour(const int idx, const float dist,
						dtCrowdNeighbour* neis, const int nneis, const int maxNeis)
{
	// Insert neighbour based on the distance.
	dtCrowdNeighbour* nei = 0;
	if (!nneis)
	{
		nei = &neis[nneis];
	}
	else if (dist >= neis[nneis-1].dist)
	{
		if (nneis >= maxNeis)
			return nneis;
		nei = &neis[nneis];
	}
	else
	{
		int i;
		for (i = 0; i < nneis; ++i)
			if (dist <= neis[i].dist)
				break;
		
		const int tgt = i+1;
		const int n = dtMin(nneis-i, maxNeis-tgt);
		
		dtAssert(tgt+n <= maxNeis);
		
		if (n > 0)
			memmove(&neis[tgt], &neis[i], sizeof(dtCrowdNeighbour)*n);
		nei = &neis[i];
	}
	
	memset(nei, 0, sizeof(dtCrowdNeighbour));
	
	nei->idx = idx;
	nei->dist = dist;
	
	return dtMin(nneis+1, maxNeis);
}
void dtObstacleAvoidanceDebugData::addSample(const float* vel, const float ssize, const float pen,
											 const float vpen, const float vcpen, const float spen, const float tpen)
{
	if (m_nsamples >= m_maxSamples)
		return;
	dtAssert(m_vel);
	dtAssert(m_ssize);
	dtAssert(m_pen);
	dtAssert(m_vpen);
	dtAssert(m_vcpen);
	dtAssert(m_spen);
	dtAssert(m_tpen);
	dtVcopy(&m_vel[m_nsamples*3], vel);
	m_ssize[m_nsamples] = ssize;
	m_pen[m_nsamples] = pen;
	m_vpen[m_nsamples] = vpen;
	m_vcpen[m_nsamples] = vcpen;
	m_spen[m_nsamples] = spen;
	m_tpen[m_nsamples] = tpen;
	m_nsamples++;
}
void dtLocalBoundary::addSegment(const float dist, const float* s)
{
	// Insert neighbour based on the distance.
	Segment* seg = 0;
	if (!m_nsegs)
	{
		// First, trivial accept.
		seg = &m_segs[0];
	}
	else if (dist >= m_segs[m_nsegs-1].d)
	{
		// Further than the last segment, skip.
		if (m_nsegs >= MAX_LOCAL_SEGS)
			return;
		// Last, trivial accept.
		seg = &m_segs[m_nsegs];
	}
	else
	{
		// Insert inbetween.
		int i;
		for (i = 0; i < m_nsegs; ++i)
			if (dist <= m_segs[i].d)
				break;
		const int tgt = i+1;
		const int n = dtMin(m_nsegs-i, MAX_LOCAL_SEGS-tgt);
		dtAssert(tgt+n <= MAX_LOCAL_SEGS);
		if (n > 0)
			memmove(&m_segs[tgt], &m_segs[i], sizeof(Segment)*n);
		seg = &m_segs[i];
	}
	
	seg->d = dist;
	memcpy(seg->s, s, sizeof(float)*6);
	
	if (m_nsegs < MAX_LOCAL_SEGS)
		m_nsegs++;
}
Beispiel #25
0
int dtMergeCorridorEndMoved(dtPolyRef* path, const int npath, const int maxPath,
							const dtPolyRef* visited, const int nvisited)
{
	int furthestPath = -1;
	int furthestVisited = -1;
	
	// Find furthest common polygon.
	for (int i = 0; i < npath; ++i)
	{
		bool found = false;
		for (int j = nvisited-1; j >= 0; --j)
		{
			if (path[i] == visited[j])
			{
				furthestPath = i;
				furthestVisited = j;
				found = true;
			}
		}
		if (found)
			break;
	}
	
	// If no intersection found just return current path. 
	if (furthestPath == -1 || furthestVisited == -1)
		return npath;
	
	// Concatenate paths.
	const int ppos = furthestPath+1;
	const int vpos = furthestVisited+1;
	const int count = dtMin(nvisited-vpos, maxPath-ppos);
	dtAssert(ppos+count <= maxPath);
	if (count)
		memcpy(path+ppos, visited+vpos, sizeof(dtPolyRef)*count);
	
	return ppos+count;
}
void dtCrowd::updateMoveRequest(const float /*dt*/)
{
	const int PATH_MAX_AGENTS = 8;
	dtCrowdAgent* queue[PATH_MAX_AGENTS];
	int nqueue = 0;
	
	// Fire off new requests.
	for (int i = 0; i < m_maxAgents; ++i)
	{
		dtCrowdAgent* ag = &m_agents[i];
		if (!ag->active)
			continue;
		if (ag->state == DT_CROWDAGENT_STATE_INVALID)
			continue;
		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
			continue;

		if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING)
		{
			const dtPolyRef* path = ag->corridor.getPath();
			const int npath = ag->corridor.getPathCount();
			dtAssert(npath);

			static const int MAX_RES = 32;
			float reqPos[3];
			dtPolyRef reqPath[MAX_RES];	// The path to the request location
			int reqPathCount = 0;

			// Quick seach towards the goal.
			static const int MAX_ITER = 20;
			m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filter);
			m_navquery->updateSlicedFindPath(MAX_ITER, 0);
			dtStatus status = 0;
			if (ag->targetReplan) // && npath > 10)
			{
				// Try to use existing steady path during replan if possible.
				status = m_navquery->finalizeSlicedFindPathPartial(path, npath, reqPath, &reqPathCount, MAX_RES);
			}
			else
			{
				// Try to move towards target when goal changes.
				status = m_navquery->finalizeSlicedFindPath(reqPath, &reqPathCount, MAX_RES);
			}

			if (!dtStatusFailed(status) && reqPathCount > 0)
			{
				// In progress or succeed.
				if (reqPath[reqPathCount-1] != ag->targetRef)
				{
					// Partial path, constrain target position inside the last polygon.
					status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos, 0);
					if (dtStatusFailed(status))
						reqPathCount = 0;
				}
				else
				{
					dtVcopy(reqPos, ag->targetPos);
				}
			}
			else
			{
				reqPathCount = 0;
			}
				
			if (!reqPathCount)
			{
				// Could not find path, start the request from current location.
				dtVcopy(reqPos, ag->npos);
				reqPath[0] = path[0];
				reqPathCount = 1;
			}

			ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
			ag->boundary.reset();

			if (reqPath[reqPathCount-1] == ag->targetRef)
			{
				ag->targetState = DT_CROWDAGENT_TARGET_VALID;
				ag->targetReplanTime = 0.0;
			}
			else
			{
				// The path is longer or potentially unreachable, full plan.
				ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE;
			}
		}
		
		if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
		{
			nqueue = addToPathQueue(ag, queue, nqueue, PATH_MAX_AGENTS);
		}
	}

	for (int i = 0; i < nqueue; ++i)
	{
		dtCrowdAgent* ag = queue[i];
		ag->targetPathqRef = m_pathq.request(ag->corridor.getLastPoly(), ag->targetRef,
											 ag->corridor.getTarget(), ag->targetPos, &m_filter);
		if (ag->targetPathqRef != DT_PATHQ_INVALID)
			ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
	}

	
	// Update requests.
	m_pathq.update(MAX_ITERS_PER_UPDATE);

	dtStatus status;

	// Process path results.
	for (int i = 0; i < m_maxAgents; ++i)
	{
		dtCrowdAgent* ag = &m_agents[i];
		if (!ag->active)
			continue;
		if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
			continue;
		
		if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
		{
			// Poll path queue.
			status = m_pathq.getRequestStatus(ag->targetPathqRef);
			if (dtStatusFailed(status))
			{
				// Path find failed, retry if the target location is still valid.
				ag->targetPathqRef = DT_PATHQ_INVALID;
				if (ag->targetRef)
					ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
				else
					ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
				ag->targetReplanTime = 0.0;
			}
			else if (dtStatusSucceed(status))
			{
				const dtPolyRef* path = ag->corridor.getPath();
				const int npath = ag->corridor.getPathCount();
				dtAssert(npath);
				
				// Apply results.
				float targetPos[3];
				dtVcopy(targetPos, ag->targetPos);
				
				dtPolyRef* res = m_pathResult;
				bool valid = true;
				int nres = 0;
				status = m_pathq.getPathResult(ag->targetPathqRef, res, &nres, m_maxPathResult);
				if (dtStatusFailed(status) || !nres)
					valid = false;
				
				// Merge result and existing path.
				// The agent might have moved whilst the request is
				// being processed, so the path may have changed.
				// We assume that the end of the path is at the same location
				// where the request was issued.
				
				// The last ref in the old path should be the same as
				// the location where the request was issued..
				if (valid && path[npath-1] != res[0])
					valid = false;
				
				if (valid)
				{
					// Put the old path infront of the old path.
					if (npath > 1)
					{
						// Make space for the old path.
						if ((npath-1)+nres > m_maxPathResult)
							nres = m_maxPathResult - (npath-1);
						
						memmove(res+npath-1, res, sizeof(dtPolyRef)*nres);
						// Copy old path in the beginning.
						memcpy(res, path, sizeof(dtPolyRef)*(npath-1));
						nres += npath-1;
						
						// Remove trackbacks
						for (int j = 0; j < nres; ++j)
						{
							if (j-1 >= 0 && j+1 < nres)
							{
								if (res[j-1] == res[j+1])
								{
									memmove(res+(j-1), res+(j+1), sizeof(dtPolyRef)*(nres-(j+1)));
									nres -= 2;
									j -= 2;
								}
							}
						}
						
					}
					
					// Check for partial path.
					if (res[nres-1] != ag->targetRef)
					{
						// Partial path, constrain target position inside the last polygon.
						float nearest[3];
						status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest, 0);
						if (dtStatusSucceed(status))
							dtVcopy(targetPos, nearest);
						else
							valid = false;
					}
				}
				
				if (valid)
				{
					// Set current corridor.
					ag->corridor.setCorridor(targetPos, res, nres);
					// Force to update boundary.
					ag->boundary.reset();
					ag->targetState = DT_CROWDAGENT_TARGET_VALID;
				}
				else
				{
					// Something went wrong.
					ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
				}

				ag->targetReplanTime = 0.0;
			}
		}
	}
	
}
static dtStatus CollectRegionsChunky(dtTileCacheAlloc* alloc, dtTileCacheLayer& layer, int chunkSize, dtLayerMonotoneRegion*& regs, int& nregs)
{
    dtAssert(alloc);

    const int w = (int)layer.header->width;
    const int h = (int)layer.header->height;

    memset(layer.regs,0xff,sizeof(unsigned short)*w*h);

    const int nsweeps = w;
    dtFixedArray<dtLayerSweepSpan> sweeps(alloc, nsweeps);
    if (!sweeps)
        return DT_FAILURE | DT_OUT_OF_MEMORY;
    memset(sweeps,0,sizeof(dtLayerSweepSpan)*nsweeps);

    // Partition walkable area into monotone regions.
    dtIntArray prevCount(256);
    unsigned short regId = 0;

    for (int chunkx = 0; chunkx < w; chunkx += chunkSize)
    {
        for (int chunky = 0; chunky < h; chunky += chunkSize)
        {
            const int maxx = dtMin(chunkx + chunkSize, w);
            const int maxy = dtMin(chunky + chunkSize, h);

            for (int y = chunky; y < maxy; ++y)
            {
                prevCount.resize(regId+1);
                memset(&prevCount[0],0,sizeof(int)*regId);
                unsigned short sweepId = 0;

                for (int x = chunkx; x < maxx; ++x)
                {
                    const int idx = x + y*w;
                    if (layer.areas[idx] == DT_TILECACHE_NULL_AREA) continue;

                    unsigned short sid = 0xffff;

                    // -x
                    if (x > chunkx && isConnected(layer, idx, 0))
                    {
                        const int xidx = (x-1)+y*w;
                        if (layer.regs[xidx] != 0xffff && layer.areas[xidx] == layer.areas[idx])
                            sid = layer.regs[xidx];
                    }

                    if (sid == 0xffff)
                    {
                        sid = sweepId++;
                        sweeps[sid].nei = 0xffff;
                        sweeps[sid].ns = 0;
                    }

                    // -y
                    if (y > chunky && isConnected(layer, idx, 3))
                    {
                        const int yidx = x+(y-1)*w;
                        const unsigned short nr = layer.regs[yidx];
                        if (nr != 0xffff && layer.areas[yidx] == layer.areas[idx])
                        {
                            // Set neighbour when first valid neighbour is encoutered.
                            if (sweeps[sid].ns == 0)
                                sweeps[sid].nei = nr;

                            if (sweeps[sid].nei == nr)
                            {
                                // Update existing neighbour
                                sweeps[sid].ns++;
                                prevCount[nr]++;
                            }
                            else
                            {
                                // This is hit if there is nore than one neighbour.
                                // Invalidate the neighbour.
                                sweeps[sid].nei = 0xffff;
                            }
                        }
                    }

                    layer.regs[idx] = sid;
                }

                // Create unique ID.
                for (int i = 0; i < sweepId; ++i)
                {
                    // If the neighbour is set and there is only one continuous connection to it,
                    // the sweep will be merged with the previous one, else new region is created.
                    if (sweeps[i].nei != 0xffff && prevCount[sweeps[i].nei] == sweeps[i].ns)
                    {
                        sweeps[i].id = sweeps[i].nei;
                    }
                    else
                    {
                        sweeps[i].id = regId++;
                    }
                }

                // Remap local sweep ids to region ids.
                for (int x = chunkx; x < maxx; ++x)
                {
                    const int idx = x+y*w;
                    if (layer.regs[idx] != 0xffff)
                    {
                        unsigned short id = sweeps[layer.regs[idx]].id;

                        layer.regs[idx] = id;
                    }
                }
            }
        }
    }

    // Allocate and init layer regions.
    nregs = (int)regId;
    regs = (dtLayerMonotoneRegion*)alloc->alloc(sizeof(dtLayerMonotoneRegion) * nregs);
    if (!regs)
        return DT_FAILURE | DT_OUT_OF_MEMORY;

    memset(regs, 0, sizeof(dtLayerMonotoneRegion)*nregs);
    for (int i = 0; i < nregs; ++i)
    {
        regs[i].regId = 0xffff;
        regs[i].neis.resize(16);
        regs[i].neis.resize(0);
    }

    // Find region neighbours.
    for (int y = 0; y < h; ++y)
    {
        const int chunkYOffset = (y / chunkSize) * chunkSize;
        const bool borderY = (y == 0) || (y == (h - 1));
        for (int x = 0; x < w; ++x)
        {
            const int idx = x+y*w;
            const unsigned short ri = layer.regs[idx];
            if (ri == 0xffff)
                continue;

            // Update area.
            regs[ri].area++;
            regs[ri].areaId = layer.areas[idx];
            regs[ri].chunkId = (x / chunkSize) + chunkYOffset;
            regs[ri].border |= borderY || (x == 0) || (x == (w - 1));

            // Update neighbours
            if (y > 0 && isConnected(layer, idx, 3))
            {
                const int ymi = x+(y-1)*w;
                const unsigned short rai = layer.regs[ymi];
                if (rai != 0xffff && rai != ri)
                {
                    addUniqueLast(regs[ri].neis, rai);
                    addUniqueLast(regs[rai].neis, ri);
                }
            }
        }
    }

    return DT_SUCCESS;
}
dtStatus dtBuildTileCacheRegions(dtTileCacheAlloc* alloc,
                                 const int minRegionArea, const int mergeRegionArea,
                                 dtTileCacheLayer& layer, dtTileCacheDistanceField dfield)
{
    dtAssert(alloc);

    const int w = (int)layer.header->width;
    const int h = (int)layer.header->height;
    const int size = w*h;

    dtFixedArray<unsigned short> buf(alloc, size*4);
    if (!buf)
    {
        return DT_FAILURE | DT_OUT_OF_MEMORY;
    }

    dtIntArray stack(1024);
    dtIntArray visited(1024);

    unsigned short* srcReg = buf;
    unsigned short* srcDist = buf+size;
    unsigned short* dstReg = buf+size*2;
    unsigned short* dstDist = buf+size*3;

    memset(srcReg, 0, sizeof(unsigned short)*size);
    memset(srcDist, 0, sizeof(unsigned short)*size);

    unsigned short regionId = 1;
    unsigned short level = (dfield.maxDist+1) & ~1;

    // TODO: Figure better formula, expandIters defines how much the
    // watershed "overflows" and simplifies the regions. Tying it to
    // agent radius was usually good indication how greedy it could be.
    //	const int expandIters = 4 + walkableRadius * 2;
    const int expandIters = 8;

    while (level > 0)
    {
        level = level >= 2 ? level-2 : 0;

        // Expand current regions until no empty connected cells found.
        if (expandRegions(expandIters, level, layer, dfield, srcReg, srcDist, dstReg, dstDist, stack) != srcReg)
        {
            dtSwap(srcReg, dstReg);
            dtSwap(srcDist, dstDist);
        }

        // Mark new regions with IDs.
        for (int y = 0; y < h; ++y)
        {
            for (int x = 0; x < w; ++x)
            {
                const int i=x+y*w;
                if (dfield.data[i] < level || srcReg[i] != 0 || layer.areas[i] == DT_TILECACHE_NULL_AREA)
                    continue;
                if (floodRegion(x, y, i, level, regionId, layer, dfield, srcReg, srcDist, stack))
                    regionId++;
            }
        }
    }

    // Expand current regions until no empty connected cells found.
    if (expandRegions(expandIters*8, 0, layer, dfield, srcReg, srcDist, dstReg, dstDist, stack) != srcReg)
    {
        dtSwap(srcReg, dstReg);
        dtSwap(srcDist, dstDist);
    }

    dtStatus status = filterSmallRegions(alloc, layer, minRegionArea, mergeRegionArea, regionId, srcReg);
    if (dtStatusFailed(status))
    {
        return status;
    }

    // Write the result out.
    memcpy(layer.regs, srcReg, sizeof(unsigned short)*size);
    layer.regCount = regionId;

    return DT_SUCCESS;
}
Beispiel #29
0
void dtCrowd::updateMoveRequest(const float /*dt*/)
{
	// Fire off new requests.
	for (int i = 0; i < m_moveRequestCount; ++i)
	{
		MoveRequest* req = &m_moveRequests[i];
		dtCrowdAgent* ag = &m_agents[req->idx];
		
		// Agent not active anymore, kill request.
		if (!ag->active)
			req->state = MR_TARGET_FAILED;
		
		// Adjust target
		if (req->aref)
		{
			if (req->state == MR_TARGET_ADJUST)
			{
				// Adjust existing path.
				ag->corridor.moveTargetPosition(req->apos, m_navquery, &m_filter);
				req->state = MR_TARGET_VALID;
			}
			else
			{
				// Adjust on the flight request.
				float result[3];
				static const int MAX_VISITED = 16;
				dtPolyRef visited[MAX_VISITED];
				int nvisited = 0;
				m_navquery->moveAlongSurface(req->temp[req->ntemp-1], req->pos, req->apos, &m_filter,
											 result, visited, &nvisited, MAX_VISITED);
				req->ntemp = dtMergeCorridorEndMoved(req->temp, req->ntemp, MAX_TEMP_PATH, visited, nvisited);
				dtVcopy(req->pos, result);
				
				// Reset adjustment.
				dtVset(req->apos, 0,0,0);
				req->aref = 0;
			}
		}
		
		
		if (req->state == MR_TARGET_REQUESTING)
		{
			// Calculate request position.
			// If there is a lot of latency between requests, it is possible to
			// project the current position ahead and use raycast to find the actual
			// location and path.
			const dtPolyRef* path = ag->corridor.getPath();
			const int npath = ag->corridor.getPathCount();
			dtAssert(npath);
			
			// Here we take the simple approach and set the path to be just the current location.
			float reqPos[3];
			dtVcopy(reqPos, ag->corridor.getPos());	// The location of the request
			dtPolyRef reqPath[8];					// The path to the request location
			reqPath[0] = path[0];
			int reqPathCount = 1;
			
			req->pathqRef = m_pathq.request(reqPath[reqPathCount-1], req->ref, reqPos, req->pos, &m_filter);
			if (req->pathqRef != DT_PATHQ_INVALID)
			{
				ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
				req->state = MR_TARGET_WAITING_FOR_PATH;
			}
		}
	}

	
	// Update requests.
	m_pathq.update(MAX_ITERS_PER_UPDATE);

	// Process path results.
	for (int i = 0; i < m_moveRequestCount; ++i)
	{
		MoveRequest* req = &m_moveRequests[i];
		dtCrowdAgent* ag = &m_agents[req->idx];
		
		if (req->state == MR_TARGET_WAITING_FOR_PATH)
		{
			// Poll path queue.
			dtStatus status = m_pathq.getRequestStatus(req->pathqRef);
			if (dtStatusFailed(status))
			{
				req->pathqRef = DT_PATHQ_INVALID;
				req->state = MR_TARGET_FAILED;
			}
			else if (dtStatusSucceed(status))
			{
				const dtPolyRef* path = ag->corridor.getPath();
				const int npath = ag->corridor.getPathCount();
				dtAssert(npath);
				
				// Apply results.
				float targetPos[3];
				dtVcopy(targetPos, req->pos);
				
				dtPolyRef* res = m_pathResult;
				bool valid = true;
				int nres = 0;
				dtStatus status = m_pathq.getPathResult(req->pathqRef, res, &nres, m_maxPathResult);
				if (dtStatusFailed(status) || !nres)
					valid = false;
				
				// Merge with any target adjustment that happened during the search.
				if (req->ntemp > 1)
				{
					nres = dtMergeCorridorEndMoved(res, nres, m_maxPathResult, req->temp, req->ntemp);
				}
				
				// Merge result and existing path.
				// The agent might have moved whilst the request is
				// being processed, so the path may have changed.
				// We assume that the end of the path is at the same location
				// where the request was issued.
				
				// The last ref in the old path should be the same as
				// the location where the request was issued..
				if (valid && path[npath-1] != res[0])
					valid = false;
				
				if (valid)
				{
					// Put the old path infront of the old path.
					if (npath > 1)
					{
						// Make space for the old path.
						if ((npath-1)+nres > m_maxPathResult)
							nres = m_maxPathResult - (npath-1);
						memmove(res+npath-1, res, sizeof(dtPolyRef)*nres);
						// Copy old path in the beginning.
						memcpy(res, path, sizeof(dtPolyRef)*(npath-1));
						nres += npath-1;
					}
					
					// Check for partial path.
					if (res[nres-1] != req->ref)
					{
						// Partial path, constrain target position inside the last polygon.
						float nearest[3];
						if (m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest) == DT_SUCCESS)
							dtVcopy(targetPos, nearest);
						else
							valid = false;
					}
				}
				
				if (valid)
				{
					ag->corridor.setCorridor(targetPos, res, nres);
					req->state = MR_TARGET_VALID;
				}
				else
				{
					// Something went wrong.
					req->state = MR_TARGET_FAILED;
				}
			}
		}
		
		// Remove request when done with it.
		if (req->state == MR_TARGET_VALID || req->state == MR_TARGET_FAILED)
		{
			m_moveRequestCount--;
			if (i != m_moveRequestCount)
				memcpy(&m_moveRequests[i], &m_moveRequests[m_moveRequestCount], sizeof(MoveRequest));
			--i;
		}
	}
	
}
dtStatus dtTileCache::buildNavMeshTile(const dtCompressedTileRef ref, dtNavMesh* navmesh)
{	
	dtAssert(m_talloc);
	dtAssert(m_tcomp);
	
	unsigned int idx = decodeTileIdTile(ref);
	if (idx > (unsigned int)m_params.maxTiles)
		return DT_FAILURE | DT_INVALID_PARAM;
	const dtCompressedTile* tile = &m_tiles[idx];
	unsigned int salt = decodeTileIdSalt(ref);
	if (tile->salt != salt)
		return DT_FAILURE | DT_INVALID_PARAM;
	
	m_talloc->reset();
	
	BuildContext bc(m_talloc);
	const int walkableClimbVx = (int)(m_params.walkableClimb / m_params.ch);
	dtStatus status;
	
	// Decompress tile layer data. 
	status = dtDecompressTileCacheLayer(m_talloc, m_tcomp, tile->data, tile->dataSize, &bc.layer);
	if (dtStatusFailed(status))
		return status;

#if 0
	if (tile->header->tx != 1 || tile->header->ty != 0 || tile->header->tlayer < 1)
		return status;
#endif
	
	// Rasterize obstacles.
	for (int i = 0; i < m_params.maxObstacles; ++i)
	{
		const dtTileCacheObstacle* ob = &m_obstacles[i];
		if (ob->state == DT_OBSTACLE_EMPTY || ob->state == DT_OBSTACLE_REMOVING)
			continue;
		if (contains(ob->touched, ob->ntouched, ref))
		{
			dtMarkCylinderArea(*bc.layer, tile->header->bmin, m_params.cs, m_params.ch,
							   ob->pos, ob->radius, ob->height, 0);
		}
	}
	
	if (m_tmproc)
	{
		m_tmproc->markAreas(bc.layer, tile->header->bmin, m_params.cs, m_params.ch);
	}

	// Build navmesh
	if (m_params.regionPartitioning == DT_REGION_MONOTONE)
	{
		status = dtBuildTileCacheRegionsMonotone(m_talloc, m_params.minRegionArea, m_params.mergeRegionArea, *bc.layer);
	}
	else if (m_params.regionPartitioning == DT_REGION_WATERSHED)
	{
		bc.dfield = dtAllocTileCacheDistanceField(m_talloc);
		if (!bc.dfield)
			return status;

		status = dtBuildTileCacheDistanceField(m_talloc, *bc.layer, *bc.dfield);
		if (dtStatusFailed(status))
			return status;

		status = dtBuildTileCacheRegions(m_talloc, m_params.minRegionArea, m_params.mergeRegionArea, *bc.layer, *bc.dfield);
	}
	else
	{
		status = dtBuildTileCacheRegionsChunky(m_talloc, m_params.minRegionArea, m_params.mergeRegionArea, *bc.layer, m_params.regionChunkSize);
	}
	if (dtStatusFailed(status))
		return status;
	
	bc.lcset = dtAllocTileCacheContourSet(m_talloc);
	bc.lclusters = dtAllocTileCacheClusterSet(m_talloc);
	if (!bc.lcset || !bc.lclusters)
		return status;
	status = dtBuildTileCacheContours(m_talloc, *bc.layer, walkableClimbVx,
									  m_params.maxSimplificationError, m_params.cs, m_params.ch,
									  *bc.lcset, *bc.lclusters);
	if (dtStatusFailed(status))
		return status;
	
	bc.lmesh = dtAllocTileCachePolyMesh(m_talloc);
	if (!bc.lmesh)
		return status;
	status = dtBuildTileCachePolyMesh(m_talloc, 0, *bc.lcset, *bc.lmesh);
	if (dtStatusFailed(status))
		return status;
	
	// Early out if the mesh tile is empty.
	if (!bc.lmesh->npolys)
		return DT_SUCCESS;

	status = dtBuildTileCacheClusters(m_talloc, *bc.lclusters, *bc.lmesh);
	if (dtStatusFailed(status))
		return status;
	
	dtNavMeshCreateParams params;
	memset(&params, 0, sizeof(params));
	params.verts = bc.lmesh->verts;
	params.vertCount = bc.lmesh->nverts;
	params.polys = bc.lmesh->polys;
	params.polyAreas = bc.lmesh->areas;
	params.polyFlags = bc.lmesh->flags;
	params.polyCount = bc.lmesh->npolys;
	params.nvp = DT_VERTS_PER_POLYGON;
	params.walkableHeight = m_params.walkableHeight;
	params.walkableRadius = m_params.walkableRadius;
	params.walkableClimb = m_params.walkableClimb;
	params.tileX = tile->header->tx;
	params.tileY = tile->header->ty;
	params.tileLayer = tile->header->tlayer;
	params.cs = m_params.cs;
	params.ch = m_params.ch;
	params.buildBvTree = false;
	dtVcopy(params.bmin, tile->header->bmin);
	dtVcopy(params.bmax, tile->header->bmax);
	params.polyClusters = bc.lclusters->polyMap;
	params.clusterCount = (unsigned short)bc.lclusters->nclusters;
	
	if (m_tmproc)
	{
		m_tmproc->process(&params, bc.lmesh->areas, bc.lmesh->flags);
	}
	
	unsigned char* navData = 0;
	int navDataSize = 0;
	if (!dtCreateNavMeshData(&params, &navData, &navDataSize))
		return DT_FAILURE;

	// Remove existing tile.
	navmesh->removeTile(navmesh->getTileRefAt(tile->header->tx,tile->header->ty,tile->header->tlayer),0,0);

	// Add new tile, or leave the location empty.
	if (navData)
	{
		// Let the navmesh own the data.
		status = navmesh->addTile(navData,navDataSize,DT_TILE_FREE_DATA,0,0);
		if (dtStatusFailed(status))
		{
			dtFree(navData);
			return status;
		}
	}
	
	return DT_SUCCESS;
}