diff --git a/DetourCrowd/Include/DetourPathCorridor.h b/DetourCrowd/Include/DetourPathCorridor.h index 6a3d338..9544ea5 100644 --- a/DetourCrowd/Include/DetourPathCorridor.h +++ b/DetourCrowd/Include/DetourPathCorridor.h @@ -76,6 +76,8 @@ public: float* startPos, float* endPos, dtNavMeshQuery* navquery); + bool fixPathStart(dtPolyRef safeRef, const float* safePos); + bool trimInvalidPath(dtPolyRef safeRef, const float* safePos, dtNavMeshQuery* navquery, const dtQueryFilter* filter); diff --git a/DetourCrowd/Source/DetourCrowd.cpp b/DetourCrowd/Source/DetourCrowd.cpp index e14c7ed..e7312ef 100644 --- a/DetourCrowd/Source/DetourCrowd.cpp +++ b/DetourCrowd/Source/DetourCrowd.cpp @@ -46,7 +46,7 @@ void dtFreeCrowd(dtCrowd* ptr) } -static const int MAX_ITERS_PER_UPDATE = 10; +static const int MAX_ITERS_PER_UPDATE = 100; static const int MAX_PATHQUEUE_NODES = 4096; static const int MAX_COMMON_NODES = 512; @@ -218,6 +218,79 @@ static int getNeighbours(const float* pos, const float height, const float range return n; } +static int addToOptQueue(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->topologyOptTime <= agents[nagents-1]->topologyOptTime) + { + if (nagents >= maxAgents) + return nagents; + slot = nagents; + } + else + { + int i; + for (i = 0; i < nagents; ++i) + if (newag->topologyOptTime >= agents[i]->topologyOptTime) + 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); +} + +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); +} + + /** @class dtCrowd @par @@ -584,6 +657,10 @@ int dtCrowd::getActiveAgents(dtCrowdAgent** agents, const int maxAgents) 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) { @@ -611,7 +688,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/) 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) + 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); @@ -667,12 +744,19 @@ void dtCrowd::updateMoveRequest(const float /*dt*/) if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE) { - 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; + 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); @@ -795,43 +879,6 @@ void dtCrowd::updateMoveRequest(const float /*dt*/) } - -static int addToOptQueue(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->topologyOptTime <= agents[nagents-1]->topologyOptTime) - { - if (nagents >= maxAgents) - return nagents; - slot = nagents; - } - else - { - int i; - for (i = 0; i < nagents; ++i) - if (newag->topologyOptTime >= agents[i]->topologyOptTime) - 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); -} - void dtCrowd::updateTopologyOptimization(dtCrowdAgent** agents, const int nagents, const float dt) { if (!nagents) @@ -907,6 +954,10 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const continue; } + // Make sure the first polygon is valid, but leave other valid + // polygons in the path so that replanner can adjust the path better. + ag->corridor.fixPathStart(agentRef, agentPos); +// ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter); ag->boundary.reset(); dtVcopy(ag->npos, agentPos); @@ -936,8 +987,8 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter)) { // Fix current path. - ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter); - ag->boundary.reset(); +// ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter); +// ag->boundary.reset(); replan = true; } diff --git a/DetourCrowd/Source/DetourPathCorridor.cpp b/DetourCrowd/Source/DetourPathCorridor.cpp index 23b16f2..a1bfe0d 100644 --- a/DetourCrowd/Source/DetourPathCorridor.cpp +++ b/DetourCrowd/Source/DetourPathCorridor.cpp @@ -510,6 +510,27 @@ void dtPathCorridor::setCorridor(const float* target, const dtPolyRef* path, con m_npath = npath; } +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; +} + bool dtPathCorridor::trimInvalidPath(dtPolyRef safeRef, const float* safePos, dtNavMeshQuery* navquery, const dtQueryFilter* filter) {