bool PathInfo::getSteerTarget(const float* startPos, const float* endPos, const float minTargetDist, const dtPolyRef* path, const uint32 pathSize, float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef)
{
// Find steer target.
static const uint32 MAX_STEER_POINTS = 3;
float steerPath[MAX_STEER_POINTS*VERTEX_SIZE];
unsigned char steerPathFlags[MAX_STEER_POINTS];
dtPolyRef steerPathPolys[MAX_STEER_POINTS];
uint32 nsteerPath = 0;
dtStatus dtResult = m_navMeshQuery->findStraightPath(startPos, endPos, path, pathSize, steerPath, steerPathFlags, steerPathPolys, (int*)&nsteerPath, MAX_STEER_POINTS);
if (!nsteerPath || DT_SUCCESS != dtResult)
return false;
// Find vertex far enough to steer to.
uint32 ns = 0;
while (ns < nsteerPath)
{
// Stop at Off-Mesh link or when point is further than slop away.
if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) || !inRangeYZX(&steerPath[ns*VERTEX_SIZE], startPos, minTargetDist, 1000.0f))
break;
ns++;
}
// Failed to find good point to steer to.
if (ns >= nsteerPath)
return false;
dtVcopy(steerPos, &steerPath[ns*VERTEX_SIZE]);
steerPos[1] = startPos[1]; // keep Z value
steerPosFlag = steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return true;
}
dtStatus PathInfo::findSmoothPath(const float* startPos, const float* endPos,
const dtPolyRef* polyPath, const uint32 polyPathSize,
float* smoothPath, int* smoothPathSize, bool &usedOffmesh, const uint32 maxSmoothPathSize)
{
MANGOS_ASSERT(polyPathSize <= MAX_PATH_LENGTH);
*smoothPathSize = 0;
uint32 nsmoothPath = 0;
usedOffmesh = false;
dtPolyRef polys[MAX_PATH_LENGTH];
memcpy(polys, polyPath, sizeof(dtPolyRef)*polyPathSize);
uint32 npolys = polyPathSize;
float iterPos[VERTEX_SIZE], targetPos[VERTEX_SIZE];
if(DT_SUCCESS != m_navMeshQuery->closestPointOnPolyBoundary(polys[0], startPos, iterPos))
return DT_FAILURE;
if(DT_SUCCESS != m_navMeshQuery->closestPointOnPolyBoundary(polys[npolys-1], endPos, targetPos))
return DT_FAILURE;
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos);
nsmoothPath++;
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
while (npolys && nsmoothPath < maxSmoothPathSize)
{
// Find location to steer towards.
float steerPos[VERTEX_SIZE];
unsigned char steerPosFlag;
dtPolyRef steerPosRef = INVALID_POLYREF;
if (!getSteerTarget(iterPos, targetPos, SMOOTH_PATH_SLOP, polys, npolys, steerPos, steerPosFlag, steerPosRef))
break;
bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END);
bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
// Find movement delta.
float delta[VERTEX_SIZE];
dtVsub(delta, steerPos, iterPos);
float len = dtSqrt(dtVdot(delta,delta));
// If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < SMOOTH_PATH_STEP_SIZE)
len = 1.0f;
else
len = SMOOTH_PATH_STEP_SIZE / len;
float moveTgt[VERTEX_SIZE];
dtVmad(moveTgt, iterPos, delta, len);
// Move
float result[VERTEX_SIZE];
const static uint32 MAX_VISIT_POLY = 16;
dtPolyRef visited[MAX_VISIT_POLY];
uint32 nvisited = 0;
m_navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter, result, visited, (int*)&nvisited, MAX_VISIT_POLY);
npolys = fixupCorridor(polys, npolys, MAX_PATH_LENGTH, visited, nvisited);
m_navMeshQuery->getPolyHeight(polys[0], result, &result[1]);
dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 2.0f))
{
// Reached end of path.
dtVcopy(iterPos, targetPos);
if (nsmoothPath < maxSmoothPathSize)
{
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos);
nsmoothPath++;
}
break;
}
else if (offMeshConnection && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 2.0f))
{
// Reached off-mesh connection.
usedOffmesh = true;
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = INVALID_POLYREF;
dtPolyRef polyRef = polys[0];
uint32 npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (uint32 i = npos; i < npolys; ++i)
polys[i-npos] = polys[i];
npolys -= npos;
// Handle the connection.
float startPos[VERTEX_SIZE], endPos[VERTEX_SIZE];
if (DT_SUCCESS == m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos))
{
if (nsmoothPath < maxSmoothPathSize)
{
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], startPos);
nsmoothPath++;
}
// Move position at the other side of the off-mesh link.
dtVcopy(iterPos, endPos);
m_navMeshQuery->getPolyHeight(polys[0], iterPos, &iterPos[1]);
}
}
// Store results.
if (nsmoothPath < maxSmoothPathSize)
{
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos);
nsmoothPath++;
}
}
*smoothPathSize = nsmoothPath;
// this is most likely loop
return nsmoothPath < maxSmoothPathSize ? DT_SUCCESS : DT_FAILURE;
}
