LNXSDK/lib/haxerecast/recastnavigation/DetourCrowd/Source/DetourPathQueue.cpp

//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty.  In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
//    claim that you wrote the original software. If you use this software
//    in a product, an acknowledgment in the product documentation would be
//    appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
//    misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//

#include <string.h>
#include "DetourPathQueue.h"
#include "DetourNavMesh.h"
#include "DetourNavMeshQuery.h"
#include "DetourAlloc.h"
#include "DetourCommon.h"


dtPathQueue::dtPathQueue() :
	m_nextHandle(1),
	m_maxPathSize(0),
	m_queueHead(0),
	m_navquery(0)
{
	for (int i = 0; i < MAX_QUEUE; ++i)
		m_queue[i].path = 0;
}

dtPathQueue::~dtPathQueue()
{
	purge();
}

void dtPathQueue::purge()
{
	dtFreeNavMeshQuery(m_navquery);
	m_navquery = 0;
	for (int i = 0; i < MAX_QUEUE; ++i)
	{
		dtFree(m_queue[i].path);
		m_queue[i].path = 0;
	}
}

bool dtPathQueue::init(const int maxPathSize, const int maxSearchNodeCount, dtNavMesh* nav)
{
	purge();

	m_navquery = dtAllocNavMeshQuery();
	if (!m_navquery)
		return false;
	if (dtStatusFailed(m_navquery->init(nav, maxSearchNodeCount)))
		return false;
	
	m_maxPathSize = maxPathSize;
	for (int i = 0; i < MAX_QUEUE; ++i)
	{
		m_queue[i].ref = DT_PATHQ_INVALID;
		m_queue[i].path = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathSize, DT_ALLOC_PERM);
		if (!m_queue[i].path)
			return false;
	}
	
	m_queueHead = 0;
	
	return true;
}

void dtPathQueue::update(const int maxIters)
{
	static const int MAX_KEEP_ALIVE = 2; // in update ticks.

	// Update path request until there is nothing to update
	// or upto maxIters pathfinder iterations has been consumed.
	int iterCount = maxIters;
	
	for (int i = 0; i < MAX_QUEUE; ++i)
	{
		PathQuery& q = m_queue[m_queueHead % MAX_QUEUE];
		
		// Skip inactive requests.
		if (q.ref == DT_PATHQ_INVALID)
		{
			m_queueHead++;
			continue;
		}
		
		// Handle completed request.
		if (dtStatusSucceed(q.status) || dtStatusFailed(q.status))
		{
			// If the path result has not been read in few frames, free the slot.
			q.keepAlive++;
			if (q.keepAlive > MAX_KEEP_ALIVE)
			{
				q.ref = DT_PATHQ_INVALID;
				q.status = 0;
			}
			
			m_queueHead++;
			continue;
		}
		
		// Handle query start.
		if (q.status == 0)
		{
			q.status = m_navquery->initSlicedFindPath(q.startRef, q.endRef, q.startPos, q.endPos, q.filter);
		}		
		// Handle query in progress.
		if (dtStatusInProgress(q.status))
		{
			int iters = 0;
			q.status = m_navquery->updateSlicedFindPath(iterCount, &iters);
			iterCount -= iters;
		}
		if (dtStatusSucceed(q.status))
		{
			q.status = m_navquery->finalizeSlicedFindPath(q.path, &q.npath, m_maxPathSize);
		}

		if (iterCount <= 0)
			break;

		m_queueHead++;
	}
}

dtPathQueueRef dtPathQueue::request(dtPolyRef startRef, dtPolyRef endRef,
									const float* startPos, const float* endPos,
									const dtQueryFilter* filter)
{
	// Find empty slot
	int slot = -1;
	for (int i = 0; i < MAX_QUEUE; ++i)
	{
		if (m_queue[i].ref == DT_PATHQ_INVALID)
		{
			slot = i;
			break;
		}
	}
	// Could not find slot.
	if (slot == -1)
		return DT_PATHQ_INVALID;
	
	dtPathQueueRef ref = m_nextHandle++;
	if (m_nextHandle == DT_PATHQ_INVALID) m_nextHandle++;
	
	PathQuery& q = m_queue[slot];
	q.ref = ref;
	dtVcopy(q.startPos, startPos);
	q.startRef = startRef;
	dtVcopy(q.endPos, endPos);
	q.endRef = endRef;
	
	q.status = 0;
	q.npath = 0;
	q.filter = filter;
	q.keepAlive = 0;
	
	return ref;
}

dtStatus dtPathQueue::getRequestStatus(dtPathQueueRef ref) const
{
	for (int i = 0; i < MAX_QUEUE; ++i)
	{
		if (m_queue[i].ref == ref)
			return m_queue[i].status;
	}
	return DT_FAILURE;
}

dtStatus dtPathQueue::getPathResult(dtPathQueueRef ref, dtPolyRef* path, int* pathSize, const int maxPath)
{
	for (int i = 0; i < MAX_QUEUE; ++i)
	{
		if (m_queue[i].ref == ref)
		{
			PathQuery& q = m_queue[i];
			dtStatus details = q.status & DT_STATUS_DETAIL_MASK;
			// Free request for reuse.
			q.ref = DT_PATHQ_INVALID;
			q.status = 0;
			// Copy path
			int n = dtMin(q.npath, maxPath);
			memcpy(path, q.path, sizeof(dtPolyRef)*n);
			*pathSize = n;
			return details | DT_SUCCESS;
		}
	}
	return DT_FAILURE;
}