/**
@par
Inaccurate locomotion or dynamic obstacle avoidance can force the argent position significantly outside the
original corridor. Over time this can result in the formation of a non-optimal corridor. Non-optimal paths can
also form near the corners of tiles.
This function uses an efficient local visibility search to try to optimize the corridor
between the current position and @p next.
The corridor will change only if @p next is visible from the current position and moving directly toward the point
is better than following the existing path.
The more inaccurate the agent movement, the more beneficial this function becomes. Simply adjust the frequency
of the call to match the needs to the agent.
This function is not suitable for long distance searches.
*/
void dtPathCorridor::optimizePathVisibility(const float* next, const float pathOptimizationRange,
dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
dtAssert(m_path);
// Clamp the ray to max distance.
float goal[3];
dtVcopy(goal, next);
float dist = dtVdist2D(m_pos, goal);
// If too close to the goal, do not try to optimize.
if (dist < 0.01f)
return;
// Overshoot a little. This helps to optimize open fields in tiled meshes.
dist = dtMin(dist+0.01f, pathOptimizationRange);
// Adjust ray length.
float delta[3];
dtVsub(delta, goal, m_pos);
dtVmad(goal, m_pos, delta, pathOptimizationRange/dist);
static const int MAX_RES = 32;
dtPolyRef res[MAX_RES];
float t, norm[3];
int nres = 0;
navquery->raycast(m_path[0], m_pos, goal, filter, &t, norm, res, &nres, MAX_RES);
if (nres > 1 && t > 0.99f)
{
m_npath = dtMergeCorridorStartShortcut(m_path, m_npath, m_maxPath, res, nres);
}
}
static void integrate(dtCrowdAgent* ag, const float dt)
{
if (dtVlen(ag->velocity) > EPSILON)
dtVmad(ag->position, ag->position, ag->velocity, dt);
else
dtVset(ag->velocity,0,0,0);
}
static void integrate(dtCrowdAgent* ag, const float dt)
{
// Fake dynamic constraint.
const float maxDelta = ag->params.maxAcceleration * dt;
float dv[3];
dtVsub(dv, ag->nvel, ag->vel);
float ds = dtVlen(dv);
if (ds > maxDelta)
dtVscale(dv, dv, maxDelta/ds);
dtVadd(ag->vel, ag->vel, dv);
// Integrate
if (dtVlen(ag->vel) > 0.0001f)
dtVmad(ag->npos, ag->npos, ag->vel, dt);
else
dtVset(ag->vel,0,0,0);
}
float dtDistancePtSegSqr(const float* pt, const float* p, const float* q)
{
float seg[3], toPt[3], closest[3];
dtVsub(seg, q, p);
dtVsub(toPt, pt, p);
const float d1 = dtVdot(toPt, seg);
const float d2 = dtVdot(seg, seg);
if (d1 <= 0)
{
dtVcopy(closest, p);
}
else if (d2 <= d1)
{
dtVcopy(closest, q);
}
else
{
dtVmad(closest, p, seg, d1 / d2);
}
dtVsub(toPt, closest, pt);
return dtVlenSqr(toPt);
}
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;
}
bool TestCase::handleRenderOverlay(double* proj, double* model, int* view)
{
GLdouble x, y, z;
char text[64], subtext[64];
int n = 0;
static const float LABEL_DIST = 1.0f;
for (Test* iter = m_tests; iter; iter = iter->next)
{
float pt[3], dir[3];
if (iter->nstraight)
{
dtVcopy(pt, &iter->straight[3]);
if (dtVdist(pt, iter->spos) > LABEL_DIST)
{
dtVsub(dir, pt, iter->spos);
dtVnormalize(dir);
dtVmad(pt, iter->spos, dir, LABEL_DIST);
}
pt[1]+=0.5f;
}
else
{
dtVsub(dir, iter->epos, iter->spos);
dtVnormalize(dir);
dtVmad(pt, iter->spos, dir, LABEL_DIST);
pt[1]+=0.5f;
}
if (gluProject((GLdouble)pt[0], (GLdouble)pt[1], (GLdouble)pt[2],
model, proj, view, &x, &y, &z))
{
snprintf(text, 64, "Path %d\n", n);
unsigned int col = imguiRGBA(0,0,0,128);
if (iter->expand)
col = imguiRGBA(255,192,0,220);
imguiDrawText((int)x, (int)(y-25), IMGUI_ALIGN_CENTER, text, col);
}
n++;
}
static int resScroll = 0;
bool mouseOverMenu = imguiBeginScrollArea("Test Results", 10, view[3] - 10 - 350, 200, 350, &resScroll);
// mouseOverMenu = true;
n = 0;
for (Test* iter = m_tests; iter; iter = iter->next)
{
const int total = iter->findNearestPolyTime + iter->findPathTime + iter->findStraightPathTime;
snprintf(subtext, 64, "%.4f ms", (float)total/1000.0f);
snprintf(text, 64, "Path %d", n);
if (imguiCollapse(text, subtext, iter->expand))
iter->expand = !iter->expand;
if (iter->expand)
{
snprintf(text, 64, "Poly: %.4f ms", (float)iter->findNearestPolyTime/1000.0f);
imguiValue(text);
snprintf(text, 64, "Path: %.4f ms", (float)iter->findPathTime/1000.0f);
imguiValue(text);
snprintf(text, 64, "Straight: %.4f ms", (float)iter->findStraightPathTime/1000.0f);
imguiValue(text);
imguiSeparator();
}
n++;
}
imguiEndScrollArea();
return mouseOverMenu;
}
/// @par
///
/// The output data array is allocated using the detour allocator (dtAlloc()). The method
/// used to free the memory will be determined by how the tile is added to the navigation
/// mesh.
///
/// @see dtNavMesh, dtNavMesh::addTile()
bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData, int* outDataSize)
{
if (params->nvp > DT_VERTS_PER_POLYGON)
return false;
if (params->vertCount >= 0xffff)
return false;
if (!params->vertCount || !params->verts)
return false;
if (!params->polyCount || !params->polys)
return false;
const int nvp = params->nvp;
// Classify off-mesh connection points. We store only the connections
// whose start point is inside the tile.
unsigned char* offMeshConClass = 0;
int storedOffMeshConCount = 0;
int offMeshConLinkCount = 0;
int storedOffMeshSegCount = 0;
if (params->offMeshConCount > 0)
{
offMeshConClass = (unsigned char*)dtAlloc(sizeof(unsigned char)*params->offMeshConCount*2, DT_ALLOC_TEMP);
if (!offMeshConClass)
return false;
memset(offMeshConClass, 0, sizeof(unsigned char)*params->offMeshConCount*2);
// Find tight heigh bounds, used for culling out off-mesh start locations.
float hmin = FLT_MAX;
float hmax = -FLT_MAX;
for (int i = 0; i < params->vertCount; ++i)
{
const unsigned short* iv = ¶ms->verts[i*3];
const float h = params->bmin[1] + iv[1] * params->ch;
hmin = dtMin(hmin,h);
hmax = dtMax(hmax,h);
}
if (params->detailVerts && params->detailVertsCount)
{
for (int i = 0; i < params->detailVertsCount; ++i)
{
const float h = params->detailVerts[i*3+1];
hmin = dtMin(hmin,h);
hmax = dtMax(hmax,h);
}
}
hmin -= params->walkableClimb;
hmax += params->walkableClimb;
float bmin[3], bmax[3];
dtVcopy(bmin, params->bmin);
dtVcopy(bmax, params->bmax);
bmin[1] = hmin;
bmax[1] = hmax;
float bverts[3*4];
bverts[ 0] = bmin[0]; bverts[ 2] = bmin[2];
bverts[ 3] = bmax[0]; bverts[ 5] = bmin[2];
bverts[ 6] = bmax[0]; bverts[ 8] = bmax[2];
bverts[ 9] = bmin[0]; bverts[11] = bmax[2];
for (int i = 0; i < params->offMeshConCount; ++i)
{
const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i];
if (offMeshCon.type & DT_OFFMESH_CON_POINT)
{
offMeshConClass[i*2+0] = classifyOffMeshPoint(offMeshCon.vertsA0, bmin, bmax);
offMeshConClass[i*2+1] = classifyOffMeshPoint(offMeshCon.vertsB0, bmin, bmax);
// Zero out off-mesh start positions which are not even potentially touching the mesh.
if (offMeshConClass[i*2+0] == 0xff)
{
if ((offMeshCon.vertsA0[1] - offMeshCon.snapHeight) < bmin[1] &&
(offMeshCon.vertsA0[1] + offMeshCon.snapHeight) > bmax[1])
{
offMeshConClass[i * 2 + 0] = 0;
}
}
// Count how many links should be allocated for off-mesh connections.
if (offMeshConClass[i*2+0] == 0xff)
offMeshConLinkCount++;
if (offMeshConClass[i*2+1] == 0xff)
offMeshConLinkCount++;
if (offMeshConClass[i*2+0] == 0xff)
storedOffMeshConCount++;
}
else if (offMeshCon.type & DT_OFFMESH_CON_SEGMENT)
{
int smin, smax;
float tmin, tmax;
if ((offMeshCon.vertsA0[1] >= bmin[1] && offMeshCon.vertsA0[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsA0, bmin, bmax) == 0xff) ||
(offMeshCon.vertsA1[1] >= bmin[1] && offMeshCon.vertsA1[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsA1, bmin, bmax) == 0xff) ||
(offMeshCon.vertsB0[1] >= bmin[1] && offMeshCon.vertsB0[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsB0, bmin, bmax) == 0xff) ||
(offMeshCon.vertsB1[1] >= bmin[1] && offMeshCon.vertsB1[1] <= bmax[1] && classifyOffMeshPoint(offMeshCon.vertsB1, bmin, bmax) == 0xff) ||
dtIntersectSegmentPoly2D(offMeshCon.vertsA0, offMeshCon.vertsA1, bverts, 4, tmin, tmax, smin, smax) ||
dtIntersectSegmentPoly2D(offMeshCon.vertsB0, offMeshCon.vertsB1, bverts, 4, tmin, tmax, smin, smax))
{
offMeshConClass[i*2] = 0xff;
storedOffMeshSegCount++;
}
}
}
}
// Off-mesh connections are stored as polygons, adjust values.
const int firstSegVert = params->vertCount + storedOffMeshConCount*2;
const int firstSegPoly = params->polyCount + storedOffMeshConCount;
const int totPolyCount = firstSegPoly + storedOffMeshSegCount*DT_MAX_OFFMESH_SEGMENT_PARTS;
const int totVertCount = firstSegVert + storedOffMeshSegCount*DT_MAX_OFFMESH_SEGMENT_PARTS*4;
// Find portal edges which are at tile borders.
int edgeCount = 0;
int portalCount = 0;
for (int i = 0; i < params->polyCount; ++i)
{
const unsigned short* p = ¶ms->polys[i*2*nvp];
for (int j = 0; j < nvp; ++j)
{
if (p[j] == MESH_NULL_IDX) break;
edgeCount++;
if (p[nvp+j] & 0x8000)
{
unsigned short dir = p[nvp+j] & 0xf;
if (dir != 0xf)
portalCount++;
}
}
}
// offmesh links will be added in dynamic array
const int maxLinkCount = edgeCount + portalCount*2;
// Find unique detail vertices.
int uniqueDetailVertCount = 0;
int detailTriCount = 0;
if (params->detailMeshes)
{
// Has detail mesh, count unique detail vertex count and use input detail tri count.
detailTriCount = params->detailTriCount;
for (int i = 0; i < params->polyCount; ++i)
{
const unsigned short* p = ¶ms->polys[i*nvp*2];
int ndv = params->detailMeshes[i*4+1];
int nv = 0;
for (int j = 0; j < nvp; ++j)
{
if (p[j] == MESH_NULL_IDX) break;
nv++;
}
ndv -= nv;
uniqueDetailVertCount += ndv;
}
}
else
{
// No input detail mesh, build detail mesh from nav polys.
uniqueDetailVertCount = 0; // No extra detail verts.
detailTriCount = 0;
for (int i = 0; i < params->polyCount; ++i)
{
const unsigned short* p = ¶ms->polys[i*nvp*2];
int nv = 0;
for (int j = 0; j < nvp; ++j)
{
if (p[j] == MESH_NULL_IDX) break;
nv++;
}
detailTriCount += nv-2;
}
}
// Calculate data size
const int headerSize = dtAlign4(sizeof(dtMeshHeader));
const int vertsSize = dtAlign4(sizeof(float)*3*totVertCount);
const int polysSize = dtAlign4(sizeof(dtPoly)*totPolyCount);
const int linksSize = dtAlign4(sizeof(dtLink)*maxLinkCount);
const int detailMeshesSize = dtAlign4(sizeof(dtPolyDetail)*params->polyCount);
const int detailVertsSize = dtAlign4(sizeof(float)*3*uniqueDetailVertCount);
const int detailTrisSize = dtAlign4(sizeof(unsigned char)*4*detailTriCount);
const int bvTreeSize = params->buildBvTree ? dtAlign4(sizeof(dtBVNode)*params->polyCount*2) : 0;
const int offMeshConsSize = dtAlign4(sizeof(dtOffMeshConnection)*storedOffMeshConCount);
const int offMeshSegSize = dtAlign4(sizeof(dtOffMeshSegmentConnection)*storedOffMeshSegCount);
const int clustersSize = dtAlign4(sizeof(dtCluster)*params->clusterCount);
const int polyClustersSize = dtAlign4(sizeof(unsigned short)*params->polyCount);
const int dataSize = headerSize + vertsSize + polysSize + linksSize +
detailMeshesSize + detailVertsSize + detailTrisSize +
bvTreeSize + offMeshConsSize + offMeshSegSize +
clustersSize + polyClustersSize;
unsigned char* data = (unsigned char*)dtAlloc(sizeof(unsigned char)*dataSize, DT_ALLOC_PERM);
if (!data)
{
dtFree(offMeshConClass);
return false;
}
memset(data, 0, dataSize);
unsigned char* d = data;
dtMeshHeader* header = (dtMeshHeader*)d; d += headerSize;
float* navVerts = (float*)d; d += vertsSize;
dtPoly* navPolys = (dtPoly*)d; d += polysSize;
d += linksSize;
dtPolyDetail* navDMeshes = (dtPolyDetail*)d; d += detailMeshesSize;
float* navDVerts = (float*)d; d += detailVertsSize;
unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize;
dtBVNode* navBvtree = (dtBVNode*)d; d += bvTreeSize;
dtOffMeshConnection* offMeshCons = (dtOffMeshConnection*)d; d += offMeshConsSize;
dtOffMeshSegmentConnection* offMeshSegs = (dtOffMeshSegmentConnection*)d; d += offMeshSegSize;
dtCluster* clusters = (dtCluster*)d; d += clustersSize;
unsigned short* polyClusters = (unsigned short*)d; d += polyClustersSize;
// Store header
header->magic = DT_NAVMESH_MAGIC;
header->version = DT_NAVMESH_VERSION;
header->x = params->tileX;
header->y = params->tileY;
header->layer = params->tileLayer;
header->userId = params->userId;
header->polyCount = totPolyCount;
header->vertCount = totVertCount;
header->maxLinkCount = maxLinkCount;
dtVcopy(header->bmin, params->bmin);
dtVcopy(header->bmax, params->bmax);
header->detailMeshCount = params->polyCount;
header->detailVertCount = uniqueDetailVertCount;
header->detailTriCount = detailTriCount;
header->bvQuantFactor = 1.0f / params->cs;
header->offMeshBase = params->polyCount;
header->offMeshSegPolyBase = firstSegPoly;
header->offMeshSegVertBase = firstSegVert;
header->walkableHeight = params->walkableHeight;
header->walkableRadius = params->walkableRadius;
header->walkableClimb = params->walkableClimb;
header->offMeshConCount = storedOffMeshConCount;
header->offMeshSegConCount = storedOffMeshSegCount;
header->bvNodeCount = params->buildBvTree ? params->polyCount*2 : 0;
header->clusterCount = params->clusterCount;
const int offMeshVertsBase = params->vertCount;
const int offMeshPolyBase = params->polyCount;
// Store vertices
// Mesh vertices
for (int i = 0; i < params->vertCount; ++i)
{
const unsigned short* iv = ¶ms->verts[i*3];
float* v = &navVerts[i*3];
v[0] = params->bmin[0] + iv[0] * params->cs;
v[1] = params->bmin[1] + iv[1] * params->ch;
v[2] = params->bmin[2] + iv[2] * params->cs;
}
// Off-mesh point link vertices.
int n = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i];
// Only store connections which start from this tile.
if ((offMeshConClass[i*2+0] == 0xff) && (offMeshCon.type & DT_OFFMESH_CON_POINT))
{
float* v = &navVerts[(offMeshVertsBase + n*2)*3];
dtVcopy(&v[0], &offMeshCon.vertsA0[0]);
dtVcopy(&v[3], &offMeshCon.vertsB0[0]);
n++;
}
}
// Store polygons
// Mesh polys
const unsigned short* src = params->polys;
for (int i = 0; i < params->polyCount; ++i)
{
dtPoly* p = &navPolys[i];
p->vertCount = 0;
p->flags = params->polyFlags[i];
p->setArea(params->polyAreas[i]);
p->setType(DT_POLYTYPE_GROUND);
for (int j = 0; j < nvp; ++j)
{
if (src[j] == MESH_NULL_IDX) break;
p->verts[j] = src[j];
if (src[nvp+j] & 0x8000)
{
// Border or portal edge.
unsigned short dir = src[nvp+j] & 0xf;
if (dir == 0xf) // Border
p->neis[j] = 0;
else if (dir == 0) // Portal x-
p->neis[j] = DT_EXT_LINK | 4;
else if (dir == 1) // Portal z+
p->neis[j] = DT_EXT_LINK | 2;
else if (dir == 2) // Portal x+
p->neis[j] = DT_EXT_LINK | 0;
else if (dir == 3) // Portal z-
p->neis[j] = DT_EXT_LINK | 6;
}
else
{
// Normal connection
p->neis[j] = src[nvp+j]+1;
}
p->vertCount++;
}
src += nvp*2;
}
// Off-mesh point connection polygons.
n = 0;
int nseg = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i];
// Only store connections which start from this tile.
if (offMeshConClass[i*2+0] == 0xff)
{
if (offMeshCon.type & DT_OFFMESH_CON_POINT)
{
dtPoly* p = &navPolys[offMeshPolyBase+n];
p->vertCount = 2;
p->verts[0] = (unsigned short)(offMeshVertsBase + n*2+0);
p->verts[1] = (unsigned short)(offMeshVertsBase + n*2+1);
p->flags = offMeshCon.polyFlag;
p->setArea(offMeshCon.area);
p->setType(DT_POLYTYPE_OFFMESH_POINT);
n++;
}
else
{
for (int j = 0; j < DT_MAX_OFFMESH_SEGMENT_PARTS; j++)
{
dtPoly* p = &navPolys[firstSegPoly+nseg];
p->vertCount = 0;
p->flags = offMeshCon.polyFlag;
p->setArea(offMeshCon.area);
p->setType(DT_POLYTYPE_OFFMESH_SEGMENT);
nseg++;
}
}
}
}
// Store detail meshes and vertices.
// The nav polygon vertices are stored as the first vertices on each mesh.
// We compress the mesh data by skipping them and using the navmesh coordinates.
if (params->detailMeshes)
{
unsigned short vbase = 0;
for (int i = 0; i < params->polyCount; ++i)
{
dtPolyDetail& dtl = navDMeshes[i];
const int vb = (int)params->detailMeshes[i*4+0];
const int ndv = (int)params->detailMeshes[i*4+1];
const int nv = navPolys[i].vertCount;
dtl.vertBase = (unsigned int)vbase;
dtl.vertCount = (unsigned char)(ndv-nv);
dtl.triBase = (unsigned int)params->detailMeshes[i*4+2];
dtl.triCount = (unsigned char)params->detailMeshes[i*4+3];
// Copy vertices except the first 'nv' verts which are equal to nav poly verts.
if (ndv-nv)
{
memcpy(&navDVerts[vbase*3], ¶ms->detailVerts[(vb+nv)*3], sizeof(float)*3*(ndv-nv));
vbase += (unsigned short)(ndv-nv);
}
}
// Store triangles.
memcpy(navDTris, params->detailTris, sizeof(unsigned char)*4*params->detailTriCount);
}
else
{
// Create dummy detail mesh by triangulating polys.
int tbase = 0;
for (int i = 0; i < params->polyCount; ++i)
{
dtPolyDetail& dtl = navDMeshes[i];
const int nv = navPolys[i].vertCount;
dtl.vertBase = 0;
dtl.vertCount = 0;
dtl.triBase = (unsigned int)tbase;
dtl.triCount = (unsigned char)(nv-2);
// Triangulate polygon (local indices).
for (int j = 2; j < nv; ++j)
{
unsigned char* t = &navDTris[tbase*4];
t[0] = 0;
t[1] = (unsigned char)(j-1);
t[2] = (unsigned char)j;
// Bit for each edge that belongs to poly boundary.
t[3] = (1<<2);
if (j == 2) t[3] |= (1<<0);
if (j == nv-1) t[3] |= (1<<4);
tbase++;
}
}
}
// Store and create BVtree.
if (params->buildBvTree)
{
createBVTree(params->verts, params->vertCount, params->polys, params->polyCount, nvp,
navDMeshes, navDVerts, navDTris, params->bmin,
params->cs, params->ch, params->polyCount*2, navBvtree);
}
// Store Off-Mesh connections.
n = 0;
nseg = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
const dtOffMeshLinkCreateParams& offMeshCon = params->offMeshCons[i];
// Only store connections which start from this tile.
if (offMeshConClass[i*2+0] == 0xff)
{
if (offMeshCon.type & DT_OFFMESH_CON_POINT)
{
dtOffMeshConnection* con = &offMeshCons[n];
con->poly = (unsigned short)(offMeshPolyBase + n);
// Copy connection end-points.
dtVcopy(&con->pos[0], &offMeshCon.vertsA0[0]);
dtVcopy(&con->pos[3], &offMeshCon.vertsB0[0]);
con->rad = offMeshCon.snapRadius;
con->height = offMeshCon.snapHeight;
con->setFlags(offMeshCon.type);
con->side = offMeshConClass[i*2+1] == 0xff ? DT_CONNECTION_INTERNAL : offMeshConClass[i*2+1];
if (offMeshCon.userID)
con->userId = offMeshCon.userID;
n++;
}
else
{
dtOffMeshSegmentConnection* con = &offMeshSegs[nseg];
dtVcopy(con->startA, &offMeshCon.vertsA0[0]);
dtVcopy(con->endA, &offMeshCon.vertsA1[0]);
dtVcopy(con->startB, &offMeshCon.vertsB0[0]);
dtVcopy(con->endB, &offMeshCon.vertsB1[0]);
con->rad = offMeshCon.snapRadius;
con->height = offMeshCon.snapHeight;
con->setFlags(offMeshCon.type);
if (offMeshCon.userID)
con->userId = offMeshCon.userID;
nseg++;
}
}
}
dtFree(offMeshConClass);
// Store clusters
if (params->polyClusters)
{
memcpy(polyClusters, params->polyClusters, sizeof(unsigned short)*params->polyCount);
}
for (int i = 0; i < params->clusterCount; i++)
{
dtCluster& cluster = clusters[i];
cluster.firstLink = DT_NULL_LINK;
cluster.numLinks = 0;
dtVset(cluster.center, 0.f, 0.f, 0.f);
// calculate center point: take from first poly
for (int j = 0; j < params->polyCount; j++)
{
if (polyClusters[j] != i)
{
continue;
}
const dtPoly* poly = &navPolys[j];
float c[3] = { 0.0f, 0.0f, 0.0f };
for (int iv = 0; iv < poly->vertCount; iv++)
{
dtVadd(c, c, &navVerts[poly->verts[iv] * 3]);
}
dtVmad(cluster.center, cluster.center, c, 1.0f / poly->vertCount);
break;
}
}
*outData = data;
*outDataSize = dataSize;
return true;
}
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 NavMeshTesterTool::handleRender()
{
DebugDrawGL dd;
static const unsigned int startCol = duRGBA(128,25,0,192);
static const unsigned int endCol = duRGBA(51,102,0,129);
static const unsigned int pathCol = duRGBA(0,0,0,64);
const float agentRadius = m_sample->getAgentRadius();
const float agentHeight = m_sample->getAgentHeight();
const float agentClimb = m_sample->getAgentClimb();
dd.depthMask(false);
if (m_sposSet)
drawAgent(m_spos, agentRadius, agentHeight, agentClimb, startCol);
if (m_eposSet)
drawAgent(m_epos, agentRadius, agentHeight, agentClimb, endCol);
dd.depthMask(true);
if (!m_navMesh)
{
return;
}
if (m_toolMode == TOOLMODE_PATHFIND_FOLLOW)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol);
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_endRef, endCol);
if (m_npolys)
{
for (int i = 1; i < m_npolys-1; ++i)
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
}
if (m_nsmoothPath)
{
dd.depthMask(false);
const unsigned int pathCol = duRGBA(0,0,0,220);
dd.begin(DU_DRAW_LINES, 3.0f);
for (int i = 0; i < m_nsmoothPath; ++i)
dd.vertex(m_smoothPath[i*3], m_smoothPath[i*3+1]+0.1f, m_smoothPath[i*3+2], pathCol);
dd.end();
dd.depthMask(true);
}
if (m_pathIterNum)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_pathIterPolys[0], duRGBA(255,255,255,128));
dd.depthMask(false);
dd.begin(DU_DRAW_LINES, 1.0f);
const unsigned int prevCol = duRGBA(255,192,0,220);
const unsigned int curCol = duRGBA(255,255,255,220);
const unsigned int steerCol = duRGBA(0,192,255,220);
dd.vertex(m_prevIterPos[0],m_prevIterPos[1]-0.3f,m_prevIterPos[2], prevCol);
dd.vertex(m_prevIterPos[0],m_prevIterPos[1]+0.3f,m_prevIterPos[2], prevCol);
dd.vertex(m_iterPos[0],m_iterPos[1]-0.3f,m_iterPos[2], curCol);
dd.vertex(m_iterPos[0],m_iterPos[1]+0.3f,m_iterPos[2], curCol);
dd.vertex(m_prevIterPos[0],m_prevIterPos[1]+0.3f,m_prevIterPos[2], prevCol);
dd.vertex(m_iterPos[0],m_iterPos[1]+0.3f,m_iterPos[2], prevCol);
dd.vertex(m_prevIterPos[0],m_prevIterPos[1]+0.3f,m_prevIterPos[2], steerCol);
dd.vertex(m_steerPos[0],m_steerPos[1]+0.3f,m_steerPos[2], steerCol);
for (int i = 0; i < m_steerPointCount-1; ++i)
{
dd.vertex(m_steerPoints[i*3+0],m_steerPoints[i*3+1]+0.2f,m_steerPoints[i*3+2], duDarkenCol(steerCol));
dd.vertex(m_steerPoints[(i+1)*3+0],m_steerPoints[(i+1)*3+1]+0.2f,m_steerPoints[(i+1)*3+2], duDarkenCol(steerCol));
}
dd.end();
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT || m_toolMode == TOOLMODE_PATHFIND_SLICED)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol);
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_endRef, endCol);
if (m_npolys)
{
for (int i = 1; i < m_npolys-1; ++i)
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
}
if (m_nstraightPath)
{
dd.depthMask(false);
const unsigned int pathCol = duRGBA(64,16,0,220);
const unsigned int offMeshCol = duRGBA(128,96,0,220);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int i = 0; i < m_nstraightPath-1; ++i)
{
unsigned int col = 0;
if (m_straightPathFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
col = offMeshCol;
else
col = pathCol;
dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], col);
dd.vertex(m_straightPath[(i+1)*3], m_straightPath[(i+1)*3+1]+0.4f, m_straightPath[(i+1)*3+2], col);
}
dd.end();
dd.begin(DU_DRAW_POINTS, 6.0f);
for (int i = 0; i < m_nstraightPath; ++i)
{
unsigned int col = 0;
if (m_straightPathFlags[i] & DT_STRAIGHTPATH_START)
col = startCol;
else if (m_straightPathFlags[i] & DT_STRAIGHTPATH_START)
col = endCol;
else if (m_straightPathFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION)
col = offMeshCol;
else
col = pathCol;
dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], pathCol);
}
dd.end();
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_RAYCAST)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol);
if (m_nstraightPath)
{
for (int i = 1; i < m_npolys; ++i)
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
dd.depthMask(false);
const unsigned int pathCol = m_hitResult ? duRGBA(64,16,0,220) : duRGBA(240,240,240,220);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int i = 0; i < m_nstraightPath-1; ++i)
{
dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], pathCol);
dd.vertex(m_straightPath[(i+1)*3], m_straightPath[(i+1)*3+1]+0.4f, m_straightPath[(i+1)*3+2], pathCol);
}
dd.end();
dd.begin(DU_DRAW_POINTS, 4.0f);
for (int i = 0; i < m_nstraightPath; ++i)
dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], pathCol);
dd.end();
if (m_hitResult)
{
const unsigned int hitCol = duRGBA(0,0,0,128);
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(m_hitPos[0], m_hitPos[1] + 0.4f, m_hitPos[2], hitCol);
dd.vertex(m_hitPos[0] + m_hitNormal[0]*agentRadius,
m_hitPos[1] + 0.4f + m_hitNormal[1]*agentRadius,
m_hitPos[2] + m_hitNormal[2]*agentRadius, hitCol);
dd.end();
}
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_DISTANCE_TO_WALL)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol);
dd.depthMask(false);
duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], m_distanceToWall, duRGBA(64,16,0,220), 2.0f);
dd.begin(DU_DRAW_LINES, 3.0f);
dd.vertex(m_hitPos[0], m_hitPos[1] + 0.02f, m_hitPos[2], duRGBA(0,0,0,192));
dd.vertex(m_hitPos[0], m_hitPos[1] + agentHeight, m_hitPos[2], duRGBA(0,0,0,192));
dd.end();
dd.depthMask(true);
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_CIRCLE)
{
for (int i = 0; i < m_npolys; ++i)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
dd.depthMask(false);
if (m_parent[i])
{
float p0[3], p1[3];
dd.depthMask(false);
getPolyCenter(m_navMesh, m_parent[i], p0);
getPolyCenter(m_navMesh, m_polys[i], p1);
duDebugDrawArc(&dd, p0[0],p0[1],p0[2], p1[0],p1[1],p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0,0,0,128), 2.0f);
dd.depthMask(true);
}
dd.depthMask(true);
}
if (m_sposSet && m_eposSet)
{
dd.depthMask(false);
const float dx = m_epos[0] - m_spos[0];
const float dz = m_epos[2] - m_spos[2];
const float dist = sqrtf(dx*dx + dz*dz);
duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], dist, duRGBA(64,16,0,220), 2.0f);
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_SHAPE)
{
for (int i = 0; i < m_npolys; ++i)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
dd.depthMask(false);
if (m_parent[i])
{
float p0[3], p1[3];
dd.depthMask(false);
getPolyCenter(m_navMesh, m_parent[i], p0);
getPolyCenter(m_navMesh, m_polys[i], p1);
duDebugDrawArc(&dd, p0[0],p0[1],p0[2], p1[0],p1[1],p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0,0,0,128), 2.0f);
dd.depthMask(true);
}
dd.depthMask(true);
}
if (m_sposSet && m_eposSet)
{
dd.depthMask(false);
const unsigned int col = duRGBA(64,16,0,220);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int i = 0, j = 3; i < 4; j=i++)
{
const float* p0 = &m_queryPoly[j*3];
const float* p1 = &m_queryPoly[i*3];
dd.vertex(p0, col);
dd.vertex(p1, col);
}
dd.end();
dd.depthMask(true);
}
}
else if (m_toolMode == TOOLMODE_FIND_LOCAL_NEIGHBOURHOOD)
{
for (int i = 0; i < m_npolys; ++i)
{
duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol);
dd.depthMask(false);
if (m_parent[i])
{
float p0[3], p1[3];
dd.depthMask(false);
getPolyCenter(m_navMesh, m_parent[i], p0);
getPolyCenter(m_navMesh, m_polys[i], p1);
duDebugDrawArc(&dd, p0[0],p0[1],p0[2], p1[0],p1[1],p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0,0,0,128), 2.0f);
dd.depthMask(true);
}
static const int MAX_SEGS = DT_VERTS_PER_POLYGON*2;
float segs[MAX_SEGS*6];
int nsegs = 0;
m_navQuery->getPolyWallSegments(m_polys[i], &m_filter, segs, &nsegs, MAX_SEGS);
dd.begin(DU_DRAW_LINES, 2.0f);
for (int j = 0; j < nsegs; ++j)
{
const float* s = &segs[j*6];
// Skip too distant segments.
float tseg;
float distSqr = dtDistancePtSegSqr2D(m_spos, s, s+3, tseg);
if (distSqr > dtSqr(m_neighbourhoodRadius))
continue;
float delta[3], norm[3], p0[3], p1[3];
dtVsub(delta, s+3,s);
dtVmad(p0, s, delta, 0.5f);
norm[0] = delta[2];
norm[1] = 0;
norm[2] = -delta[0];
dtVnormalize(norm);
dtVmad(p1, p0, norm, agentRadius*0.5f);
// Skip backfacing segments.
unsigned int col = duRGBA(255,255,255,192);
if (dtTriArea2D(m_spos, s, s+3) < 0.0f)
col = duRGBA(255,255,255,64);
dd.vertex(p0[0],p0[1]+agentClimb,p0[2],duRGBA(0,0,0,128));
dd.vertex(p1[0],p1[1]+agentClimb,p1[2],duRGBA(0,0,0,128));
dd.vertex(s[0],s[1]+agentClimb,s[2],col);
dd.vertex(s[3],s[4]+agentClimb,s[5],col);
}
dd.end();
dd.depthMask(true);
}
if (m_sposSet)
{
dd.depthMask(false);
duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], m_neighbourhoodRadius, duRGBA(64,16,0,220), 2.0f);
dd.depthMask(true);
}
}
}
void NavMeshTesterTool::recalc()
{
if (!m_navMesh)
return;
if (m_sposSet)
m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, 0);
else
m_startRef = 0;
if (m_eposSet)
m_navQuery->findNearestPoly(m_epos, m_polyPickExt, &m_filter, &m_endRef, 0);
else
m_endRef = 0;
m_pathFindStatus = DT_FAILURE;
if (m_toolMode == TOOLMODE_PATHFIND_FOLLOW)
{
m_pathIterNum = 0;
if (m_sposSet && m_eposSet && m_startRef && m_endRef)
{
#ifdef DUMP_REQS
printf("pi %f %f %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
m_nsmoothPath = 0;
if (m_npolys)
{
// Iterate over the path to find smooth path on the detail mesh surface.
dtPolyRef polys[MAX_POLYS];
memcpy(polys, m_polys, sizeof(dtPolyRef)*m_npolys);
int npolys = m_npolys;
float iterPos[3], targetPos[3];
m_navQuery->closestPointOnPolyBoundary(m_startRef, m_spos, iterPos);
m_navQuery->closestPointOnPolyBoundary(polys[npolys-1], m_epos, targetPos);
static const float STEP_SIZE = 0.5f;
static const float SLOP = 0.01f;
m_nsmoothPath = 0;
dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
m_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 && m_nsmoothPath < MAX_SMOOTH)
{
// Find location to steer towards.
float steerPos[3];
unsigned char steerPosFlag;
dtPolyRef steerPosRef;
if (!getSteerTarget(m_navQuery, iterPos, targetPos, SLOP,
polys, npolys, steerPos, steerPosFlag, steerPosRef))
break;
bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
// Find movement delta.
float delta[3], len;
dtVsub(delta, steerPos, iterPos);
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 < STEP_SIZE)
len = 1;
else
len = STEP_SIZE / len;
float moveTgt[3];
dtVmad(moveTgt, iterPos, delta, len);
// Move
float result[3];
dtPolyRef visited[16];
int nvisited = 0;
m_navQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter,
result, visited, &nvisited, 16);
npolys = fixupCorridor(polys, npolys, MAX_POLYS, visited, nvisited);
float h = 0;
m_navQuery->getPolyHeight(polys[0], result, &h);
result[1] = h;
dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(iterPos, steerPos, SLOP, 1.0f))
{
// Reached end of path.
dtVcopy(iterPos, targetPos);
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
m_nsmoothPath++;
}
break;
}
else if (offMeshConnection && inRange(iterPos, steerPos, SLOP, 1.0f))
{
// Reached off-mesh connection.
float startPos[3], endPos[3];
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = 0, polyRef = polys[0];
int npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (int i = npos; i < npolys; ++i)
polys[i-npos] = polys[i];
npolys -= npos;
// Handle the connection.
if (m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos) == DT_SUCCESS)
{
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if (m_nsmoothPath & 1)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
dtVcopy(iterPos, endPos);
float h;
m_navQuery->getPolyHeight(polys[0], iterPos, &h);
iterPos[1] = h;
}
}
// Store results.
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
m_nsmoothPath++;
}
}
}
}
else
{
m_npolys = 0;
m_nsmoothPath = 0;
}
}
else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT)
{
if (m_sposSet && m_eposSet && m_startRef && m_endRef)
{
#ifdef DUMP_REQS
printf("ps %f %f %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
m_nstraightPath = 0;
if (m_npolys)
{
// In case of partial path, make sure the end point is clamped to the last polygon.
float epos[3];
dtVcopy(epos, m_epos);
if (m_polys[m_npolys-1] != m_endRef)
m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos);
m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
m_straightPath, m_straightPathFlags,
m_straightPathPolys, &m_nstraightPath, MAX_POLYS);
}
}
else
{
m_npolys = 0;
m_nstraightPath = 0;
}
}
else if (m_toolMode == TOOLMODE_PATHFIND_SLICED)
{
if (m_sposSet && m_eposSet && m_startRef && m_endRef)
{
#ifdef DUMP_REQS
printf("ps %f %f %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_npolys = 0;
m_nstraightPath = 0;
m_pathFindStatus = m_navQuery->initSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter);
}
else
{
m_npolys = 0;
m_nstraightPath = 0;
}
}
else if (m_toolMode == TOOLMODE_RAYCAST)
{
m_nstraightPath = 0;
if (m_sposSet && m_eposSet && m_startRef)
{
#ifdef DUMP_REQS
printf("rc %f %f %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
float t = 0;
m_npolys = 0;
m_nstraightPath = 2;
m_straightPath[0] = m_spos[0];
m_straightPath[1] = m_spos[1];
m_straightPath[2] = m_spos[2];
m_navQuery->raycast(m_startRef, m_spos, m_epos, &m_filter, &t, m_hitNormal, m_polys, &m_npolys, MAX_POLYS);
if (t > 1)
{
// No hit
dtVcopy(m_hitPos, m_epos);
m_hitResult = false;
}
else
{
// Hit
m_hitPos[0] = m_spos[0] + (m_epos[0] - m_spos[0]) * t;
m_hitPos[1] = m_spos[1] + (m_epos[1] - m_spos[1]) * t;
m_hitPos[2] = m_spos[2] + (m_epos[2] - m_spos[2]) * t;
if (m_npolys)
{
float h = 0;
m_navQuery->getPolyHeight(m_polys[m_npolys-1], m_hitPos, &h);
m_hitPos[1] = h;
}
m_hitResult = true;
}
dtVcopy(&m_straightPath[3], m_hitPos);
}
}
else if (m_toolMode == TOOLMODE_DISTANCE_TO_WALL)
{
m_distanceToWall = 0;
if (m_sposSet && m_startRef)
{
#ifdef DUMP_REQS
printf("dw %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], 100.0f,
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_distanceToWall = 0.0f;
m_navQuery->findDistanceToWall(m_startRef, m_spos, 100.0f, &m_filter, &m_distanceToWall, m_hitPos, m_hitNormal);
}
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_CIRCLE)
{
if (m_sposSet && m_startRef && m_eposSet)
{
const float dx = m_epos[0] - m_spos[0];
const float dz = m_epos[2] - m_spos[2];
float dist = sqrtf(dx*dx + dz*dz);
#ifdef DUMP_REQS
printf("fpc %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], dist,
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPolysAroundCircle(m_startRef, m_spos, dist, &m_filter,
m_polys, m_parent, 0, &m_npolys, MAX_POLYS);
}
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_SHAPE)
{
if (m_sposSet && m_startRef && m_eposSet)
{
const float nx = (m_epos[2] - m_spos[2])*0.25f;
const float nz = -(m_epos[0] - m_spos[0])*0.25f;
const float agentHeight = m_sample ? m_sample->getAgentHeight() : 0;
m_queryPoly[0] = m_spos[0] + nx*1.2f;
m_queryPoly[1] = m_spos[1] + agentHeight/2;
m_queryPoly[2] = m_spos[2] + nz*1.2f;
m_queryPoly[3] = m_spos[0] - nx*1.3f;
m_queryPoly[4] = m_spos[1] + agentHeight/2;
m_queryPoly[5] = m_spos[2] - nz*1.3f;
m_queryPoly[6] = m_epos[0] - nx*0.8f;
m_queryPoly[7] = m_epos[1] + agentHeight/2;
m_queryPoly[8] = m_epos[2] - nz*0.8f;
m_queryPoly[9] = m_epos[0] + nx;
m_queryPoly[10] = m_epos[1] + agentHeight/2;
m_queryPoly[11] = m_epos[2] + nz;
#ifdef DUMP_REQS
printf("fpp %f %f %f %f %f %f %f %f %f %f %f %f 0x%x 0x%x\n",
m_queryPoly[0],m_queryPoly[1],m_queryPoly[2],
m_queryPoly[3],m_queryPoly[4],m_queryPoly[5],
m_queryPoly[6],m_queryPoly[7],m_queryPoly[8],
m_queryPoly[9],m_queryPoly[10],m_queryPoly[11],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPolysAroundShape(m_startRef, m_queryPoly, 4, &m_filter,
m_polys, m_parent, 0, &m_npolys, MAX_POLYS);
}
}
else if (m_toolMode == TOOLMODE_FIND_LOCAL_NEIGHBOURHOOD)
{
if (m_sposSet && m_startRef)
{
#ifdef DUMP_REQS
printf("fln %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_neighbourhoodRadius,
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findLocalNeighbourhood(m_startRef, m_spos, m_neighbourhoodRadius, &m_filter,
m_polys, m_parent, &m_npolys, MAX_POLYS);
}
}
}
void NavMeshTesterTool::handleToggle()
{
// TODO: merge separate to a path iterator. Use same code in recalc() too.
if (m_toolMode != TOOLMODE_PATHFIND_FOLLOW)
return;
if (!m_sposSet || !m_eposSet || !m_startRef || !m_endRef)
return;
static const float STEP_SIZE = 0.5f;
static const float SLOP = 0.01f;
if (m_pathIterNum == 0)
{
m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
m_nsmoothPath = 0;
m_pathIterPolyCount = m_npolys;
if (m_pathIterPolyCount)
memcpy(m_pathIterPolys, m_polys, sizeof(dtPolyRef)*m_pathIterPolyCount);
if (m_pathIterPolyCount)
{
// Iterate over the path to find smooth path on the detail mesh surface.
m_navQuery->closestPointOnPolyBoundary(m_startRef, m_spos, m_iterPos);
m_navQuery->closestPointOnPolyBoundary(m_pathIterPolys[m_pathIterPolyCount-1], m_epos, m_targetPos);
m_nsmoothPath = 0;
dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos);
m_nsmoothPath++;
}
}
dtVcopy(m_prevIterPos, m_iterPos);
m_pathIterNum++;
if (!m_pathIterPolyCount)
return;
if (m_nsmoothPath >= MAX_SMOOTH)
return;
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
// Find location to steer towards.
float steerPos[3];
unsigned char steerPosFlag;
dtPolyRef steerPosRef;
if (!getSteerTarget(m_navQuery, m_iterPos, m_targetPos, SLOP,
m_pathIterPolys, m_pathIterPolyCount, steerPos, steerPosFlag, steerPosRef,
m_steerPoints, &m_steerPointCount))
return;
dtVcopy(m_steerPos, steerPos);
bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
// Find movement delta.
float delta[3], len;
dtVsub(delta, steerPos, m_iterPos);
len = sqrtf(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 < STEP_SIZE)
len = 1;
else
len = STEP_SIZE / len;
float moveTgt[3];
dtVmad(moveTgt, m_iterPos, delta, len);
// Move
float result[3];
dtPolyRef visited[16];
int nvisited = 0;
m_navQuery->moveAlongSurface(m_pathIterPolys[0], m_iterPos, moveTgt, &m_filter,
result, visited, &nvisited, 16);
m_pathIterPolyCount = fixupCorridor(m_pathIterPolys, m_pathIterPolyCount, MAX_POLYS, visited, nvisited);
float h = 0;
m_navQuery->getPolyHeight(m_pathIterPolys[0], result, &h);
result[1] = h;
dtVcopy(m_iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(m_iterPos, steerPos, SLOP, 1.0f))
{
// Reached end of path.
dtVcopy(m_iterPos, m_targetPos);
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos);
m_nsmoothPath++;
}
return;
}
else if (offMeshConnection && inRange(m_iterPos, steerPos, SLOP, 1.0f))
{
// Reached off-mesh connection.
float startPos[3], endPos[3];
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = 0, polyRef = m_pathIterPolys[0];
int npos = 0;
while (npos < m_pathIterPolyCount && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = m_pathIterPolys[npos];
npos++;
}
for (int i = npos; i < m_pathIterPolyCount; ++i)
m_pathIterPolys[i-npos] = m_pathIterPolys[i];
m_pathIterPolyCount -= npos;
// Handle the connection.
if (m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos) == DT_SUCCESS)
{
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if (m_nsmoothPath & 1)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
dtVcopy(m_iterPos, endPos);
float h;
m_navQuery->getPolyHeight(m_pathIterPolys[0], m_iterPos, &h);
m_iterPos[1] = h;
}
}
// Store results.
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos);
m_nsmoothPath++;
}
}
void dtCrowd::updatePosition(const float dt, unsigned* agentsIdx, unsigned nbIdx)
{
// If we want to update every agent
if (nbIdx == 0)
{
agentsIdx = m_agentsToUpdate;
nbIdx = m_maxAgents;
}
nbIdx = (nbIdx < m_maxAgents) ? nbIdx : m_maxAgents;
// The current start position of the agent (not yet modified)
dtPolyRef* currentPosPoly = (dtPolyRef*) dtAlloc(sizeof(dtPolyRef) * nbIdx, DT_ALLOC_TEMP);
float* currentPos = (float*) dtAlloc(sizeof(float) * 3 * nbIdx, DT_ALLOC_TEMP);
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
m_crowdQuery->getNavMeshQuery()->findNearestPoly(ag->position, m_crowdQuery->getQueryExtents(), m_crowdQuery->getQueryFilter(), currentPosPoly + i, currentPos + (i * 3));
}
// Integrate.
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
integrate(ag, dt);
}
// Handle collisions.
static const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (unsigned iter = 0; iter < 4; ++iter)
{
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
const int idx0 = getAgentIndex(ag);
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
float* disp = m_disp[agentsIdx[i]];
dtVset(disp, 0, 0, 0);
float w = 0;
for (unsigned j = 0; j < m_agentsEnv[ag->id].nbNeighbors; ++j)
{
const dtCrowdAgent* nei = &m_agents[m_agentsEnv[ag->id].neighbors[j].idx];
const int idx1 = getAgentIndex(nei);
float diff[3];
dtVsub(diff, ag->position, nei->position);
diff[1] = 0;
float dist = dtVlenSqr(diff);
if (dist > dtSqr(ag->radius + nei->radius) + EPSILON)
continue;
dist = sqrtf(dist);
float pen = (ag->radius + nei->radius) - dist;
if (dist < EPSILON)
{
// Agents on top of each other, try to choose diverging separation directions.
if (idx0 > idx1)
dtVset(diff, -ag->desiredVelocity[2], 0, ag->desiredVelocity[0]);
else
dtVset(diff, ag->desiredVelocity[2], 0, -ag->desiredVelocity[0]);
pen = 0.01f;
}
else
{
pen = (1.0f / dist) * (pen * 0.5f) * COLLISION_RESOLVE_FACTOR;
}
dtVmad(disp, disp, diff, pen);
w += 1.0f;
}
if (w > EPSILON)
{
const float iw = 1.0f / w;
dtVscale(disp, disp, iw);
}
}
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
float* disp = m_disp[agentsIdx[i]];
dtVadd(ag->position, ag->position, disp);
}
}
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
// Move along navmesh.
float newPos[3];
dtPolyRef visited[dtPathCorridor::MAX_VISITED];
int visitedCount;
m_crowdQuery->getNavMeshQuery()->moveAlongSurface(currentPosPoly[i], currentPos + (i * 3), ag->position, m_crowdQuery->getQueryFilter(), newPos,
visited, &visitedCount, dtPathCorridor::MAX_VISITED);
// Get valid constrained position back.
float newHeight = *(currentPos + (i * 3) + 1);
m_crowdQuery->getNavMeshQuery()->getPolyHeight(currentPosPoly[i], newPos, &newHeight);
newPos[1] = newHeight;
dtVcopy(ag->position, newPos);
}
// Update agents using off-mesh connection.
for (unsigned i = 0; i < nbIdx; ++i)
{
dtCrowdAgent* ag = 0;
if (!getActiveAgent(&ag, agentsIdx[i]))
continue;
float offmeshTotalTime = ag->offmeshInitToStartTime + ag->offmeshStartToEndTime;
if (ag->state == DT_CROWDAGENT_STATE_OFFMESH && offmeshTotalTime > EPSILON)
{
ag->offmeshElaspedTime += dt;
if (ag->offmeshElaspedTime > offmeshTotalTime)
{
// Prepare agent for walking.
ag->state = DT_CROWDAGENT_STATE_WALKING;
continue;
}
// Update position
if (ag->offmeshElaspedTime < ag->offmeshInitToStartTime)
{
const float u = tween(ag->offmeshElaspedTime, 0.0, ag->offmeshInitToStartTime);
dtVlerp(ag->position, ag->offmeshInitPos, ag->offmeshStartPos, u);
}
else
{
const float u = tween(ag->offmeshElaspedTime, ag->offmeshInitToStartTime, offmeshTotalTime);
dtVlerp(ag->position, ag->offmeshStartPos, ag->offmeshEndPos, u);
}
// Update velocity.
dtVset(ag->velocity, 0,0,0);
dtVset(ag->desiredVelocity, 0,0,0);
}
}
dtFree(currentPosPoly);
dtFree(currentPos);
}
void dtCrowd::updateStepMove(const float dt, dtCrowdAgentDebugInfo*)
{
// Integrate.
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[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 < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[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 = sqrtf(dist);
float pen = (ag->params.radius + nei->params.radius) - dist;
if (dist < 0.0001f)
{
// m_activeAgents 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 < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[i];
if (ag->state != DT_CROWDAGENT_STATE_WALKING)
continue;
dtVadd(ag->npos, ag->npos, ag->disp);
}
}
}
void dtCrowd::updateStepSteering(const float dt, dtCrowdAgentDebugInfo*)
{
// Calculate steering.
for (int i = 0; i < m_numActiveAgents; ++i)
{
dtCrowdAgent* ag = m_activeAgents[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);
float speedScale = 1.0f;
if (ag->params.updateFlags & DT_CROWD_SLOWDOWN_AT_GOAL)
{
// 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.
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 = sqrtf(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);
}
}
/**
@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;
}
void cocos2d::NavMesh::findPath(const Vec3 &start, const Vec3 &end, std::vector<Vec3> &pathPoints)
{
static const int MAX_POLYS = 256;
static const int MAX_SMOOTH = 2048;
float ext[3];
ext[0] = 2; ext[1] = 4; ext[2] = 2;
dtQueryFilter filter;
dtPolyRef startRef, endRef;
dtPolyRef polys[MAX_POLYS];
int npolys = 0;
_navMeshQuery->findNearestPoly(&start.x, ext, &filter, &startRef, 0);
_navMeshQuery->findNearestPoly(&end.x, ext, &filter, &endRef, 0);
_navMeshQuery->findPath(startRef, endRef, &start.x, &end.x, &filter, polys, &npolys, MAX_POLYS);
if (npolys)
{
//// Iterate over the path to find smooth path on the detail mesh surface.
//dtPolyRef polys[MAX_POLYS];
//memcpy(polys, polys, sizeof(dtPolyRef)*npolys);
//int npolys = npolys;
float iterPos[3], targetPos[3];
_navMeshQuery->closestPointOnPoly(startRef, &start.x, iterPos, 0);
_navMeshQuery->closestPointOnPoly(polys[npolys - 1], &end.x, targetPos, 0);
static const float STEP_SIZE = 0.5f;
static const float SLOP = 0.01f;
int nsmoothPath = 0;
//dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
//m_nsmoothPath++;
pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2]));
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 < MAX_SMOOTH)
{
// Find location to steer towards.
float steerPos[3];
unsigned char steerPosFlag;
dtPolyRef steerPosRef;
if (!getSteerTarget(_navMeshQuery, iterPos, targetPos, SLOP,
polys, npolys, steerPos, steerPosFlag, steerPosRef))
break;
bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
// Find movement delta.
float delta[3], len;
dtVsub(delta, steerPos, iterPos);
len = dtMathSqrtf(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 < STEP_SIZE)
len = 1;
else
len = STEP_SIZE / len;
float moveTgt[3];
dtVmad(moveTgt, iterPos, delta, len);
// Move
float result[3];
dtPolyRef visited[16];
int nvisited = 0;
_navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &filter,
result, visited, &nvisited, 16);
npolys = fixupCorridor(polys, npolys, MAX_POLYS, visited, nvisited);
npolys = fixupShortcuts(polys, npolys, _navMeshQuery);
float h = 0;
_navMeshQuery->getPolyHeight(polys[0], result, &h);
result[1] = h;
dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(iterPos, steerPos, SLOP, 1.0f))
{
// Reached end of path.
dtVcopy(iterPos, targetPos);
if (nsmoothPath < MAX_SMOOTH)
{
//dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
//m_nsmoothPath++;
pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2]));
nsmoothPath++;
}
break;
}
else if (offMeshConnection && inRange(iterPos, steerPos, SLOP, 1.0f))
{
// Reached off-mesh connection.
float startPos[3], endPos[3];
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = 0, polyRef = polys[0];
int npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (int i = npos; i < npolys; ++i)
polys[i - npos] = polys[i];
npolys -= npos;
// Handle the connection.
dtStatus status = _navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
if (dtStatusSucceed(status))
{
if (nsmoothPath < MAX_SMOOTH)
{
//dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
//m_nsmoothPath++;
pathPoints.push_back(Vec3(startPos[0], startPos[1], startPos[2]));
nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if (nsmoothPath & 1)
{
//dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
//m_nsmoothPath++;
pathPoints.push_back(Vec3(startPos[0], startPos[1], startPos[2]));
nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
dtVcopy(iterPos, endPos);
float eh = 0.0f;
_navMeshQuery->getPolyHeight(polys[0], iterPos, &eh);
iterPos[1] = eh;
}
}
// Store results.
if (nsmoothPath < MAX_SMOOTH)
{
//dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
//m_nsmoothPath++;
pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2]));
nsmoothPath++;
}
}
}
}