dtPolyRef PathFinder::getPathPolyByPosition(const dtPolyRef* polyPath, uint32 polyPathSize, const float* point, float* distance) const
{
if (!polyPath || !polyPathSize)
{ return INVALID_POLYREF; }
dtPolyRef nearestPoly = INVALID_POLYREF;
float minDist2d = FLT_MAX;
float minDist3d = 0.0f;
for (uint32 i = 0; i < polyPathSize; ++i)
{
float closestPoint[VERTEX_SIZE];
if (dtStatusFailed(m_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint)))
continue;
float d = dtVdist2DSqr(point, closestPoint);
if (d < minDist2d)
{
minDist2d = d;
nearestPoly = polyPath[i];
minDist3d = dtVdistSqr(point, closestPoint);
}
if (minDist2d < 1.0f) // shortcut out - close enough for us
{ break; }
}
if (distance)
{ *distance = dtSqrt(minDist3d); }
return (minDist2d < 3.0f) ? nearestPoly : INVALID_POLYREF;
}
dtPolyRef PathInfo::getPathPolyByPosition(dtPolyRef *polyPath, uint32 polyPathSize, const float* point, float *distance)
{
if (!polyPath || !polyPathSize)
return INVALID_POLYREF;
dtPolyRef nearestPoly = INVALID_POLYREF;
float minDist2d = FLT_MAX;
float minDist3d = 0.0f;
for (uint32 i = 0; i < polyPathSize; ++i)
{
MANGOS_ASSERT(polyPath[i] != INVALID_POLYREF);
float closestPoint[VERTEX_SIZE];
if (DT_SUCCESS != m_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint))
continue;
float d = dtVdist2DSqr(point, closestPoint);
if (d < minDist2d)
{
minDist2d = d;
nearestPoly = m_pathPolyRefs[i];
minDist3d = dtVdistSqr(point, closestPoint);
}
if(minDist2d < 1.0f) // shortcut out - close enough for us
break;
}
if (distance)
*distance = dtSqrt(minDist3d);
return (minDist2d < 4.0f) ? nearestPoly : INVALID_POLYREF;
}
dtPolyRef PathGenerator::GetPathPolyByPosition(dtPolyRef const* polyPath, uint32 polyPathSize, float const* point, float* distance) const
{
if (!polyPath || !polyPathSize)
return INVALID_POLYREF;
dtPolyRef nearestPoly = INVALID_POLYREF;
float minDist2d = FLT_MAX;
float minDist3d = 0.0f;
for (uint32 i = 0; i < polyPathSize; ++i)
{
float closestPoint[VERTEX_SIZE];
if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint, NULL)))
continue;
float d = dtVdist2DSqr(point, closestPoint);
if (d < minDist2d)
{
minDist2d = d;
nearestPoly = polyPath[i];
minDist3d = dtVdistSqr(point, closestPoint);
}
if (minDist2d < 1.0f) // shortcut out - close enough for us
break;
}
if (distance)
*distance = dtMathSqrtf(minDist3d);
return (minDist2d < 3.0f) ? nearestPoly : INVALID_POLYREF;
}
static bool overOffmeshConnection(const dtCrowdAgent* ag, const float radius)
{
if (!ag->ncorners)
return false;
const bool offMeshConnection = (ag->cornerFlags[ag->ncorners-1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
if (offMeshConnection)
{
const float distSq = dtVdist2DSqr(ag->npos, &ag->cornerVerts[(ag->ncorners-1)*3]);
if (distSq < radius*radius)
return true;
}
return false;
}
static bool withinRadiusOfOffMeshConnection( const Bot_t *bot, rVec pos, rVec off, dtPolyRef conPoly )
{
const dtOffMeshConnection *con = bot->nav->mesh->getOffMeshConnectionByRef( conPoly );
if ( !con )
{
return false;
}
if ( dtVdist2DSqr( pos, off ) < con->rad * con->rad )
{
return true;
}
return false;
}
void dtCrowd::updateEnvironment(unsigned* agentsIdx, unsigned nbIdx)
{
// If we want to update every agent
if (agentsIdx == 0)
{
agentsIdx = m_agentsToUpdate;
nbIdx = m_maxAgents;
}
nbIdx = (nbIdx < m_maxAgents) ? nbIdx : m_maxAgents;
// Get nearby navmesh segments and agents to collide with.
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->state == DT_CROWDAGENT_STATE_INVALID)
m_agentsEnv[ag->id].boundary.reset();
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->perceptionDistance * 0.25f;
if (dtVdist2DSqr(ag->position, m_agentsEnv[ag->id].boundary.getCenter()) > dtSqr(updateThr) ||
!m_agentsEnv[ag->id].boundary.isValid(m_crowdQuery->getNavMeshQuery(), m_crowdQuery->getQueryFilter()))
{
dtPolyRef ref;
float nearest[3];
m_crowdQuery->getNavMeshQuery()->findNearestPoly(ag->position, m_crowdQuery->getQueryExtents(), m_crowdQuery->getQueryFilter(), &ref, nearest);
m_agentsEnv[ag->id].boundary.update(ref, ag->position, ag->perceptionDistance,
m_crowdQuery->getNavMeshQuery(), m_crowdQuery->getQueryFilter());
}
// Query neighbour agents
m_agentsEnv[ag->id].nbNeighbors = this->computeNeighbors(ag->id);
for (unsigned j = 0; j < m_agentsEnv[ag->id].nbNeighbors; j++)
m_agentsEnv[ag->id].neighbors[j].idx = getAgentIndex(&m_agents[m_agentsEnv[ag->id].neighbors[j].idx]);
}
}
dtPolyRef PathFinder::getPathPolyByPosition(const dtPolyRef *polyPath, uint32 polyPathSize, const float* point, float *distance) const
{
if (!polyPath || !polyPathSize)
return INVALID_POLYREF;
dtPolyRef nearestPoly = INVALID_POLYREF;
float minDist2d = FLT_MAX;
float minDist3d = 0.0f;
for (uint32 i = 0; i < polyPathSize; ++i)
{
// skip DT_POLYTYPE_OFFMESH_CONNECTION they aren't handled in closestPointOnPoly
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
if (m_navMesh->getTileAndPolyByRef(polyPath[i], &tile, &poly) != DT_SUCCESS)
continue;
if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
float closestPoint[VERTEX_SIZE];
if (DT_SUCCESS != m_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint))
continue;
float d = dtVdist2DSqr(point, closestPoint);
if (d < minDist2d)
{
minDist2d = d;
nearestPoly = m_pathPolyRefs[i];
minDist3d = dtVdistSqr(point, closestPoint);
}
if(minDist2d < 1.0f) // shortcut out - close enough for us
break;
}
if (distance)
*distance = dtSqrt(minDist3d);
return (minDist2d < 3.0f) ? nearestPoly : INVALID_POLYREF;
}
/**
@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;
}
void dtCrowd::update(const float dt, dtCrowdAgentDebugInfo* debug)
{
m_velocitySampleCount = 0;
const int debugIdx = debug ? debug->idx : -1;
dtCrowdAgent** agents = m_activeAgents;
int nagents = getActiveAgents(agents, m_maxAgents);
// Check that all agents still have valid paths.
checkPathValidity(agents, nagents, dt);
// Update async move request and path finder.
updateMoveRequest(dt);
// Optimize path topology.
updateTopologyOptimization(agents, nagents, dt);
// Register agents to proximity grid.
m_grid->clear();
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const float* p = ag->npos;
const float r = ag->params.radius;
m_grid->addItem((unsigned short)i, p[0]-r, p[2]-r, p[0]+r, p[2]+r);
}
// Get nearby navmesh segments and agents to collide with.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->params.collisionQueryRange*0.25f;
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
!ag->boundary.isValid(m_navquery, &m_filter))
{
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
m_navquery, &m_filter);
}
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
agents, nagents, m_grid);
for (int j = 0; j < ag->nneis; j++)
ag->neis[j].idx = getAgentIndex(agents[ag->neis[j].idx]);
}
// Find next corner to steer to.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Find corners for steering
ag->ncorners = ag->corridor.findCorners(ag->cornerVerts, ag->cornerFlags, ag->cornerPolys,
DT_CROWDAGENT_MAX_CORNERS, m_navquery, &m_filter);
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.
if ((ag->params.updateFlags & DT_CROWD_OPTIMIZE_VIS) && ag->ncorners > 0)
{
const float* target = &ag->cornerVerts[dtMin(1,ag->ncorners-1)*3];
ag->corridor.optimizePathVisibility(target, ag->params.pathOptimizationRange, m_navquery, &m_filter);
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVcopy(debug->optStart, ag->corridor.getPos());
dtVcopy(debug->optEnd, target);
}
}
else
{
// Copy data for debug purposes.
if (debugIdx == i)
{
dtVset(debug->optStart, 0,0,0);
dtVset(debug->optEnd, 0,0,0);
}
}
}
// Trigger off-mesh connections (depends on corners).
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
// Check
const float triggerRadius = ag->params.radius*2.25f;
if (overOffmeshConnection(ag, triggerRadius))
{
// Prepare to off-mesh connection.
const int idx = (int)(ag - m_agents);
dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
// Adjust the path over the off-mesh connection.
dtPolyRef refs[2];
if (ag->corridor.moveOverOffmeshConnection(ag->cornerPolys[ag->ncorners-1], refs,
anim->startPos, anim->endPos, m_navquery))
{
dtVcopy(anim->initPos, ag->npos);
anim->polyRef = refs[1];
anim->active = 1;
anim->t = 0.0f;
anim->tmax = (dtVdist2D(anim->startPos, anim->endPos) / ag->params.maxSpeed) * 0.5f;
ag->state = DT_CROWDAGENT_STATE_OFFMESH;
ag->ncorners = 0;
ag->nneis = 0;
continue;
}
else
{
// Path validity check will ensure that bad/blocked connections will be replanned.
}
}
}
// Calculate steering.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE)
continue;
float dvel[3] = {0,0,0};
if (ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
dtVcopy(dvel, ag->targetPos);
ag->desiredSpeed = dtVlen(ag->targetPos);
}
else
{
// Calculate steering direction.
if (ag->params.updateFlags & DT_CROWD_ANTICIPATE_TURNS)
calcSmoothSteerDirection(ag, dvel);
else
calcStraightSteerDirection(ag, dvel);
// Calculate speed scale, which tells the agent to slowdown at the end of the path.
const float slowDownRadius = ag->params.radius*2; // TODO: make less hacky.
const float speedScale = getDistanceToGoal(ag, slowDownRadius) / slowDownRadius;
ag->desiredSpeed = ag->params.maxSpeed;
dtVscale(dvel, dvel, ag->desiredSpeed * speedScale);
}
// Separation
if (ag->params.updateFlags & DT_CROWD_SEPARATION)
{
const float separationDist = ag->params.collisionQueryRange;
const float invSeparationDist = 1.0f / separationDist;
const float separationWeight = ag->params.separationWeight;
float w = 0;
float disp[3] = {0,0,0};
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
const float distSqr = dtVlenSqr(diff);
if (distSqr < 0.00001f)
continue;
if (distSqr > dtSqr(separationDist))
continue;
const float dist = dtMathSqrtf(distSqr);
const float weight = separationWeight * (1.0f - dtSqr(dist*invSeparationDist));
dtVmad(disp, disp, diff, weight/dist);
w += 1.0f;
}
if (w > 0.0001f)
{
// Adjust desired velocity.
dtVmad(dvel, dvel, disp, 1.0f/w);
// Clamp desired velocity to desired speed.
const float speedSqr = dtVlenSqr(dvel);
const float desiredSqr = dtSqr(ag->desiredSpeed);
if (speedSqr > desiredSqr)
dtVscale(dvel, dvel, desiredSqr/speedSqr);
}
}
// Set the desired velocity.
dtVcopy(ag->dvel, dvel);
}
// Velocity planning.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
if (ag->params.updateFlags & DT_CROWD_OBSTACLE_AVOIDANCE)
{
m_obstacleQuery->reset();
// Add neighbours as obstacles.
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
m_obstacleQuery->addCircle(nei->npos, nei->params.radius, nei->vel, nei->dvel);
}
// Append neighbour segments as obstacles.
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
if (dtTriArea2D(ag->npos, s, s+3) < 0.0f)
continue;
m_obstacleQuery->addSegment(s, s+3);
}
dtObstacleAvoidanceDebugData* vod = 0;
if (debugIdx == i)
vod = debug->vod;
// Sample new safe velocity.
bool adaptive = true;
int ns = 0;
const dtObstacleAvoidanceParams* params = &m_obstacleQueryParams[ag->params.obstacleAvoidanceType];
if (adaptive)
{
ns = m_obstacleQuery->sampleVelocityAdaptive(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
else
{
ns = m_obstacleQuery->sampleVelocityGrid(ag->npos, ag->params.radius, ag->desiredSpeed,
ag->vel, ag->dvel, ag->nvel, params, vod);
}
m_velocitySampleCount += ns;
}
else
{
// If not using velocity planning, new velocity is directly the desired velocity.
dtVcopy(ag->nvel, ag->dvel);
}
}
// Integrate.
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (int iter = 0; iter < 4; ++iter)
{
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVset(ag->disp, 0,0,0);
float w = 0;
for (int j = 0; j < ag->nneis; ++j)
{
const dtCrowdAgent* nei = &m_agents[ag->neis[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->npos, nei->npos);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->params.radius + nei->params.radius))
continue;
dist = dtMathSqrtf(dist);
float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f)
{
// Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->dvel[2],0,ag->dvel[0]);
else
dtVset(diff, ag->dvel[2],0,-ag->dvel[0]);
pen = 0.01f;
}
else
{
pen = (1.0f/dist) * (pen*0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(ag->disp, ag->disp, diff, pen);
w += 1.0f;
}
if (w > 0.0001f)
{
const float iw = 1.0f / w;
dtVscale(ag->disp, ag->disp, iw);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVadd(ag->npos, ag->npos, ag->disp);
}
}
for (int i = 0; i < nagents; ++i)
{
dtCrowdAgent* ag = agents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Move along navmesh.
ag->corridor.movePosition(ag->npos, m_navquery, &m_filter);
// Get valid constrained position back.
dtVcopy(ag->npos, ag->corridor.getPos());
// If not using path, truncate the corridor to just one poly.
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
{
ag->corridor.reset(ag->corridor.getFirstPoly(), ag->npos);
}
}
// Update agents using off-mesh connection.
for (int i = 0; i < m_maxAgents; ++i)
{
dtCrowdAgentAnimation* anim = &m_agentAnims[i];
if (!anim->active)
continue;
dtCrowdAgent* ag = agents[i];
anim->t += dt;
if (anim->t > anim->tmax)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
}
// Update position
const float ta = anim->tmax*0.15f;
const float tb = anim->tmax;
if (anim->t < ta)
{
const float u = tween(anim->t, 0.0, ta);
dtVlerp(ag->npos, anim->initPos, anim->startPos, u);
}
else
{
const float u = tween(anim->t, ta, tb);
dtVlerp(ag->npos, anim->startPos, anim->endPos, u);
}
// Update velocity.
dtVset(ag->vel, 0,0,0);
dtVset(ag->dvel, 0,0,0);
}
}
void dtCrowd::updateStepProximityData(const float dt, dtCrowdAgentDebugInfo*)
{
// Register agents to proximity grid.
m_grid->clear();
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const float* p = ag->npos;
const float r = ag->params.radius;
m_grid->addItem((unsigned short)i, p[0] - r, p[2] - r, p[0] + r, p[2] + r);
}
// Get nearby navmesh segments and agents to collide with.
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
m_navquery->updateLinkFilter(ag->params.linkFilter);
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
const float updateThr = ag->params.collisionQueryRange*0.25f;
if (dtVdist2DSqr(ag->npos, ag->boundary.getCenter()) > dtSqr(updateThr) ||
!ag->boundary.isValid(m_navquery, &m_filters[ag->params.filter]))
{
// UE4: force removing segments too close to offmesh links
const bool useForcedRemove = m_linkRemovalRadius > 0.0f && ag->ncorners &&
(ag->cornerFlags[ag->ncorners - 1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
const float* removePos = useForcedRemove ? &ag->cornerVerts[(ag->ncorners - 1) * 3] : 0;
const float removeRadius = m_linkRemovalRadius;
// UE4: prepare filter containing only current area
// boundary update will take all corner polys and include them in local neighborhood
unsigned char allowedArea = DT_WALKABLE_AREA;
if (m_raycastSingleArea)
{
m_navquery->getAttachedNavMesh()->getPolyArea(ag->corridor.getFirstPoly(), &allowedArea);
m_raycastFilter.setAreaCost(allowedArea, 1.0f);
}
// UE4: move dir for segment scoring
float moveDir[3] = { 0.0f };
if (ag->ncorners)
{
dtVsub(moveDir, &ag->cornerVerts[2], &ag->cornerVerts[0]);
}
else
{
dtVcopy(moveDir, ag->vel);
}
dtVnormalize(moveDir);
ag->boundary.update(ag->corridor.getFirstPoly(), ag->npos, ag->params.collisionQueryRange,
removePos, removeRadius, useForcedRemove,
ag->corridor.getPath(), ag->corridor.getPathCount(),
moveDir,
m_navquery, m_raycastSingleArea ? &m_raycastFilter : &m_filters[ag->params.filter]);
m_raycastFilter.setAreaCost(allowedArea, DT_UNWALKABLE_POLY_COST);
}
// Query neighbour agents
ag->nneis = getNeighbours(ag->npos, ag->params.height, ag->params.collisionQueryRange,
ag, ag->neis, DT_CROWDAGENT_MAX_NEIGHBOURS,
m_activeAgents, m_numActiveAgents, m_grid);
for (int j = 0; j < ag->nneis; j++)
ag->neis[j].idx = getAgentIndex(m_activeAgents[ag->neis[j].idx]);
}
}
/**
@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,
float radius)
{
dtAssert(m_path);
dtAssert(m_npath);
static const float MIN_TARGET_DIST = 0.01f;
dtQueryResult result;
navquery->findStraightPath(m_pos, m_target, m_path, m_npath, result);
int ncorners = dtMin(result.size(), maxCorners);
result.copyRefs(cornerPolys, maxCorners);
result.copyFlags(cornerFlags, maxCorners);
result.copyPos(cornerVerts, maxCorners);
// 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;
}
}
// [UE4] Offset path points from corners
const dtNavMesh* nav = navquery->getAttachedNavMesh();
float v1[3], v2[3], dir[3];
for (int i = 0; i < ncorners - 1; i++)
{
int fromIdx = 0;
while (m_path[fromIdx] != cornerPolys[i] && fromIdx < m_npath)
{
fromIdx++;
}
if (m_path[fromIdx] != cornerPolys[i] || fromIdx == 0)
continue;
const dtMeshTile* tile0 = 0;
const dtMeshTile* tile1 = 0;
const dtPoly* poly0 = 0;
const dtPoly* poly1 = 0;
nav->getTileAndPolyByRefUnsafe(m_path[fromIdx - 1], &tile0, &poly0);
nav->getTileAndPolyByRefUnsafe(m_path[fromIdx], &tile1, &poly1);
if (tile0 != tile1)
continue;
float* corner = &cornerVerts[i * 3];
unsigned char dummyT1, dummyT2;
navquery->getPortalPoints(m_path[fromIdx - 1], m_path[fromIdx], v1, v2, dummyT1, dummyT2);
const float edgeLen = dtVdist(v1, v2);
if (edgeLen > 0.001f)
{
const float edgeOffset = dtMin(radius, edgeLen * 0.75f) / edgeLen;
if (dtVequal(corner, v1))
{
dtVsub(dir, v2, v1);
dtVmad(corner, corner, dir, edgeOffset);
}
else
{
dtVsub(dir, v1, v2);
dtVmad(corner, corner, dir, edgeOffset);
}
}
}
return ncorners;
}