/**
@par
Behavior:
- The movement is constrained to the surface of the navigation mesh.
- The corridor is automatically adjusted (shorted or lengthened) in order to remain valid.
- The new position will be located in the adjusted corridor's first polygon.
The expected use case is that the desired position will be 'near' the current corridor. What is considered 'near'
depends on local polygon density, query search extents, etc.
The resulting position will differ from the desired position if the desired position is not on the navigation mesh,
or it can't be reached using a local search.
*/
bool dtPathCorridor::movePosition(const float* npos, dtNavMeshQuery* navquery, const dtQueryFilter* filter)
{
dtAssert(m_path);
dtAssert(m_npath);
// Move along navmesh and update new position.
float result[3];
static const int MAX_VISITED = 16;
dtPolyRef visited[MAX_VISITED];
int nvisited = 0;
dtStatus status = navquery->moveAlongSurface(m_path[0], m_pos, npos, filter,
result, visited, &nvisited, MAX_VISITED);
if (dtStatusSucceed(status)) {
m_npath = dtMergeCorridorStartMoved(m_path, m_npath, m_maxPath, visited, nvisited);
// Adjust the position to stay on top of the navmesh.
float h = m_pos[1];
navquery->getPolyHeight(m_path[0], result, &h);
result[1] = h;
dtVcopy(m_pos, result);
return true;
}
return false;
}
bool PathInfo::getSteerTarget(const float* startPos, const float* endPos,
float minTargetDist, const dtPolyRef* path, 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 dtNavMesh::init(const dtNavMeshParams* params)
{
memcpy(&m_params, params, sizeof(dtNavMeshParams));
dtVcopy(m_orig, params->orig);
m_tileWidth = params->tileWidth;
m_tileHeight = params->tileHeight;
// Init tiles
m_maxTiles = params->maxTiles;
m_tileLutSize = dtNextPow2(params->maxTiles/4);
if (!m_tileLutSize) m_tileLutSize = 1;
m_tileLutMask = m_tileLutSize-1;
m_tiles = (dtMeshTile*)dtAlloc(sizeof(dtMeshTile)*m_maxTiles, DT_ALLOC_PERM);
if (!m_tiles)
return DT_FAILURE | DT_OUT_OF_MEMORY;
m_posLookup = (dtMeshTile**)dtAlloc(sizeof(dtMeshTile*)*m_tileLutSize, DT_ALLOC_PERM);
if (!m_posLookup)
return DT_FAILURE | DT_OUT_OF_MEMORY;
memset(m_tiles, 0, sizeof(dtMeshTile)*m_maxTiles);
memset(m_posLookup, 0, sizeof(dtMeshTile*)*m_tileLutSize);
m_nextFree = 0;
for (int i = m_maxTiles-1; i >= 0; --i)
{
m_tiles[i].salt = 1;
m_tiles[i].next = m_nextFree;
m_nextFree = &m_tiles[i];
}
// Edited by TC
m_tileBits = STATIC_TILE_BITS;
m_polyBits = STATIC_POLY_BITS;
m_saltBits = STATIC_SALT_BITS;
return DT_SUCCESS;
}
/**
@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.
*/
bool 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 true;
// Overshoot a little. This helps to optimize open fields in tiled meshes.
// UE4: changes to ray adjustment - make sure it's not going further than newDist
float newDist = dtMin(dist+0.01f, pathOptimizationRange);
// Adjust ray length.
float delta[3];
dtVsub(delta, goal, m_pos);
dtVmad(goal, m_pos, delta, newDist / 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);
return true;
}
return false;
}
EXPORT_API void dtcGetQueryExtents(dtCrowd* crowd, float* extents)
{
const float* e = crowd->getQueryExtents();
dtVcopy(extents, e);
}
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 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;
if (!params->detailMeshes || !params->detailVerts || !params->detailTris)
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;
if (params->offMeshConCount > 0)
{
offMeshConClass = (unsigned char*)dtAlloc(sizeof(unsigned char)*params->offMeshConCount*2, DT_ALLOC_TEMP);
if (!offMeshConClass)
return false;
for (int i = 0; i < params->offMeshConCount; ++i)
{
offMeshConClass[i*2+0] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+0)*3], params->bmin, params->bmax);
offMeshConClass[i*2+1] = classifyOffMeshPoint(¶ms->offMeshConVerts[(i*2+1)*3], params->bmin, params->bmax);
// Cound 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++;
}
}
// Off-mesh connectionss are stored as polygons, adjust values.
const int totPolyCount = params->polyCount + storedOffMeshConCount;
const int totVertCount = params->vertCount + storedOffMeshConCount*2;
// 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;
int nj = j+1;
if (nj >= nvp || p[nj] == MESH_NULL_IDX) nj = 0;
const unsigned short* va = ¶ms->verts[p[j]*3];
const unsigned short* vb = ¶ms->verts[p[nj]*3];
edgeCount++;
if (params->tileSize > 0)
{
if (va[0] == params->tileSize && vb[0] == params->tileSize)
portalCount++; // x+
else if (va[2] == params->tileSize && vb[2] == params->tileSize)
portalCount++; // z+
else if (va[0] == 0 && vb[0] == 0)
portalCount++; // x-
else if (va[2] == 0 && vb[2] == 0)
portalCount++; // z-
}
}
}
const int maxLinkCount = edgeCount + portalCount*2 + offMeshConLinkCount*2;
// Find unique detail vertices.
int uniqueDetailVertCount = 0;
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;
}
// 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*params->detailTriCount);
const int bvTreeSize = dtAlign4(sizeof(dtBVNode)*params->polyCount*2);
const int offMeshConsSize = dtAlign4(sizeof(dtOffMeshConnection)*storedOffMeshConCount);
const int dataSize = headerSize + vertsSize + polysSize + linksSize +
detailMeshesSize + detailVertsSize + detailTrisSize +
bvTreeSize + offMeshConsSize;
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;
// Store header
header->magic = DT_NAVMESH_MAGIC;
header->version = DT_NAVMESH_VERSION;
header->x = params->tileX;
header->y = params->tileY;
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 = params->detailTriCount;
header->bvQuantFactor = 1.0f / params->cs;
header->offMeshBase = params->polyCount;
header->walkableHeight = params->walkableHeight;
header->walkableRadius = params->walkableRadius;
header->walkableClimb = params->walkableClimb;
header->offMeshConCount = storedOffMeshConCount;
header->bvNodeCount = params->polyCount*2;
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 link vertices.
int n = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
// Only store connections which start from this tile.
if (offMeshConClass[i*2+0] == 0xff)
{
const float* linkv = ¶ms->offMeshConVerts[i*2*3];
float* v = &navVerts[(offMeshVertsBase + n*2)*3];
dtVcopy(&v[0], &linkv[0]);
dtVcopy(&v[3], &linkv[3]);
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->area = params->polyAreas[i];
p->type = DT_POLYTYPE_GROUND;
for (int j = 0; j < nvp; ++j)
{
if (src[j] == MESH_NULL_IDX) break;
p->verts[j] = src[j];
p->neis[j] = (src[nvp+j]+1) & 0xffff;
p->vertCount++;
}
src += nvp*2;
}
// Off-mesh connection vertices.
n = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
// Only store connections which start from this tile.
if (offMeshConClass[i*2+0] == 0xff)
{
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 = params->offMeshConFlags[i];
p->area = params->offMeshConAreas[i];
p->type = DT_POLYTYPE_OFFMESH_CONNECTION;
n++;
}
}
// Store portal edges.
if (params->tileSize > 0)
{
for (int i = 0; i < params->polyCount; ++i)
{
dtPoly* poly = &navPolys[i];
for (int j = 0; j < poly->vertCount; ++j)
{
int nj = j+1;
if (nj >= poly->vertCount) nj = 0;
const unsigned short* va = ¶ms->verts[poly->verts[j]*3];
const unsigned short* vb = ¶ms->verts[poly->verts[nj]*3];
if (va[0] == params->tileSize && vb[0] == params->tileSize) // x+
poly->neis[j] = DT_EXT_LINK | 0;
else if (va[2] == params->tileSize && vb[2] == params->tileSize) // z+
poly->neis[j] = DT_EXT_LINK | 2;
else if (va[0] == 0 && vb[0] == 0) // x-
poly->neis[j] = DT_EXT_LINK | 4;
else if (va[2] == 0 && vb[2] == 0) // z-
poly->neis[j] = DT_EXT_LINK | 6;
}
}
}
// 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.
unsigned short vbase = 0;
for (int i = 0; i < params->polyCount; ++i)
{
dtPolyDetail& dtl = navDMeshes[i];
const int vb = params->detailMeshes[i*4+0];
const int ndv = params->detailMeshes[i*4+1];
const int nv = navPolys[i].vertCount;
dtl.vertBase = vbase;
dtl.vertCount = (unsigned short)(ndv-nv);
dtl.triBase = params->detailMeshes[i*4+2];
dtl.triCount = 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);
// Store and create BVtree.
// TODO: take detail mesh into account! use byte per bbox extent?
createBVTree(params->verts, params->vertCount, params->polys, params->polyCount,
nvp, params->cs, params->ch, params->polyCount*2, navBvtree);
// Store Off-Mesh connections.
n = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
// Only store connections which start from this tile.
if (offMeshConClass[i*2+0] == 0xff)
{
dtOffMeshConnection* con = &offMeshCons[n];
con->poly = (unsigned short)(offMeshPolyBase + n);
// Copy connection end-points.
const float* endPts = ¶ms->offMeshConVerts[i*2*3];
dtVcopy(&con->pos[0], &endPts[0]);
dtVcopy(&con->pos[3], &endPts[3]);
con->rad = params->offMeshConRad[i];
con->flags = params->offMeshConDir[i] ? DT_OFFMESH_CON_BIDIR : 0;
con->side = offMeshConClass[i*2+1];
n++;
}
}
dtFree(offMeshConClass);
*outData = data;
*outDataSize = dataSize;
return true;
}
void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
{
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
getTileAndPolyByRefUnsafe(ref, &tile, &poly);
// Off-mesh connections don't have detail polygons.
if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
{
const float* v0 = &tile->verts[poly->verts[0]*3];
const float* v1 = &tile->verts[poly->verts[1]*3];
const float d0 = dtVdist(pos, v0);
const float d1 = dtVdist(pos, v1);
const float u = d0 / (d0+d1);
dtVlerp(closest, v0, v1, u);
if (posOverPoly)
*posOverPoly = false;
return;
}
const unsigned int ip = (unsigned int)(poly - tile->polys);
const dtPolyDetail* pd = &tile->detailMeshes[ip];
// Clamp point to be inside the polygon.
float verts[DT_VERTS_PER_POLYGON*3];
float edged[DT_VERTS_PER_POLYGON];
float edget[DT_VERTS_PER_POLYGON];
const int nv = poly->vertCount;
for (int i = 0; i < nv; ++i)
dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
dtVcopy(closest, pos);
if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
{
// Point is outside the polygon, dtClamp to nearest edge.
float dmin = FLT_MAX;
int imin = -1;
for (int i = 0; i < nv; ++i)
{
if (edged[i] < dmin)
{
dmin = edged[i];
imin = i;
}
}
const float* va = &verts[imin*3];
const float* vb = &verts[((imin+1)%nv)*3];
dtVlerp(closest, va, vb, edget[imin]);
if (posOverPoly)
*posOverPoly = false;
}
else
{
if (posOverPoly)
*posOverPoly = true;
}
// Find height at the location.
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 h;
if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
{
closest[1] = h;
break;
}
}
}
static int rasterizeTileLayers(BuildContext* ctx, CMapMesh* geom,
const int tx, const int ty,
const rcConfig& cfg,
TileCacheData* tiles,
const int maxTiles)
{
if (!geom || !geom->GetChunkyMesh())
{
ctx->log(RC_LOG_ERROR, "buildTile: Input mesh is not specified.");
return 0;
}
FastLZCompressor comp;
RasterizationContext rc;
const float* verts = geom->GetVerts();
const int nverts = geom->GetNumVerts();
const rcChunkyTriMesh* chunkyMesh = geom->GetChunkyMesh();
// Tile bounds.
const float tcs = cfg.tileSize * cfg.cs;
rcConfig tcfg;
memcpy(&tcfg, &cfg, sizeof(tcfg));
tcfg.bmin[0] = cfg.bmin[0] + tx*tcs;
tcfg.bmin[1] = cfg.bmin[1];
tcfg.bmin[2] = cfg.bmin[2] + ty*tcs;
tcfg.bmax[0] = cfg.bmin[0] + (tx+1)*tcs;
tcfg.bmax[1] = cfg.bmax[1];
tcfg.bmax[2] = cfg.bmin[2] + (ty+1)*tcs;
tcfg.bmin[0] -= tcfg.borderSize*tcfg.cs;
tcfg.bmin[2] -= tcfg.borderSize*tcfg.cs;
tcfg.bmax[0] += tcfg.borderSize*tcfg.cs;
tcfg.bmax[2] += tcfg.borderSize*tcfg.cs;
// Allocate voxel heightfield where we rasterize our input data to.
rc.solid = rcAllocHeightfield();
if (!rc.solid)
{
ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'.");
return 0;
}
if (!rcCreateHeightfield(ctx, *rc.solid, tcfg.width, tcfg.height, tcfg.bmin, tcfg.bmax, tcfg.cs, tcfg.ch))
{
ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield.");
return 0;
}
// Allocate array that can hold triangle flags.
// If you have multiple meshes you need to process, allocate
// and array which can hold the max number of triangles you need to process.
rc.triareas = new unsigned char[chunkyMesh->maxTrisPerChunk];
if (!rc.triareas)
{
ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", chunkyMesh->maxTrisPerChunk);
return 0;
}
float tbmin[2], tbmax[2];
tbmin[0] = tcfg.bmin[0];
tbmin[1] = tcfg.bmin[2];
tbmax[0] = tcfg.bmax[0];
tbmax[1] = tcfg.bmax[2];
int cid[512];// TODO: Make grow when returning too many items.
const int ncid = rcGetChunksOverlappingRect(chunkyMesh, tbmin, tbmax, cid, 512);
if (!ncid)
{
return 0; // empty
}
for (int i = 0; i < ncid; ++i)
{
const rcChunkyTriMeshNode& node = chunkyMesh->nodes[cid[i]];
const int* tris = &chunkyMesh->tris[node.i*3];
const int ntris = node.n;
memset(rc.triareas, 0, ntris*sizeof(unsigned char));
rcMarkWalkableTriangles(ctx, tcfg.walkableSlopeAngle,
verts, nverts, tris, ntris, rc.triareas);
rcRasterizeTriangles(ctx, verts, nverts, tris, rc.triareas, ntris, *rc.solid, tcfg.walkableClimb);
}
// Once all geometry is rasterized, we do initial pass of filtering to
// remove unwanted overhangs caused by the conservative rasterization
// as well as filter spans where the character cannot possibly stand.
rcFilterLowHangingWalkableObstacles(ctx, tcfg.walkableClimb, *rc.solid);
rcFilterLedgeSpans(ctx, tcfg.walkableHeight, tcfg.walkableClimb, *rc.solid);
rcFilterWalkableLowHeightSpans(ctx, tcfg.walkableHeight, *rc.solid);
rc.chf = rcAllocCompactHeightfield();
if (!rc.chf)
{
ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'.");
return 0;
}
if (!rcBuildCompactHeightfield(ctx, tcfg.walkableHeight, tcfg.walkableClimb, *rc.solid, *rc.chf))
{
ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data.");
return 0;
}
// Erode the walkable area by agent radius.
if (!rcErodeWalkableArea(ctx, tcfg.walkableRadius, *rc.chf))
{
ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode.");
return 0;
}
#if 0
// (Optional) Mark areas.
const ConvexVolume* vols = geom->getConvexVolumes();
for (int i = 0; i < geom->getConvexVolumeCount(); ++i)
{
rcMarkConvexPolyArea(ctx, vols[i].verts, vols[i].nverts,
vols[i].hmin, vols[i].hmax,
(unsigned char)vols[i].area, *rc.chf);
}
#endif // 0
rc.lset = rcAllocHeightfieldLayerSet();
if (!rc.lset)
{
ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'lset'.");
return 0;
}
if (!rcBuildHeightfieldLayers(ctx, *rc.chf, tcfg.borderSize, tcfg.walkableHeight, *rc.lset))
{
ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build heighfield layers.");
return 0;
}
rc.ntiles = 0;
for (int i = 0; i < rcMin(rc.lset->nlayers, MAX_LAYERS); ++i)
{
TileCacheData* tile = &rc.tiles[rc.ntiles++];
const rcHeightfieldLayer* layer = &rc.lset->layers[i];
// Store header
dtTileCacheLayerHeader header;
header.magic = DT_TILECACHE_MAGIC;
header.version = DT_TILECACHE_VERSION;
// Tile layer location in the navmesh.
header.tx = tx;
header.ty = ty;
header.tlayer = i;
dtVcopy(header.bmin, layer->bmin);
dtVcopy(header.bmax, layer->bmax);
// Tile info.
header.width = (unsigned char)layer->width;
header.height = (unsigned char)layer->height;
header.minx = (unsigned char)layer->minx;
header.maxx = (unsigned char)layer->maxx;
header.miny = (unsigned char)layer->miny;
header.maxy = (unsigned char)layer->maxy;
header.hmin = (unsigned short)layer->hmin;
header.hmax = (unsigned short)layer->hmax;
dtStatus status = dtBuildTileCacheLayer(&comp, &header, layer->heights, layer->areas, layer->cons,
&tile->data, &tile->dataSize);
if (dtStatusFailed(status))
{
return 0;
}
}
// Transfer ownsership of tile data from build context to the caller.
int n = 0;
for (int i = 0; i < rcMin(rc.ntiles, maxTiles); ++i)
{
tiles[n++] = rc.tiles[i];
rc.tiles[i].data = 0;
rc.tiles[i].dataSize = 0;
}
return n;
}
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;
}
//-------------------------------------------------------------------------------------
int NavMeshHandle::findStraightPath(int layer, const Position3D& start, const Position3D& end, std::vector<Position3D>& paths)
{
std::map<int, NavmeshLayer>::iterator iter = navmeshLayer.find(layer);
if(iter == navmeshLayer.end())
{
ERROR_MSG(fmt::format("NavMeshHandle::findStraightPath: not found layer({})\n", layer));
return NAV_ERROR;
}
dtNavMeshQuery* navmeshQuery = iter->second.pNavmeshQuery;
// dtNavMesh*
float spos[3];
spos[0] = start.x;
spos[1] = start.y;
spos[2] = start.z;
float epos[3];
epos[0] = end.x;
epos[1] = end.y;
epos[2] = end.z;
dtQueryFilter filter;
filter.setIncludeFlags(0xffff);
filter.setExcludeFlags(0);
const float extents[3] = {2.f, 4.f, 2.f};
dtPolyRef startRef = INVALID_NAVMESH_POLYREF;
dtPolyRef endRef = INVALID_NAVMESH_POLYREF;
float startNearestPt[3];
float endNearestPt[3];
navmeshQuery->findNearestPoly(spos, extents, &filter, &startRef, startNearestPt);
navmeshQuery->findNearestPoly(epos, extents, &filter, &endRef, endNearestPt);
if (!startRef || !endRef)
{
ERROR_MSG(fmt::format("NavMeshHandle::findStraightPath({2}): Could not find any nearby poly's ({0}, {1})\n", startRef, endRef, resPath));
return NAV_ERROR_NEARESTPOLY;
}
dtPolyRef polys[MAX_POLYS];
int npolys;
float straightPath[MAX_POLYS * 3];
unsigned char straightPathFlags[MAX_POLYS];
dtPolyRef straightPathPolys[MAX_POLYS];
int nstraightPath;
int pos = 0;
navmeshQuery->findPath(startRef, endRef, startNearestPt, endNearestPt, &filter, polys, &npolys, MAX_POLYS);
nstraightPath = 0;
if (npolys)
{
float epos1[3];
dtVcopy(epos1, endNearestPt);
if (polys[npolys-1] != endRef)
navmeshQuery->closestPointOnPoly(polys[npolys-1], endNearestPt, epos1, 0);
navmeshQuery->findStraightPath(startNearestPt, endNearestPt, polys, npolys, straightPath, straightPathFlags, straightPathPolys, &nstraightPath, MAX_POLYS);
Position3D currpos;
for(int i = 0; i < nstraightPath * 3; )
{
currpos.x = straightPath[i++];
currpos.y = straightPath[i++];
currpos.z = straightPath[i++];
paths.push_back(currpos);
pos++;
//DEBUG_MSG(fmt::format("NavMeshHandle::findStraightPath: {}->{}, {}, {}\n", pos, currpos.x, currpos.y, currpos.z));
}
}
return pos;
}
void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 const& endPos)
{
// *** getting start/end poly logic ***
float distToStartPoly, distToEndPoly;
float startPoint[VERTEX_SIZE] = {startPos.y, startPos.z, startPos.x};
float endPoint[VERTEX_SIZE] = {endPos.y, endPos.z, endPos.x};
dtPolyRef startPoly = GetPolyByLocation(startPoint, &distToStartPoly);
dtPolyRef endPoly = GetPolyByLocation(endPoint, &distToEndPoly);
// we have a hole in our mesh
// make shortcut path and mark it as NOPATH ( with flying and swimming exception )
// its up to caller how he will use this info
if (startPoly == INVALID_POLYREF || endPoly == INVALID_POLYREF)
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPoly == 0 || endPoly == 0)\n");
BuildShortcut();
bool path = _sourceUnit->GetTypeId() == TYPEID_UNIT && _sourceUnit->ToCreature()->CanFly();
bool waterPath = _sourceUnit->GetTypeId() == TYPEID_UNIT && _sourceUnit->ToCreature()->CanSwim();
if (waterPath)
{
// Check both start and end points, if they're both in water, then we can *safely* let the creature move
for (uint32 i = 0; i < _pathPoints.size(); ++i)
{
ZLiquidStatus status = _sourceUnit->GetBaseMap()->getLiquidStatus(_pathPoints[i].x, _pathPoints[i].y, _pathPoints[i].z, MAP_ALL_LIQUIDS, NULL);
// One of the points is not in the water, cancel movement.
if (status == LIQUID_MAP_NO_WATER)
{
waterPath = false;
break;
}
}
}
_type = (path || waterPath) ? PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH) : PATHFIND_NOPATH;
return;
}
// we may need a better number here
bool farFromPoly = (distToStartPoly > 7.0f || distToEndPoly > 7.0f);
if (farFromPoly)
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f\n", distToStartPoly, distToEndPoly);
bool buildShotrcut = false;
if (_sourceUnit->GetTypeId() == TYPEID_UNIT)
{
Creature* owner = (Creature*)_sourceUnit;
G3D::Vector3 const& p = (distToStartPoly > 7.0f) ? startPos : endPos;
if (_sourceUnit->GetBaseMap()->IsUnderWater(p.x, p.y, p.z))
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: underWater case\n");
if (owner->CanSwim())
buildShotrcut = true;
}
else
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: flying case\n");
if (owner->CanFly())
buildShotrcut = true;
}
}
if (buildShotrcut)
{
BuildShortcut();
_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
return;
}
else
{
float closestPoint[VERTEX_SIZE];
// we may want to use closestPointOnPolyBoundary instead
if (dtStatusSucceed(_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint, NULL)))
{
dtVcopy(endPoint, closestPoint);
SetActualEndPosition(G3D::Vector3(endPoint[2], endPoint[0], endPoint[1]));
}
_type = PATHFIND_INCOMPLETE;
}
}
// *** poly path generating logic ***
// start and end are on same polygon
// just need to move in straight line
if (startPoly == endPoly)
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPoly == endPoly)\n");
BuildShortcut();
_pathPolyRefs[0] = startPoly;
_polyLength = 1;
_type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL;
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: path type %d\n", _type);
return;
}
// look for startPoly/endPoly in current path
/// @todo we can merge it with getPathPolyByPosition() loop
bool startPolyFound = false;
bool endPolyFound = false;
uint32 pathStartIndex = 0;
uint32 pathEndIndex = 0;
if (_polyLength)
{
for (; pathStartIndex < _polyLength; ++pathStartIndex)
{
// here to catch few bugs
if (_pathPolyRefs[pathStartIndex] == INVALID_POLYREF)
{
TC_LOG_ERROR("maps", "Invalid poly ref in BuildPolyPath. _polyLength: %u, pathStartIndex: %u,"
" startPos: %s, endPos: %s, mapid: %u",
_polyLength, pathStartIndex, startPos.toString().c_str(), endPos.toString().c_str(),
_sourceUnit->GetMapId());
break;
}
if (_pathPolyRefs[pathStartIndex] == startPoly)
{
startPolyFound = true;
break;
}
}
for (pathEndIndex = _polyLength-1; pathEndIndex > pathStartIndex; --pathEndIndex)
if (_pathPolyRefs[pathEndIndex] == endPoly)
{
endPolyFound = true;
break;
}
}
if (startPolyFound && endPolyFound)
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPolyFound && endPolyFound)\n");
// we moved along the path and the target did not move out of our old poly-path
// our path is a simple subpath case, we have all the data we need
// just "cut" it out
_polyLength = pathEndIndex - pathStartIndex + 1;
memmove(_pathPolyRefs, _pathPolyRefs + pathStartIndex, _polyLength * sizeof(dtPolyRef));
}
else if (startPolyFound && !endPolyFound)
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (startPolyFound && !endPolyFound)\n");
// we are moving on the old path but target moved out
// so we have atleast part of poly-path ready
_polyLength -= pathStartIndex;
// try to adjust the suffix of the path instead of recalculating entire length
// at given interval the target cannot get too far from its last location
// thus we have less poly to cover
// sub-path of optimal path is optimal
// take ~80% of the original length
/// @todo play with the values here
uint32 prefixPolyLength = uint32(_polyLength * 0.8f + 0.5f);
memmove(_pathPolyRefs, _pathPolyRefs+pathStartIndex, prefixPolyLength * sizeof(dtPolyRef));
dtPolyRef suffixStartPoly = _pathPolyRefs[prefixPolyLength-1];
// we need any point on our suffix start poly to generate poly-path, so we need last poly in prefix data
float suffixEndPoint[VERTEX_SIZE];
if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, NULL)))
{
// we can hit offmesh connection as last poly - closestPointOnPoly() don't like that
// try to recover by using prev polyref
--prefixPolyLength;
suffixStartPoly = _pathPolyRefs[prefixPolyLength-1];
if (dtStatusFailed(_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint, NULL)))
{
// suffixStartPoly is still invalid, error state
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
}
// generate suffix
uint32 suffixPolyLength = 0;
dtStatus dtResult;
if (_straightLine)
{
float hit = 0;
float hitNormal[3];
memset(hitNormal, 0, sizeof(hitNormal));
dtResult = _navMeshQuery->raycast(
suffixStartPoly,
suffixEndPoint,
endPoint,
&_filter,
&hit,
hitNormal,
_pathPolyRefs + prefixPolyLength - 1,
(int*)&suffixPolyLength,
MAX_PATH_LENGTH - prefixPolyLength);
// raycast() sets hit to FLT_MAX if there is a ray between start and end
if (hit != FLT_MAX)
{
// the ray hit something, return no path instead of the incomplete one
_type = PATHFIND_NOPATH;
return;
}
}
else
{
dtResult = _navMeshQuery->findPath(
suffixStartPoly, // start polygon
endPoly, // end polygon
suffixEndPoint, // start position
endPoint, // end position
&_filter, // polygon search filter
_pathPolyRefs + prefixPolyLength - 1, // [out] path
(int*)&suffixPolyLength,
MAX_PATH_LENGTH - prefixPolyLength); // max number of polygons in output path
}
if (!suffixPolyLength || dtStatusFailed(dtResult))
{
// this is probably an error state, but we'll leave it
// and hopefully recover on the next Update
// we still need to copy our preffix
TC_LOG_ERROR("maps", "%u's Path Build failed: 0 length path", _sourceUnit->GetGUIDLow());
}
TC_LOG_DEBUG("maps", "++ m_polyLength=%u prefixPolyLength=%u suffixPolyLength=%u \n", _polyLength, prefixPolyLength, suffixPolyLength);
// new path = prefix + suffix - overlap
_polyLength = prefixPolyLength + suffixPolyLength - 1;
}
else
{
TC_LOG_DEBUG("maps", "++ BuildPolyPath :: (!startPolyFound && !endPolyFound)\n");
// either we have no path at all -> first run
// or something went really wrong -> we aren't moving along the path to the target
// just generate new path
// free and invalidate old path data
Clear();
dtStatus dtResult;
if (_straightLine)
{
float hit = 0;
float hitNormal[3];
memset(hitNormal, 0, sizeof(hitNormal));
dtResult = _navMeshQuery->raycast(
startPoly,
startPoint,
endPoint,
&_filter,
&hit,
hitNormal,
_pathPolyRefs,
(int*)&_polyLength,
MAX_PATH_LENGTH);
// raycast() sets hit to FLT_MAX if there is a ray between start and end
if (hit != FLT_MAX)
{
// the ray hit something, return no path instead of the incomplete one
_type = PATHFIND_NOPATH;
return;
}
}
else
{
dtResult = _navMeshQuery->findPath(
startPoly, // start polygon
endPoly, // end polygon
startPoint, // start position
endPoint, // end position
&_filter, // polygon search filter
_pathPolyRefs, // [out] path
(int*)&_polyLength,
MAX_PATH_LENGTH); // max number of polygons in output path
}
if (!_polyLength || dtStatusFailed(dtResult))
{
// only happens if we passed bad data to findPath(), or navmesh is messed up
TC_LOG_ERROR("maps", "%u's Path Build failed: 0 length path", _sourceUnit->GetGUIDLow());
BuildShortcut();
_type = PATHFIND_NOPATH;
return;
}
}
// by now we know what type of path we can get
if (_pathPolyRefs[_polyLength - 1] == endPoly && !(_type & PATHFIND_INCOMPLETE))
_type = PATHFIND_NORMAL;
else
_type = PATHFIND_INCOMPLETE;
// generate the point-path out of our up-to-date poly-path
BuildPointPath(startPoint, endPoint);
}
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 dtClosestPtPointTriangle(float* closest, const float* p,
const float* a, const float* b, const float* c)
{
// Check if P in vertex region outside A
float ab[3], ac[3], ap[3];
dtVsub(ab, b, a);
dtVsub(ac, c, a);
dtVsub(ap, p, a);
float d1 = dtVdot(ab, ap);
float d2 = dtVdot(ac, ap);
if (d1 <= 0.0f && d2 <= 0.0f)
{
// barycentric coordinates (1,0,0)
dtVcopy(closest, a);
return;
}
// Check if P in vertex region outside B
float bp[3];
dtVsub(bp, p, b);
float d3 = dtVdot(ab, bp);
float d4 = dtVdot(ac, bp);
if (d3 >= 0.0f && d4 <= d3)
{
// barycentric coordinates (0,1,0)
dtVcopy(closest, b);
return;
}
// Check if P in edge region of AB, if so return projection of P onto AB
float vc = d1*d4 - d3*d2;
if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
{
// barycentric coordinates (1-v,v,0)
float v = d1 / (d1 - d3);
closest[0] = a[0] + v * ab[0];
closest[1] = a[1] + v * ab[1];
closest[2] = a[2] + v * ab[2];
return;
}
// Check if P in vertex region outside C
float cp[3];
dtVsub(cp, p, c);
float d5 = dtVdot(ab, cp);
float d6 = dtVdot(ac, cp);
if (d6 >= 0.0f && d5 <= d6)
{
// barycentric coordinates (0,0,1)
dtVcopy(closest, c);
return;
}
// Check if P in edge region of AC, if so return projection of P onto AC
float vb = d5*d2 - d1*d6;
if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
{
// barycentric coordinates (1-w,0,w)
float w = d2 / (d2 - d6);
closest[0] = a[0] + w * ac[0];
closest[1] = a[1] + w * ac[1];
closest[2] = a[2] + w * ac[2];
return;
}
// Check if P in edge region of BC, if so return projection of P onto BC
float va = d3*d6 - d5*d4;
if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
{
// barycentric coordinates (0,1-w,w)
float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
closest[0] = b[0] + w * (c[0] - b[0]);
closest[1] = b[1] + w * (c[1] - b[1]);
closest[2] = b[2] + w * (c[2] - b[2]);
return;
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
float denom = 1.0f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
closest[0] = a[0] + ab[0] * v + ac[0] * w;
closest[1] = a[1] + ab[1] * v + ac[1] * w;
closest[2] = a[2] + ab[2] * v + ac[2] * w;
}
void dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel, float* nvel,
const int ndivs, const int nrings, const int depth,
dtObstacleAvoidanceDebugData* debug)
{
prepare(pos, dvel);
dtVset(nvel, 0,0,0);
if (debug)
debug->reset();
// Build sampling pattern aligned to desired velocity.
static const int MAX_PATTERN_DIVS = 32;
static const int MAX_PATTERN_RINGS = 4;
float pat[(MAX_PATTERN_DIVS*MAX_PATTERN_RINGS+1)*2];
int npat = 0;
const int nd = dtClamp(ndivs, 1, MAX_PATTERN_DIVS);
const int nr = dtClamp(nrings, 1, MAX_PATTERN_RINGS);
const float da = (1.0f/nd) * DT_PI*2;
const float dang = atan2f(dvel[2], dvel[0]);
// Always add sample at zero
pat[npat*2+0] = 0;
pat[npat*2+1] = 0;
npat++;
for (int j = 0; j < nr; ++j)
{
const float rad = (float)(nr-j)/(float)nr;
float a = dang + (j&1)*0.5f*da;
for (int i = 0; i < nd; ++i)
{
pat[npat*2+0] = cosf(a)*rad;
pat[npat*2+1] = sinf(a)*rad;
npat++;
a += da;
}
}
// Start sampling.
float cr = vmax * (1.0f-m_velBias);
float res[3];
dtVset(res, dvel[0] * m_velBias, 0, dvel[2] * m_velBias);
for (int k = 0; k < depth; ++k)
{
float minPenalty = FLT_MAX;
float bvel[3];
dtVset(bvel, 0,0,0);
for (int i = 0; i < npat; ++i)
{
float vcand[3];
vcand[0] = res[0] + pat[i*2+0]*cr;
vcand[1] = 0;
vcand[2] = res[2] + pat[i*2+1]*cr;
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
const float penalty = processSample(vcand,cr/10, pos,rad,vmax,vel,dvel, debug);
if (penalty < minPenalty)
{
minPenalty = penalty;
dtVcopy(bvel, vcand);
}
}
dtVcopy(res, bvel);
cr *= 0.5f;
}
dtVcopy(nvel, res);
}
void TestCase::doTests(dtNavMesh* navmesh, dtNavMeshQuery* navquery)
{
if (!navmesh || !navquery)
return;
resetTimes();
static const int MAX_POLYS = 256;
dtPolyRef polys[MAX_POLYS];
float straight[MAX_POLYS*3];
const float polyPickExt[3] = {2,4,2};
for (Test* iter = m_tests; iter; iter = iter->next)
{
delete [] iter->polys;
iter->polys = 0;
iter->npolys = 0;
delete [] iter->straight;
iter->straight = 0;
iter->nstraight = 0;
dtQueryFilter filter;
filter.setIncludeFlags(iter->includeFlags);
filter.setExcludeFlags(iter->excludeFlags);
// Find start points
TimeVal findNearestPolyStart = getPerfTime();
dtPolyRef startRef, endRef;
navquery->findNearestPoly(iter->spos, polyPickExt, &filter, &startRef, iter->nspos);
navquery->findNearestPoly(iter->epos, polyPickExt, &filter, &endRef, iter->nepos);
TimeVal findNearestPolyEnd = getPerfTime();
iter->findNearestPolyTime += getPerfTimeUsec(findNearestPolyEnd - findNearestPolyStart);
if (!startRef || ! endRef)
continue;
if (iter->type == TEST_PATHFIND)
{
// Find path
TimeVal findPathStart = getPerfTime();
navquery->findPath(startRef, endRef, iter->spos, iter->epos, &filter, polys, &iter->npolys, MAX_POLYS);
TimeVal findPathEnd = getPerfTime();
iter->findPathTime += getPerfTimeUsec(findPathEnd - findPathStart);
// Find straight path
if (iter->npolys)
{
TimeVal findStraightPathStart = getPerfTime();
navquery->findStraightPath(iter->spos, iter->epos, polys, iter->npolys,
straight, 0, 0, &iter->nstraight, MAX_POLYS);
TimeVal findStraightPathEnd = getPerfTime();
iter->findStraightPathTime += getPerfTimeUsec(findStraightPathEnd - findStraightPathStart);
}
// Copy results
if (iter->npolys)
{
iter->polys = new dtPolyRef[iter->npolys];
memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys);
}
if (iter->nstraight)
{
iter->straight = new float[iter->nstraight*3];
memcpy(iter->straight, straight, sizeof(float)*3*iter->nstraight);
}
}
else if (iter->type == TEST_RAYCAST)
{
float t = 0;
float hitNormal[3], hitPos[3];
iter->straight = new float[2*3];
iter->nstraight = 2;
iter->straight[0] = iter->spos[0];
iter->straight[1] = iter->spos[1];
iter->straight[2] = iter->spos[2];
TimeVal findPathStart = getPerfTime();
navquery->raycast(startRef, iter->spos, iter->epos, &filter, &t, hitNormal, polys, &iter->npolys, MAX_POLYS);
TimeVal findPathEnd = getPerfTime();
iter->findPathTime += getPerfTimeUsec(findPathEnd - findPathStart);
if (t > 1)
{
// No hit
dtVcopy(hitPos, iter->epos);
}
else
{
// Hit
dtVlerp(hitPos, iter->spos, iter->epos, t);
}
// Adjust height.
if (iter->npolys > 0)
{
float h = 0;
navquery->getPolyHeight(polys[iter->npolys-1], hitPos, &h);
hitPos[1] = h;
}
dtVcopy(&iter->straight[3], hitPos);
if (iter->npolys)
{
iter->polys = new dtPolyRef[iter->npolys];
memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys);
}
}
}
printf("Test Results:\n");
int n = 0;
for (Test* iter = m_tests; iter; iter = iter->next)
{
const int total = iter->findNearestPolyTime + iter->findPathTime + iter->findStraightPathTime;
printf(" - Path %02d: %.4f ms\n", n, (float)total/1000.0f);
printf(" - poly: %.4f ms\n", (float)iter->findNearestPolyTime/1000.0f);
printf(" - path: %.4f ms\n", (float)iter->findPathTime/1000.0f);
printf(" - straight: %.4f ms\n", (float)iter->findStraightPathTime/1000.0f);
n++;
}
}
int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel, float* nvel,
const dtObstacleAvoidanceParams* params,
dtObstacleAvoidanceDebugData* debug)
{
prepare(pos, dvel);
memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = 1.0f / vmax;
dtVset(nvel, 0,0,0);
if (debug)
debug->reset();
// Build sampling pattern aligned to desired velocity.
float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2];
int npat = 0;
const int ndivs = (int)m_params.adaptiveDivs;
const int nrings= (int)m_params.adaptiveRings;
const int depth = (int)m_params.adaptiveDepth;
const int nd = dtClamp(ndivs, 1, DT_MAX_PATTERN_DIVS);
const int nr = dtClamp(nrings, 1, DT_MAX_PATTERN_RINGS);
const float da = (1.0f/nd) * DT_PI*2;
const float dang = atan2f(dvel[2], dvel[0]);
// Always add sample at zero
pat[npat*2+0] = 0;
pat[npat*2+1] = 0;
npat++;
for (int j = 0; j < nr; ++j)
{
const float r = (float)(nr-j)/(float)nr;
float a = dang + (j&1)*0.5f*da;
for (int i = 0; i < nd; ++i)
{
pat[npat*2+0] = cosf(a)*r;
pat[npat*2+1] = sinf(a)*r;
npat++;
a += da;
}
}
// Start sampling.
float cr = vmax * (1.0f - m_params.velBias);
float res[3];
dtVset(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias);
int ns = 0;
for (int k = 0; k < depth; ++k)
{
float minPenalty = FLT_MAX;
float bvel[3];
dtVset(bvel, 0,0,0);
for (int i = 0; i < npat; ++i)
{
float vcand[3];
vcand[0] = res[0] + pat[i*2+0]*cr;
vcand[1] = 0;
vcand[2] = res[2] + pat[i*2+1]*cr;
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
dtVcopy(bvel, vcand);
}
}
dtVcopy(res, bvel);
cr *= 0.5f;
}
dtVcopy(nvel, res);
return ns;
}
void CrowdTool::handleRender()
{
DebugDrawGL dd;
const float s = m_sample->getAgentRadius();
dtNavMesh* nmesh = m_sample->getNavMesh();
if (!nmesh)
return;
dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
if (m_showNodes)
{
if (navquery)
duDebugDrawNavMeshNodes(&dd, *navquery);
}
dd.depthMask(false);
// Draw paths
if (m_showPath)
{
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount();
for (int i = 0; i < npath; ++i)
duDebugDrawNavMeshPoly(&dd, *nmesh, path[i], duRGBA(0,0,0,32));
}
}
if (m_targetRef)
duDebugDrawCross(&dd, m_targetPos[0],m_targetPos[1]+0.1f,m_targetPos[2], s, duRGBA(255,255,255,192), 2.0f);
// Occupancy grid.
if (m_showGrid)
{
float gridy = -FLT_MAX;
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float* pos = ag->corridor.getPos();
gridy = dtMax(gridy, pos[1]);
}
gridy += 1.0f;
dd.begin(DU_DRAW_QUADS);
const ProximityGrid* grid = m_crowd.getGrid();
const int* bounds = grid->getBounds();
const float cs = grid->getCellSize();
for (int y = bounds[1]; y <= bounds[3]; ++y)
{
for (int x = bounds[0]; x <= bounds[2]; ++x)
{
const int count = grid->getItemCountAt(x,y);
if (!count) continue;
unsigned int col = duRGBA(128,0,0,dtMin(count*40,255));
dd.vertex(x*cs, gridy, y*cs, col);
dd.vertex(x*cs, gridy, y*cs+cs, col);
dd.vertex(x*cs+cs, gridy, y*cs+cs, col);
dd.vertex(x*cs+cs, gridy, y*cs, col);
}
}
dd.end();
}
// Trail
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float* pos = ag->npos;
dd.begin(DU_DRAW_LINES,3.0f);
float prev[3], preva = 1;
dtVcopy(prev, pos);
for (int j = 0; j < AGENT_MAX_TRAIL-1; ++j)
{
const int idx = (ag->htrail + AGENT_MAX_TRAIL-j) % AGENT_MAX_TRAIL;
const float* v = &ag->trail[idx*3];
float a = 1 - j/(float)AGENT_MAX_TRAIL;
dd.vertex(prev[0],prev[1]+0.1f,prev[2], duRGBA(0,0,0,(int)(128*preva)));
dd.vertex(v[0],v[1]+0.1f,v[2], duRGBA(0,0,0,(int)(128*a)));
preva = a;
dtVcopy(prev, v);
}
dd.end();
}
// Corners & co
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float radius = ag->radius;
const float* pos = ag->npos;
if (m_showCorners)
{
if (ag->ncorners)
{
dd.begin(DU_DRAW_LINES, 2.0f);
for (int j = 0; j < ag->ncorners; ++j)
{
const float* va = j == 0 ? pos : &ag->cornerVerts[(j-1)*3];
const float* vb = &ag->cornerVerts[j*3];
dd.vertex(va[0],va[1]+radius,va[2], duRGBA(128,0,0,192));
dd.vertex(vb[0],vb[1]+radius,vb[2], duRGBA(128,0,0,192));
}
dd.end();
if (m_anticipateTurns)
{
/* float dvel[3], pos[3];
calcSmoothSteerDirection(ag->pos, ag->cornerVerts, ag->ncorners, dvel);
pos[0] = ag->pos[0] + dvel[0];
pos[1] = ag->pos[1] + dvel[1];
pos[2] = ag->pos[2] + dvel[2];
const float off = ag->radius+0.1f;
const float* tgt = &ag->cornerVerts[0];
const float y = ag->pos[1]+off;
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(ag->pos[0],y,ag->pos[2], duRGBA(255,0,0,192));
dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
dd.vertex(pos[0],y,pos[2], duRGBA(255,0,0,192));
dd.vertex(tgt[0],y,tgt[2], duRGBA(255,0,0,192));
dd.end();*/
}
}
}
if (m_showCollisionSegments)
{
const float* center = ag->boundary.getCenter();
duDebugDrawCross(&dd, center[0],center[1]+radius,center[2], 0.2f, duRGBA(192,0,128,255), 2.0f);
duDebugDrawCircle(&dd, center[0],center[1]+radius,center[2], ag->collisionQueryRange,
duRGBA(192,0,128,128), 2.0f);
dd.begin(DU_DRAW_LINES, 3.0f);
for (int j = 0; j < ag->boundary.getSegmentCount(); ++j)
{
const float* s = ag->boundary.getSegment(j);
unsigned int col = duRGBA(192,0,128,192);
if (dtTriArea2D(pos, s, s+3) < 0.0f)
col = duDarkenCol(col);
duAppendArrow(&dd, s[0],s[1]+0.2f,s[2], s[3],s[4]+0.2f,s[5], 0.0f, 0.3f, col);
}
dd.end();
}
if (m_showOpt)
{
dd.begin(DU_DRAW_LINES, 2.0f);
dd.vertex(ag->opts[0],ag->opts[1]+0.3f,ag->opts[2], duRGBA(0,128,0,192));
dd.vertex(ag->opte[0],ag->opte[1]+0.3f,ag->opte[2], duRGBA(0,128,0,192));
dd.end();
}
}
// Agent cylinders.
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float radius = ag->radius;
const float* pos = ag->npos;
duDebugDrawCircle(&dd, pos[0], pos[1], pos[2], radius, duRGBA(0,0,0,32), 2.0f);
}
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float height = ag->height;
const float radius = ag->radius;
const float* pos = ag->npos;
duDebugDrawCylinder(&dd, pos[0]-radius, pos[1]+radius*0.1f, pos[2]-radius,
pos[0]+radius, pos[1]+height, pos[2]+radius,
duRGBA(220,220,220,128));
}
// Velocity stuff.
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float radius = ag->radius;
const float height = ag->height;
const float* pos = ag->npos;
const float* vel = ag->vel;
const float* dvel = ag->dvel;
duDebugDrawCircle(&dd, pos[0], pos[1]+height, pos[2], radius, duRGBA(220,220,220,192), 2.0f);
if (m_showVO)
{
// Draw detail about agent sela
const dtObstacleAvoidanceDebugData* debug = m_crowd.getVODebugData(i);
const float dx = pos[0];
const float dy = pos[1]+height;
const float dz = pos[2];
dd.begin(DU_DRAW_QUADS);
for (int i = 0; i < debug->getSampleCount(); ++i)
{
const float* p = debug->getSampleVelocity(i);
const float sr = debug->getSampleSize(i);
const float pen = debug->getSamplePenalty(i);
const float pen2 = debug->getSamplePreferredSidePenalty(i);
unsigned int col = duLerpCol(duRGBA(255,255,255,220), duRGBA(128,96,0,220), (int)(pen*255));
col = duLerpCol(col, duRGBA(128,0,0,220), (int)(pen2*128));
dd.vertex(dx+p[0]-sr, dy, dz+p[2]-sr, col);
dd.vertex(dx+p[0]-sr, dy, dz+p[2]+sr, col);
dd.vertex(dx+p[0]+sr, dy, dz+p[2]+sr, col);
dd.vertex(dx+p[0]+sr, dy, dz+p[2]-sr, col);
}
dd.end();
}
duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
pos[0]+dvel[0],pos[1]+height+dvel[1],pos[2]+dvel[2],
0.0f, 0.4f, duRGBA(0,192,255,192), 1.0f);
duDebugDrawArrow(&dd, pos[0],pos[1]+height,pos[2],
pos[0]+vel[0],pos[1]+height+vel[1],pos[2]+vel[2],
0.0f, 0.4f, duRGBA(0,0,0,192), 2.0f);
}
// Targets
for (int i = 0; i < m_crowd.getAgentCount(); ++i)
{
const Agent* ag = m_crowd.getAgent(i);
if (!ag->active) continue;
const float* pos = ag->npos;
const float* target = ag->corridor.getTarget();
if (m_showTargets)
{
duDebugDrawArc(&dd, pos[0], pos[1], pos[2], target[0], target[1], target[2],
0.25f, 0, 0.4f, duRGBA(0,0,0,128), 1.0f);
}
}
dd.depthMask(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 dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side)
{
if (!tile) return;
// Connect off-mesh links.
// We are interested on links which land from target tile to this tile.
const unsigned char oppositeSide = (side == -1) ? 0xff : (unsigned char)dtOppositeTile(side);
for (int i = 0; i < target->header->offMeshConCount; ++i)
{
dtOffMeshConnection* targetCon = &target->offMeshCons[i];
if (targetCon->side != oppositeSide)
continue;
dtPoly* targetPoly = &target->polys[targetCon->poly];
// Skip off-mesh connections which start location could not be connected at all.
if (targetPoly->firstLink == DT_NULL_LINK)
continue;
const float ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad };
// Find polygon to connect to.
const float* p = &targetCon->pos[3];
float nearestPt[3];
dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt);
if (!ref)
continue;
// findNearestPoly may return too optimistic results, further check to make sure.
if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(targetCon->rad))
continue;
// Make sure the location is on current mesh.
float* v = &target->verts[targetPoly->verts[1]*3];
dtVcopy(v, nearestPt);
// Link off-mesh connection to target poly.
unsigned int idx = allocLink(target);
if (idx != DT_NULL_LINK)
{
dtLink* link = &target->links[idx];
link->ref = ref;
link->edge = (unsigned char)1;
link->side = oppositeSide;
link->bmin = link->bmax = 0;
// Add to linked list.
link->next = targetPoly->firstLink;
targetPoly->firstLink = idx;
}
// Link target poly to off-mesh connection.
if (targetCon->flags & DT_OFFMESH_CON_BIDIR)
{
unsigned int tidx = allocLink(tile);
if (tidx != DT_NULL_LINK)
{
const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref);
dtPoly* landPoly = &tile->polys[landPolyIdx];
dtLink* link = &tile->links[tidx];
link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly);
link->edge = 0xff;
link->side = (unsigned char)(side == -1 ? 0xff : side);
link->bmin = link->bmax = 0;
// Add to linked list.
link->next = landPoly->firstLink;
landPoly->firstLink = tidx;
}
}
}
}
void dtCrowd::updateMoveRequest(const float /*dt*/)
{
const int PATH_MAX_AGENTS = 8;
dtCrowdAgent* queue[PATH_MAX_AGENTS];
int nqueue = 0;
// Fire off new requests.
for (int i = 0; i < m_maxAgents; ++i)
{
dtCrowdAgent* ag = &m_agents[i];
if (!ag->active)
continue;
if (ag->state == DT_CROWDAGENT_STATE_INVALID)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_REQUESTING)
{
const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount();
dtAssert(npath);
static const int MAX_RES = 32;
float reqPos[3];
dtPolyRef reqPath[MAX_RES]; // The path to the request location
int reqPathCount = 0;
// Quick seach towards the goal.
static const int MAX_ITER = 20;
m_navquery->initSlicedFindPath(path[0], ag->targetRef, ag->npos, ag->targetPos, &m_filter);
m_navquery->updateSlicedFindPath(MAX_ITER, 0);
dtStatus status = 0;
if (ag->targetReplan) // && npath > 10)
{
// Try to use existing steady path during replan if possible.
status = m_navquery->finalizeSlicedFindPathPartial(path, npath, reqPath, &reqPathCount, MAX_RES);
}
else
{
// Try to move towards target when goal changes.
status = m_navquery->finalizeSlicedFindPath(reqPath, &reqPathCount, MAX_RES);
}
if (!dtStatusFailed(status) && reqPathCount > 0)
{
// In progress or succeed.
if (reqPath[reqPathCount-1] != ag->targetRef)
{
// Partial path, constrain target position inside the last polygon.
status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos, 0);
if (dtStatusFailed(status))
reqPathCount = 0;
}
else
{
dtVcopy(reqPos, ag->targetPos);
}
}
else
{
reqPathCount = 0;
}
if (!reqPathCount)
{
// Could not find path, start the request from current location.
dtVcopy(reqPos, ag->npos);
reqPath[0] = path[0];
reqPathCount = 1;
}
ag->corridor.setCorridor(reqPos, reqPath, reqPathCount);
ag->boundary.reset();
if (reqPath[reqPathCount-1] == ag->targetRef)
{
ag->targetState = DT_CROWDAGENT_TARGET_VALID;
ag->targetReplanTime = 0.0;
}
else
{
// The path is longer or potentially unreachable, full plan.
ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE;
}
}
if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE)
{
nqueue = addToPathQueue(ag, queue, nqueue, PATH_MAX_AGENTS);
}
}
for (int i = 0; i < nqueue; ++i)
{
dtCrowdAgent* ag = queue[i];
ag->targetPathqRef = m_pathq.request(ag->corridor.getLastPoly(), ag->targetRef,
ag->corridor.getTarget(), ag->targetPos, &m_filter);
if (ag->targetPathqRef != DT_PATHQ_INVALID)
ag->targetState = DT_CROWDAGENT_TARGET_WAITING_FOR_PATH;
}
// Update requests.
m_pathq.update(MAX_ITERS_PER_UPDATE);
dtStatus status;
// Process path results.
for (int i = 0; i < m_maxAgents; ++i)
{
dtCrowdAgent* ag = &m_agents[i];
if (!ag->active)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_NONE || ag->targetState == DT_CROWDAGENT_TARGET_VELOCITY)
continue;
if (ag->targetState == DT_CROWDAGENT_TARGET_WAITING_FOR_PATH)
{
// Poll path queue.
status = m_pathq.getRequestStatus(ag->targetPathqRef);
if (dtStatusFailed(status))
{
// Path find failed, retry if the target location is still valid.
ag->targetPathqRef = DT_PATHQ_INVALID;
if (ag->targetRef)
ag->targetState = DT_CROWDAGENT_TARGET_REQUESTING;
else
ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
ag->targetReplanTime = 0.0;
}
else if (dtStatusSucceed(status))
{
const dtPolyRef* path = ag->corridor.getPath();
const int npath = ag->corridor.getPathCount();
dtAssert(npath);
// Apply results.
float targetPos[3];
dtVcopy(targetPos, ag->targetPos);
dtPolyRef* res = m_pathResult;
bool valid = true;
int nres = 0;
status = m_pathq.getPathResult(ag->targetPathqRef, res, &nres, m_maxPathResult);
if (dtStatusFailed(status) || !nres)
valid = false;
// Merge result and existing path.
// The agent might have moved whilst the request is
// being processed, so the path may have changed.
// We assume that the end of the path is at the same location
// where the request was issued.
// The last ref in the old path should be the same as
// the location where the request was issued..
if (valid && path[npath-1] != res[0])
valid = false;
if (valid)
{
// Put the old path infront of the old path.
if (npath > 1)
{
// Make space for the old path.
if ((npath-1)+nres > m_maxPathResult)
nres = m_maxPathResult - (npath-1);
memmove(res+npath-1, res, sizeof(dtPolyRef)*nres);
// Copy old path in the beginning.
memcpy(res, path, sizeof(dtPolyRef)*(npath-1));
nres += npath-1;
// Remove trackbacks
for (int j = 0; j < nres; ++j)
{
if (j-1 >= 0 && j+1 < nres)
{
if (res[j-1] == res[j+1])
{
memmove(res+(j-1), res+(j+1), sizeof(dtPolyRef)*(nres-(j+1)));
nres -= 2;
j -= 2;
}
}
}
}
// Check for partial path.
if (res[nres-1] != ag->targetRef)
{
// Partial path, constrain target position inside the last polygon.
float nearest[3];
status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest, 0);
if (dtStatusSucceed(status))
dtVcopy(targetPos, nearest);
else
valid = false;
}
}
if (valid)
{
// Set current corridor.
ag->corridor.setCorridor(targetPos, res, nres);
// Force to update boundary.
ag->boundary.reset();
ag->targetState = DT_CROWDAGENT_TARGET_VALID;
}
else
{
// Something went wrong.
ag->targetState = DT_CROWDAGENT_TARGET_FAILED;
}
ag->targetReplanTime = 0.0;
}
}
}
}
int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
dtPolyRef* polys, const int maxPolys) const
{
if (tile->bvTree)
{
const dtBVNode* node = &tile->bvTree[0];
const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount];
const float* tbmin = tile->header->bmin;
const float* tbmax = tile->header->bmax;
const float qfac = tile->header->bvQuantFactor;
// Calculate quantized box
unsigned short bmin[3], bmax[3];
// dtClamp query box to world box.
float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
// Quantize
bmin[0] = (unsigned short)(qfac * minx) & 0xfffe;
bmin[1] = (unsigned short)(qfac * miny) & 0xfffe;
bmin[2] = (unsigned short)(qfac * minz) & 0xfffe;
bmax[0] = (unsigned short)(qfac * maxx + 1) | 1;
bmax[1] = (unsigned short)(qfac * maxy + 1) | 1;
bmax[2] = (unsigned short)(qfac * maxz + 1) | 1;
// Traverse tree
dtPolyRef base = getPolyRefBase(tile);
int n = 0;
while (node < end)
{
const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax);
const bool isLeafNode = node->i >= 0;
if (isLeafNode && overlap)
{
if (n < maxPolys)
polys[n++] = base | (dtPolyRef)node->i;
}
if (overlap || isLeafNode)
node++;
else
{
const int escapeIndex = -node->i;
node += escapeIndex;
}
}
return n;
}
else
{
float bmin[3], bmax[3];
int n = 0;
dtPolyRef base = getPolyRefBase(tile);
for (int i = 0; i < tile->header->polyCount; ++i)
{
dtPoly* p = &tile->polys[i];
// Do not return off-mesh connection polygons.
if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
// Calc polygon bounds.
const float* v = &tile->verts[p->verts[0]*3];
dtVcopy(bmin, v);
dtVcopy(bmax, v);
for (int j = 1; j < p->vertCount; ++j)
{
v = &tile->verts[p->verts[j]*3];
dtVmin(bmin, v);
dtVmax(bmax, v);
}
if (dtOverlapBounds(qmin,qmax, bmin,bmax))
{
if (n < maxPolys)
polys[n++] = base | (dtPolyRef)i;
}
}
return n;
}
}
void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const float dt)
{
static const int CHECK_LOOKAHEAD = 10;
static const float TARGET_REPLAN_DELAY = 1.0; // seconds
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;
ag->targetReplanTime += dt;
bool replan = false;
// First check that the current location is valid.
const int idx = getAgentIndex(ag);
float agentPos[3];
dtPolyRef agentRef = ag->corridor.getFirstPoly();
dtVcopy(agentPos, ag->npos);
if (!m_navquery->isValidPolyRef(agentRef, &m_filter))
{
// Current location is not valid, try to reposition.
// TODO: this can snap agents, how to handle that?
float nearest[3];
dtVcopy(nearest, agentPos);
agentRef = 0;
m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
dtVcopy(agentPos, nearest);
if (!agentRef)
{
// Could not find location in navmesh, set state to invalid.
ag->corridor.reset(0, agentPos);
ag->boundary.reset();
ag->state = DT_CROWDAGENT_STATE_INVALID;
continue;
}
// Make sure the first polygon is valid, but leave other valid
// polygons in the path so that replanner can adjust the path better.
ag->corridor.fixPathStart(agentRef, agentPos);
// ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
ag->boundary.reset();
dtVcopy(ag->npos, agentPos);
replan = true;
}
// Try to recover move request position.
if (ag->targetState != DT_CROWDAGENT_TARGET_NONE && ag->targetState != DT_CROWDAGENT_TARGET_FAILED)
{
if (!m_navquery->isValidPolyRef(ag->targetRef, &m_filter))
{
// Current target is not valid, try to reposition.
float nearest[3];
dtVcopy(nearest, ag->targetPos);
ag->targetRef = 0;
m_navquery->findNearestPoly(ag->targetPos, m_ext, &m_filter, &ag->targetRef, nearest);
dtVcopy(ag->targetPos, nearest);
replan = true;
}
if (!ag->targetRef)
{
// Failed to reposition target, fail moverequest.
ag->corridor.reset(agentRef, agentPos);
ag->targetState = DT_CROWDAGENT_TARGET_NONE;
}
}
// If nearby corridor is not valid, replan.
if (!ag->corridor.isValid(CHECK_LOOKAHEAD, m_navquery, &m_filter))
{
// Fix current path.
// ag->corridor.trimInvalidPath(agentRef, agentPos, m_navquery, &m_filter);
// ag->boundary.reset();
replan = true;
}
// If the end of the path is near and it is not the requested location, replan.
if (ag->targetState == DT_CROWDAGENT_TARGET_VALID)
{
if (ag->targetReplanTime > TARGET_REPLAN_DELAY &&
ag->corridor.getPathCount() < CHECK_LOOKAHEAD &&
ag->corridor.getLastPoly() != ag->targetRef)
replan = true;
}
// Try to replan path to goal.
if (replan)
{
if (ag->targetState != DT_CROWDAGENT_TARGET_NONE)
{
requestMoveTargetReplan(idx, ag->targetRef, ag->targetPos);
}
}
}
}
void PathInfo::BuildPolyPath(PathNode startPos, PathNode endPos)
{
// *** getting start/end poly logic ***
float distToStartPoly, distToEndPoly;
float startPoint[VERTEX_SIZE] = {startPos.y, startPos.z, startPos.x};
float endPoint[VERTEX_SIZE] = {endPos.y, endPos.z, endPos.x};
dtPolyRef startPoly = getPolyByLocation(startPoint, &distToStartPoly);
dtPolyRef endPoly = getPolyByLocation(endPoint, &distToEndPoly);
// we have a hole in our mesh
// make shortcut path and mark it as NOPATH ( with flying exception )
// its up to caller how he will use this info
if (startPoly == INVALID_POLYREF || endPoly == INVALID_POLYREF)
{
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: (startPoly == 0 || endPoly == 0)\n");
BuildShortcut();
m_type = (m_sourceUnit->GetTypeId() == TYPEID_UNIT && ((Creature*)m_sourceUnit)->CanFly())
? PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH) : PATHFIND_NOPATH;
return;
}
// we may need a better number here
bool farFromPoly = (distToStartPoly > 7.0f || distToEndPoly > 7.0f);
if (farFromPoly)
{
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f\n", distToStartPoly, distToEndPoly);
bool buildShotrcut = false;
if (m_sourceUnit->GetTypeId() == TYPEID_UNIT)
{
Creature* owner = (Creature*)m_sourceUnit;
PathNode p = (distToStartPoly > 7.0f) ? startPos : endPos;
if (m_sourceUnit->GetTerrain()->IsUnderWater(p.x, p.y, p.z))
{
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: underWater case\n");
if (owner->CanSwim())
buildShotrcut = true;
}
else
{
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: flying case\n");
if (owner->CanFly())
buildShotrcut = true;
}
}
if (buildShotrcut)
{
BuildShortcut();
m_type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
return;
}
else
{
float closestPoint[VERTEX_SIZE];
// we may want to use closestPointOnPolyBoundary instead
if (DT_SUCCESS == m_navMeshQuery->closestPointOnPoly(endPoly, endPoint, closestPoint))
{
dtVcopy(endPoint, closestPoint);
setActualEndPosition(PathNode(endPoint[2],endPoint[0],endPoint[1]));
}
m_type = PATHFIND_INCOMPLETE;
}
}
// *** poly path generating logic ***
// start and end are on same polygon
// just need to move in straight line
if (startPoly == endPoly)
{
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: (startPoly == endPoly)\n");
BuildShortcut();
m_pathPolyRefs[0] = startPoly;
m_polyLength = 1;
m_type = farFromPoly ? PATHFIND_INCOMPLETE : PATHFIND_NORMAL;
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: path type %d\n", m_type);
return;
}
// look for startPoly/endPoly in current path
// TODO: we can merge it with getPathPolyByPosition() loop
bool startPolyFound = false;
bool endPolyFound = false;
uint32 pathStartIndex, pathEndIndex;
if (m_polyLength)
{
for (pathStartIndex = 0; pathStartIndex < m_polyLength; ++pathStartIndex)
{
// here to carch few bugs
MANGOS_ASSERT(m_pathPolyRefs[pathStartIndex] != INVALID_POLYREF);
if (m_pathPolyRefs[pathStartIndex] == startPoly)
{
startPolyFound = true;
break;
}
}
for (pathEndIndex = m_polyLength-1; pathEndIndex > pathStartIndex; --pathEndIndex)
if (m_pathPolyRefs[pathEndIndex] == endPoly)
{
endPolyFound = true;
break;
}
}
if (startPolyFound && endPolyFound)
{
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: (startPolyFound && endPolyFound)\n");
// we moved along the path and the target did not move out of our old poly-path
// our path is a simple subpath case, we have all the data we need
// just "cut" it out
m_polyLength = pathEndIndex - pathStartIndex + 1;
memmove(m_pathPolyRefs, m_pathPolyRefs+pathStartIndex, m_polyLength*sizeof(dtPolyRef));
}
else if (startPolyFound && !endPolyFound)
{
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: (startPolyFound && !endPolyFound)\n");
// we are moving on the old path but target moved out
// so we have atleast part of poly-path ready
m_polyLength -= pathStartIndex;
// try to adjust the suffix of the path instead of recalculating entire length
// at given interval the target cannot get too far from its last location
// thus we have less poly to cover
// sub-path of optimal path is optimal
// take ~80% of the original length
// TODO : play with the values here
uint32 prefixPolyLength = uint32(m_polyLength*0.8f + 0.5f);
memmove(m_pathPolyRefs, m_pathPolyRefs+pathStartIndex, prefixPolyLength*sizeof(dtPolyRef));
dtPolyRef suffixStartPoly = m_pathPolyRefs[prefixPolyLength-1];
// we need any point on our suffix start poly to generate poly-path, so we need last poly in prefix data
float suffixEndPoint[VERTEX_SIZE];
if (DT_SUCCESS != m_navMeshQuery->closestPointOnPoly(suffixStartPoly, endPoint, suffixEndPoint))
{
// suffixStartPoly is invalid somehow, or the navmesh is broken => error state
sLog.outError("%u's Path Build failed: invalid polyRef in path", m_sourceUnit->GetGUID());
BuildShortcut();
m_type = PATHFIND_NOPATH;
return;
}
// generate suffix
uint32 suffixPolyLength = 0;
dtStatus dtResult = m_navMeshQuery->findPath(
suffixStartPoly, // start polygon
endPoly, // end polygon
suffixEndPoint, // start position
endPoint, // end position
&m_filter, // polygon search filter
m_pathPolyRefs + prefixPolyLength - 1, // [out] path
(int*)&suffixPolyLength,
MAX_PATH_LENGTH-prefixPolyLength); // max number of polygons in output path
if (!suffixPolyLength || dtResult != DT_SUCCESS)
{
// this is probably an error state, but we'll leave it
// and hopefully recover on the next Update
// we still need to copy our preffix
sLog.outError("%u's Path Build failed: 0 length path", m_sourceUnit->GetGUID());
}
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ m_polyLength=%u prefixPolyLength=%u suffixPolyLength=%u \n",m_polyLength, prefixPolyLength, suffixPolyLength);
// new path = prefix + suffix - overlap
m_polyLength = prefixPolyLength + suffixPolyLength - 1;
}
else
{
DEBUG_FILTER_LOG(LOG_FILTER_PATHFINDING, "++ BuildPolyPath :: (!startPolyFound && !endPolyFound)\n");
// either we have no path at all -> first run
// or something went really wrong -> we aren't moving along the path to the target
// just generate new path
// free and invalidate old path data
clear();
dtStatus dtResult = m_navMeshQuery->findPath(
startPoly, // start polygon
endPoly, // end polygon
startPoint, // start position
endPoint, // end position
&m_filter, // polygon search filter
m_pathPolyRefs, // [out] path
(int*)&m_polyLength,
MAX_PATH_LENGTH); // max number of polygons in output path
if (!m_polyLength || dtResult != DT_SUCCESS)
{
// only happens if we passed bad data to findPath(), or navmesh is messed up
sLog.outError("%u's Path Build failed: 0 length path", m_sourceUnit->GetGUID());
BuildShortcut();
m_type = PATHFIND_NOPATH;
return;
}
}
// by now we know what type of path we can get
if (m_pathPolyRefs[m_polyLength - 1] == endPoly && !(m_type & PATHFIND_INCOMPLETE))
m_type = PATHFIND_NORMAL;
else
m_type = PATHFIND_INCOMPLETE;
// generate the point-path out of our up-to-date poly-path
BuildPointPath(startPoint, endPoint);
}
void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile)
{
if (!tile) return;
dtPolyRef base = getPolyRefBase(tile);
// Find Off-mesh connection end points.
for (int i = 0; i < tile->header->offMeshConCount; ++i)
{
dtOffMeshConnection* con = &tile->offMeshCons[i];
dtPoly* poly = &tile->polys[con->poly];
const float ext[3] = { con->rad, tile->header->walkableClimb, con->rad };
for (int j = 0; j < 2; ++j)
{
unsigned char side = j == 0 ? 0xff : con->side;
if (side == 0xff)
{
// Find polygon to connect to.
const float* p = &con->pos[j*3];
float nearestPt[3];
dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt);
if (!ref) continue;
// findNearestPoly may return too optimistic results, further check to make sure.
if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad))
continue;
// Make sure the location is on current mesh.
float* v = &tile->verts[poly->verts[j]*3];
dtVcopy(v, nearestPt);
// Link off-mesh connection to target poly.
unsigned int idx = allocLink(tile);
if (idx != DT_NULL_LINK)
{
dtLink* link = &tile->links[idx];
link->ref = ref;
link->edge = (unsigned char)j;
link->side = 0xff;
link->bmin = link->bmax = 0;
// Add to linked list.
link->next = poly->firstLink;
poly->firstLink = idx;
}
// Start end-point is always connect back to off-mesh connection,
// Destination end-point only if it is bidirectional link.
if (j == 0 || (j == 1 && (con->flags & DT_OFFMESH_CON_BIDIR)))
{
// Link target poly to off-mesh connection.
unsigned int idx = allocLink(tile);
if (idx != DT_NULL_LINK)
{
const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref);
dtPoly* landPoly = &tile->polys[landPolyIdx];
dtLink* link = &tile->links[idx];
link->ref = base | (dtPolyRef)(con->poly);
link->edge = 0xff;
link->side = 0xff;
link->bmin = link->bmax = 0;
// Add to linked list.
link->next = landPoly->firstLink;
landPoly->firstLink = idx;
}
}
}
}
}
}
int OgreDetourTileCache::rasterizeTileLayers(InputGeom* geom, const int tx, const int ty, const rcConfig& cfg, TileCacheData* tiles, const int maxTiles)
{
if (!geom || geom->isEmpty()) {
m_recast->m_pLog->logMessage("ERROR: buildTile: Input mesh is not specified.");
return 0;
}
if (!geom->getChunkyMesh()) {
m_recast->m_pLog->logMessage("ERROR: buildTile: Input mesh has no chunkyTriMesh built.");
return 0;
}
//TODO make these member variables?
FastLZCompressor comp;
RasterizationContext rc;
const float* verts = geom->getVerts();
const int nverts = geom->getVertCount();
// The chunky tri mesh in the inputgeom is a simple spatial subdivision structure that allows to
// process the vertices in the geometry relevant to this part of the tile.
// The chunky tri mesh is a grid of axis aligned boxes that store indices to the vertices in verts
// that are positioned in that box.
const rcChunkyTriMesh* chunkyMesh = geom->getChunkyMesh();
// Tile bounds.
const float tcs = m_tileSize * m_cellSize;
rcConfig tcfg;
memcpy(&tcfg, &m_cfg, sizeof(tcfg));
tcfg.bmin[0] = m_cfg.bmin[0] + tx*tcs;
tcfg.bmin[1] = m_cfg.bmin[1];
tcfg.bmin[2] = m_cfg.bmin[2] + ty*tcs;
tcfg.bmax[0] = m_cfg.bmin[0] + (tx+1)*tcs;
tcfg.bmax[1] = m_cfg.bmax[1];
tcfg.bmax[2] = m_cfg.bmin[2] + (ty+1)*tcs;
tcfg.bmin[0] -= tcfg.borderSize*tcfg.cs;
tcfg.bmin[2] -= tcfg.borderSize*tcfg.cs;
tcfg.bmax[0] += tcfg.borderSize*tcfg.cs;
tcfg.bmax[2] += tcfg.borderSize*tcfg.cs;
// This is part of the regular recast navmesh generation pipeline as in OgreRecast::NavMeshBuild()
// but only up till step 4 and slightly modified.
// Allocate voxel heightfield where we rasterize our input data to.
rc.solid = rcAllocHeightfield();
if (!rc.solid)
{
m_recast->m_pLog->logMessage("ERROR: buildNavigation: Out of memory 'solid'.");
return 0;
}
if (!rcCreateHeightfield(m_ctx, *rc.solid, tcfg.width, tcfg.height, tcfg.bmin, tcfg.bmax, tcfg.cs, tcfg.ch))
{
m_recast->m_pLog->logMessage("ERROR: buildNavigation: Could not create solid heightfield.");
return 0;
}
// Allocate array that can hold triangle flags.
// If you have multiple meshes you need to process, allocate
// an array which can hold the max number of triangles you need to process.
rc.triareas = new unsigned char[chunkyMesh->maxTrisPerChunk];
if (!rc.triareas)
{
m_recast->m_pLog->logMessage("ERROR: buildNavigation: Out of memory 'm_triareas' ("+Ogre::StringConverter::toString(chunkyMesh->maxTrisPerChunk)+").");
return 0;
}
float tbmin[2], tbmax[2];
tbmin[0] = tcfg.bmin[0];
tbmin[1] = tcfg.bmin[2];
tbmax[0] = tcfg.bmax[0];
tbmax[1] = tcfg.bmax[2];
int cid[512];// TODO: Make grow when returning too many items.
const int ncid = rcGetChunksOverlappingRect(chunkyMesh, tbmin, tbmax, cid, 512);
if (!ncid)
{
return 0; // empty
}
for (int i = 0; i < ncid; ++i)
{
const rcChunkyTriMeshNode& node = chunkyMesh->nodes[cid[i]];
const int* tris = &chunkyMesh->tris[node.i*3];
const int ntris = node.n;
memset(rc.triareas, 0, ntris*sizeof(unsigned char));
rcMarkWalkableTriangles(m_ctx, tcfg.walkableSlopeAngle,
verts, nverts, tris, ntris, rc.triareas);
rcRasterizeTriangles(m_ctx, verts, nverts, tris, rc.triareas, ntris, *rc.solid, tcfg.walkableClimb);
}
// Once all geometry is rasterized, we do initial pass of filtering to
// remove unwanted overhangs caused by the conservative rasterization
// as well as filter spans where the character cannot possibly stand.
rcFilterLowHangingWalkableObstacles(m_ctx, tcfg.walkableClimb, *rc.solid);
rcFilterLedgeSpans(m_ctx, tcfg.walkableHeight, tcfg.walkableClimb, *rc.solid);
rcFilterWalkableLowHeightSpans(m_ctx, tcfg.walkableHeight, *rc.solid);
rc.chf = rcAllocCompactHeightfield();
if (!rc.chf)
{
m_recast->m_pLog->logMessage("ERROR: buildNavigation: Out of memory 'chf'.");
return 0;
}
if (!rcBuildCompactHeightfield(m_ctx, tcfg.walkableHeight, tcfg.walkableClimb, *rc.solid, *rc.chf))
{
m_recast->m_pLog->logMessage("ERROR: buildNavigation: Could not build compact data.");
return 0;
}
// Erode the walkable area by agent radius.
if (!rcErodeWalkableArea(m_ctx, tcfg.walkableRadius, *rc.chf))
{
m_recast->m_pLog->logMessage("ERROR: buildNavigation: Could not erode.");
return 0;
}
// Mark areas of dynamically added convex polygons
const ConvexVolume* const* vols = geom->getConvexVolumes();
for (int i = 0; i < geom->getConvexVolumeCount(); ++i)
{
rcMarkConvexPolyArea(m_ctx, vols[i]->verts, vols[i]->nverts,
vols[i]->hmin, vols[i]->hmax,
(unsigned char)vols[i]->area, *rc.chf);
}
// Up till this part was more or less the same as OgreRecast::NavMeshBuild()
// The following part is specific for creating a 2D intermediary navmesh tile.
rc.lset = rcAllocHeightfieldLayerSet();
if (!rc.lset)
{
m_recast->m_pLog->logMessage("ERROR: buildNavigation: Out of memory 'lset'.");
return 0;
}
if (!rcBuildHeightfieldLayers(m_ctx, *rc.chf, tcfg.borderSize, tcfg.walkableHeight, *rc.lset))
{
m_recast->m_pLog->logMessage("ERROR: buildNavigation: Could not build heightfield layers.");
return 0;
}
rc.ntiles = 0;
for (int i = 0; i < rcMin(rc.lset->nlayers, MAX_LAYERS); ++i)
{
TileCacheData* tile = &rc.tiles[rc.ntiles++];
const rcHeightfieldLayer* layer = &rc.lset->layers[i];
// Store header
dtTileCacheLayerHeader header;
header.magic = DT_TILECACHE_MAGIC;
header.version = DT_TILECACHE_VERSION;
// Tile layer location in the navmesh.
header.tx = tx;
header.ty = ty;
header.tlayer = i;
dtVcopy(header.bmin, layer->bmin);
dtVcopy(header.bmax, layer->bmax);
// Tile info.
header.width = (unsigned char)layer->width;
header.height = (unsigned char)layer->height;
header.minx = (unsigned char)layer->minx;
header.maxx = (unsigned char)layer->maxx;
header.miny = (unsigned char)layer->miny;
header.maxy = (unsigned char)layer->maxy;
header.hmin = (unsigned short)layer->hmin;
header.hmax = (unsigned short)layer->hmax;
dtStatus status = dtBuildTileCacheLayer(&comp, &header, layer->heights, layer->areas, layer->cons,
&tile->data, &tile->dataSize);
if (dtStatusFailed(status))
{
return 0;
}
}
// Transfer ownsership of tile data from build context to the caller.
int n = 0;
for (int i = 0; i < rcMin(rc.ntiles, maxTiles); ++i)
{
tiles[n++] = rc.tiles[i];
rc.tiles[i].data = 0;
rc.tiles[i].dataSize = 0;
}
return n;
}
/// @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;
}
int dtObstacleAvoidanceQuery::sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
const float* vel, const float* dvel, float* nvel,
const dtObstacleAvoidanceParams* params,
dtObstacleAvoidanceDebugData* debug)
{
prepare(pos, dvel);
memcpy(&m_params, params, sizeof(dtObstacleAvoidanceParams));
m_invHorizTime = 1.0f / m_params.horizTime;
m_vmax = vmax;
m_invVmax = vmax > 0 ? 1.0f / vmax : FLT_MAX;
dtVset(nvel, 0,0,0);
if (debug)
debug->reset();
// Build sampling pattern aligned to desired velocity.
float pat[(DT_MAX_PATTERN_DIVS*DT_MAX_PATTERN_RINGS+1)*2];
int npat = 0;
const int ndivs = (int)m_params.adaptiveDivs;
const int nrings= (int)m_params.adaptiveRings;
const int depth = (int)m_params.adaptiveDepth;
const int nd = dtClamp(ndivs, 1, DT_MAX_PATTERN_DIVS);
const int nr = dtClamp(nrings, 1, DT_MAX_PATTERN_RINGS);
//const int nd2 = nd / 2;
const float da = (1.0f/nd) * DT_PI*2;
const float ca = cosf(da);
const float sa = sinf(da);
// desired direction
float ddir[6];
dtVcopy(ddir, dvel);
dtNormalize2D(ddir);
dtRorate2D (ddir+3, ddir, da*0.5f); // rotated by da/2
// Always add sample at zero
pat[npat*2+0] = 0;
pat[npat*2+1] = 0;
npat++;
for (int j = 0; j < nr; ++j)
{
const float r = (float)(nr-j)/(float)nr;
pat[npat*2+0] = ddir[(j%1)*3] * r;
pat[npat*2+1] = ddir[(j%1)*3+2] * r;
float* last1 = pat + npat*2;
float* last2 = last1;
npat++;
for (int i = 1; i < nd-1; i+=2)
{
// get next point on the "right" (rotate CW)
pat[npat*2+0] = last1[0]*ca + last1[1]*sa;
pat[npat*2+1] = -last1[0]*sa + last1[1]*ca;
// get next point on the "left" (rotate CCW)
pat[npat*2+2] = last2[0]*ca - last2[1]*sa;
pat[npat*2+3] = last2[0]*sa + last2[1]*ca;
last1 = pat + npat*2;
last2 = last1 + 2;
npat += 2;
}
if ((nd&1) == 0)
{
pat[npat*2+2] = last2[0]*ca - last2[1]*sa;
pat[npat*2+3] = last2[0]*sa + last2[1]*ca;
npat++;
}
}
// Start sampling.
float cr = vmax * (1.0f - m_params.velBias);
float res[3];
dtVset(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias);
int ns = 0;
for (int k = 0; k < depth; ++k)
{
float minPenalty = FLT_MAX;
float bvel[3];
dtVset(bvel, 0,0,0);
for (int i = 0; i < npat; ++i)
{
float vcand[3];
vcand[0] = res[0] + pat[i*2+0]*cr;
vcand[1] = 0;
vcand[2] = res[2] + pat[i*2+1]*cr;
if (dtSqr(vcand[0])+dtSqr(vcand[2]) > dtSqr(vmax+0.001f)) continue;
const float penalty = processSample(vcand,cr/10, pos,rad,vel,dvel, minPenalty, debug);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
dtVcopy(bvel, vcand);
}
}
dtVcopy(res, bvel);
cr *= 0.5f;
}
dtVcopy(nvel, res);
return ns;
}
/// @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;
if (params->offMeshConCount > 0)
{
offMeshConClass = (unsigned char*)dtAlloc(sizeof(unsigned char)*params->offMeshConCount*2, DT_ALLOC_TEMP);
if (!offMeshConClass)
return false;
// Find tight heigh bounds, used for culling out off-mesh start locations.
float hmin = FLT_MAX;
float hmax = -FLT_MAX;
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);
}
}
else
{
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);
}
}
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;
for (int i = 0; i < params->offMeshConCount; ++i)
{
const float* p0 = ¶ms->offMeshConVerts[(i*2+0)*3];
const float* p1 = ¶ms->offMeshConVerts[(i*2+1)*3];
offMeshConClass[i*2+0] = classifyOffMeshPoint(p0, bmin, bmax);
offMeshConClass[i*2+1] = classifyOffMeshPoint(p1, bmin, bmax);
// Zero out off-mesh start positions which are not even potentially touching the mesh.
if (offMeshConClass[i*2+0] == 0xff)
{
if (p0[1] < bmin[1] || p0[1] > bmax[1])
offMeshConClass[i*2+0] = 0;
}
// Cound 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++;
}
}
// Off-mesh connectionss are stored as polygons, adjust values.
const int totPolyCount = params->polyCount + storedOffMeshConCount;
const int totVertCount = params->vertCount + storedOffMeshConCount*2;
// 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++;
}
}
}
const int maxLinkCount = edgeCount + portalCount*2 + offMeshConLinkCount*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 dataSize = headerSize + vertsSize + polysSize + linksSize +
detailMeshesSize + detailVertsSize + detailTrisSize +
bvTreeSize + offMeshConsSize;
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;
// 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->walkableHeight = params->walkableHeight;
header->walkableRadius = params->walkableRadius;
header->walkableClimb = params->walkableClimb;
header->offMeshConCount = storedOffMeshConCount;
header->bvNodeCount = params->buildBvTree ? params->polyCount*2 : 0;
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 link vertices.
int n = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
// Only store connections which start from this tile.
if (offMeshConClass[i*2+0] == 0xff)
{
const float* linkv = ¶ms->offMeshConVerts[i*2*3];
float* v = &navVerts[(offMeshVertsBase + n*2)*3];
dtVcopy(&v[0], &linkv[0]);
dtVcopy(&v[3], &linkv[3]);
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 connection vertices.
n = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
// Only store connections which start from this tile.
if (offMeshConClass[i*2+0] == 0xff)
{
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 = params->offMeshConFlags[i];
p->setArea(params->offMeshConAreas[i]);
p->setType(DT_POLYTYPE_OFFMESH_CONNECTION);
n++;
}
}
// 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.
// TODO: take detail mesh into account! use byte per bbox extent?
if (params->buildBvTree)
{
createBVTree(params->verts, params->vertCount, params->polys, params->polyCount,
nvp, params->cs, params->ch, params->polyCount*2, navBvtree);
}
// Store Off-Mesh connections.
n = 0;
for (int i = 0; i < params->offMeshConCount; ++i)
{
// Only store connections which start from this tile.
if (offMeshConClass[i*2+0] == 0xff)
{
dtOffMeshConnection* con = &offMeshCons[n];
con->poly = (unsigned short)(offMeshPolyBase + n);
// Copy connection end-points.
const float* endPts = ¶ms->offMeshConVerts[i*2*3];
dtVcopy(&con->pos[0], &endPts[0]);
dtVcopy(&con->pos[3], &endPts[3]);
con->rad = params->offMeshConRad[i];
con->flags = params->offMeshConDir[i] ? DT_OFFMESH_CON_BIDIR : 0;
con->side = offMeshConClass[i*2+1];
if (params->offMeshConUserID)
con->userId = params->offMeshConUserID[i];
n++;
}
}
dtFree(offMeshConClass);
*outData = data;
*outDataSize = dataSize;
return true;
}
