Skip to content

Commit

Permalink
- improved replanting logic (better use of existing path)
Browse files Browse the repository at this point in the history 
- added priority q to help end-of-path replanting contestation


git-svn-id: http://recastnavigation.googlecode.com/svn/trunk@342 678f7576-1c49-11de-8b5c-13accb87f508
  • Loading branch information
memononen committed May 31, 2012
1 parent 4e5499a commit 4afc7e7
Show file tree
Hide file tree
Showing 3 changed files with 118 additions and 44 deletions.
2 changes: 2 additions & 0 deletions DetourCrowd/Include/DetourPathCorridor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,8 @@ class dtPathCorridor
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);

Expand Down
139 changes: 95 additions & 44 deletions DetourCrowd/Source/DetourCrowd.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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;
}

Expand Down
21 changes: 21 additions & 0 deletions DetourCrowd/Source/DetourPathCorridor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down

0 comments on commit 4afc7e7

Please sign in to comment.