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;
}
void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip,
const float* pos, float* closest) const
{
const dtPoly* poly = &tile->polys[ip];
float closestDistSqr = FLT_MAX;
const dtPolyDetail* pd = &tile->detailMeshes[ip];
for (int j = 0; j < pd->triCount; ++j)
{
const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
const float* v[3];
for (int k = 0; k < 3; ++k)
{
if (t[k] < poly->vertCount)
v[k] = &tile->verts[poly->verts[t[k]]*3];
else
v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
}
float pt[3];
dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]);
float d = dtVdistSqr(pos, pt);
if (d < closestDistSqr)
{
dtVcopy(closest, pt);
closestDistSqr = d;
}
}
}
dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
const float* center, const float* extents,
float* nearestPt) const
{
float bmin[3], bmax[3];
dtVsub(bmin, center, extents);
dtVadd(bmax, center, extents);
// Get nearby polygons from proximity grid.
dtPolyRef polys[128];
int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128);
// Find nearest polygon amongst the nearby polygons.
dtPolyRef nearest = 0;
float nearestDistanceSqr = FLT_MAX;
for (int i = 0; i < polyCount; ++i)
{
dtPolyRef ref = polys[i];
float closestPtPoly[3];
closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly);
float d = dtVdistSqr(center, closestPtPoly);
if (d < nearestDistanceSqr)
{
if (nearestPt)
dtVcopy(nearestPt, closestPtPoly);
nearestDistanceSqr = d;
nearest = ref;
}
}
return nearest;
}
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 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;
}
void dtCrowd::updateAgentState(const int idx, bool repath)
{
if (idx >= 0 && idx < m_maxAgents)
{
dtCrowdAgent* ag = &m_agents[idx];
dtCrowdAgentAnimation* anim = &m_agentAnims[idx];
if (anim->active)
{
anim->active = 0;
if (m_keepOffmeshConnections)
{
const float distToStartSq = dtVdistSqr(ag->npos, anim->startPos);
const float distToEndSq = dtVdistSqr(ag->npos, anim->endPos);
if (distToEndSq < distToStartSq)
{
ag->corridor.pruneOffmeshConenction(anim->polyRef);
}
}
}
if (ag->active)
{
if (repath)
{
// switch to invalid state and force update in next tick
ag->state = DT_CROWDAGENT_STATE_INVALID;
ag->targetReplanTime = m_agentStateCheckInterval;
}
else
{
ag->state = DT_CROWDAGENT_STATE_WALKING;
}
}
}
}
void dtCrowd::updateStepOffMeshVelocity(const float dt, dtCrowdAgentDebugInfo*)
{
// UE4 version of offmesh anims, updates velocity and checks distance instead of fixed time
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
const int agentIndex = getAgentIndex(ag);
dtCrowdAgentAnimation* anim = &m_agentAnims[agentIndex];
if (!anim->active)
continue;
anim->t += dt;
const float dist = dtVdistSqr(ag->npos, anim->endPos);
const float distThres = dtSqr(5.0f);
if (dist < distThres)
{
// Reset animation
anim->active = 0;
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
// UE4: m_keepOffmeshConnections support
if (m_keepOffmeshConnections)
{
ag->corridor.pruneOffmeshConenction(anim->polyRef);
}
}
if (ag->state == DT_CROWDAGENT_STATE_OFFMESH)
{
float dir[3] = { 0 };
dtVsub(dir, anim->endPos, anim->initPos);
dir[1] = 0.0f;
dtVnormalize(dir);
dtVscale(ag->nvel, dir, ag->params.maxSpeed);
dtVcopy(ag->vel, ag->nvel);
dtVset(ag->dvel, 0, 0, 0);
}
}
}
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;
}