dtPolyRef PathFinder::getPathPolyByPosition(const dtPolyRef* polyPath, uint32 polyPathSize, const float* point, float* distance) const
{
if (!polyPath || !polyPathSize)
return INVALID_POLYREF;
dtPolyRef nearestPoly = INVALID_POLYREF;
float minDist2d = FLT_MAX;
float minDist3d = 0.0f;
for (uint32 i = 0; i < polyPathSize; ++i)
{
float closestPoint[VERTEX_SIZE];
dtStatus dtResult = m_navMeshQuery->closestPointOnPoly(polyPath[i], point, closestPoint);
if (dtStatusFailed(dtResult))
continue;
float d = dtVdist2DSqr(point, closestPoint);
if (d < minDist2d)
{
minDist2d = d;
nearestPoly = polyPath[i];
minDist3d = dtVdistSqr(point, closestPoint);
}
if (minDist2d < 1.0f) // shortcut out - close enough for us
break;
}
if (distance)
*distance = dtSqrt(minDist3d);
return (minDist2d < 3.0f) ? nearestPoly : INVALID_POLYREF;
}
/// @par
///
/// The agent's position will be constrained to the surface of the navigation mesh.
int dtCrowd::addAgent(const float* pos, const dtCrowdAgentParams* params)
{
// Find empty slot.
int idx = -1;
for (int i = 0; i < m_maxAgents; ++i)
{
if (!m_agents[i].active)
{
idx = i;
break;
}
}
if (idx == -1)
return -1;
dtCrowdAgent* ag = &m_agents[idx];
updateAgentParameters(idx, params);
// Find nearest position on navmesh and place the agent there.
float nearest[3];
dtPolyRef ref = 0;
dtVcopy(nearest, pos);
dtStatus status = m_navquery->findNearestPoly(pos, m_ext, &m_filters[ag->params.queryFilterType], &ref, nearest);
if (dtStatusFailed(status))
{
dtVcopy(nearest, pos);
ref = 0;
}
ag->corridor.reset(ref, nearest);
ag->boundary.reset();
ag->partial = false;
ag->topologyOptTime = 0;
ag->targetReplanTime = 0;
ag->nneis = 0;
dtVset(ag->dvel, 0,0,0);
dtVset(ag->nvel, 0,0,0);
dtVset(ag->vel, 0,0,0);
dtVcopy(ag->npos, nearest);
ag->desiredSpeed = 0;
if (ref)
ag->state = DT_CROWDAGENT_STATE_WALKING;
else
ag->state = DT_CROWDAGENT_STATE_INVALID;
ag->targetState = DT_CROWDAGENT_TARGET_NONE;
ag->active = true;
// Clockwork: added to fix illegal memory access when ncorners is queried before the agent has updated
ag->ncorners = 0;
return idx;
}
bool OgreDetourTileCache::buildTile(const int tx, const int ty, InputGeom *inputGeom)
{
if (tx < 0 || tx >= m_tw)
return false;
if (ty < 0 || ty >= m_th)
return false;
//TODO maybe I want to keep these values up to date
/*
m_cacheLayerCount = 0;
m_cacheCompressedSize = 0;
m_cacheRawSize = 0;
*/
TileCacheData tiles[MAX_LAYERS];
memset(tiles, 0, sizeof(tiles));
int ntiles = rasterizeTileLayers(inputGeom, tx, ty, m_cfg, tiles, MAX_LAYERS); // This is where the tile is built
dtStatus status;
// I don't know exactly why this can still be multiple tiles (??)
for (int i = 0; i < ntiles; ++i)
{
TileCacheData* tile = &tiles[i];
dtTileCacheLayerHeader* header = (dtTileCacheLayerHeader*)tile->data;
// Important: if a tile already exists at this position, first remove the old one or it will not be updated!
removeTile( m_tileCache->getTileRef(m_tileCache->getTileAt(header->tx, header->ty,header->tlayer)) );
status = m_tileCache->addTile(tile->data, tile->dataSize, DT_COMPRESSEDTILE_FREE_DATA, 0); // Add compressed tiles to tileCache
if (dtStatusFailed(status))
{
dtFree(tile->data);
tile->data = 0;
continue; // TODO maybe return false here?
}
// TODO this has to be recalculated differently when rebuilding a tile
/*
m_cacheLayerCount++;
m_cacheCompressedSize += tile->dataSize;
m_cacheRawSize += calcLayerBufferSize(m_tcparams.width, m_tcparams.height);
*/
}
//TODO add a deferred command for this?
// Build navmesh tile from cached tile
m_tileCache->buildNavMeshTilesAt(tx,ty, m_recast->m_navMesh); // This immediately builds the tile, without the need of a dtTileCache::update()
//TODO update this value?
//m_cacheBuildMemUsage = m_talloc->high;
// TODO extract debug drawing to a separate class
drawDetail(tx, ty);
return true;
}
void dtPathQueue::update(const int maxIters)
{
static const int MAX_KEEP_ALIVE = 2; // in update ticks.
// Update path request until there is nothing to update
// or upto maxIters pathfinder iterations has been consumed.
int iterCount = maxIters;
for (int i = 0; i < MAX_QUEUE; ++i)
{
PathQuery& q = m_queue[m_queueHead % MAX_QUEUE];
// Skip inactive requests.
if (q.ref == DT_PATHQ_INVALID)
{
m_queueHead++;
continue;
}
// Handle completed request.
if (dtStatusSucceed(q.status) || dtStatusFailed(q.status))
{
// If the path result has not been read in few frames, free the slot.
q.keepAlive++;
if (q.keepAlive > MAX_KEEP_ALIVE)
{
q.ref = DT_PATHQ_INVALID;
q.status = 0;
}
m_queueHead++;
continue;
}
m_navquery->updateLinkFilter(q.linkFilter.Get());
// Handle query start.
if (q.status == 0)
{
q.status = m_navquery->initSlicedFindPath(q.startRef, q.endRef, q.startPos, q.endPos, q.filter);
}
// Handle query in progress.
if (dtStatusInProgress(q.status))
{
int iters = 0;
q.status = m_navquery->updateSlicedFindPath(iterCount, &iters);
iterCount -= iters;
}
if (dtStatusSucceed(q.status))
{
q.status = m_navquery->finalizeSlicedFindPath(q.path, &q.npath, m_maxPathSize);
}
if (iterCount <= 0)
break;
m_queueHead++;
}
}
bool NavPath::visitNext()
{
U32 s = mVisitPoints.size();
if(s < 2)
return false;
// Current leg of journey.
Point3F &start = mVisitPoints[s-1];
Point3F &end = mVisitPoints[s-2];
// Drop to height of statics.
RayInfo info;
if(getContainer()->castRay(start, start - Point3F(0, 0, mMesh->mWalkableHeight * 2.0f), StaticObjectType, &info))
start = info.point;
if(getContainer()->castRay(end + Point3F(0, 0, 0.1f), end - Point3F(0, 0, mMesh->mWalkableHeight * 2.0f), StaticObjectType, &info))
end = info.point;
// Convert to Detour-friendly coordinates and data structures.
F32 from[] = {start.x, start.z, -start.y};
F32 to[] = {end.x, end.z, -end.y};
F32 extx = mMesh->mWalkableRadius * 4.0f;
F32 extz = mMesh->mWalkableHeight;
F32 extents[] = {extx, extz, extx};
dtPolyRef startRef, endRef;
if(dtStatusFailed(mQuery->findNearestPoly(from, extents, &mFilter, &startRef, NULL)) || !startRef)
{
Con::errorf("No NavMesh polygon near visit point (%g, %g, %g) of NavPath %s",
start.x, start.y, start.z, getIdString());
return false;
}
if(dtStatusFailed(mQuery->findNearestPoly(to, extents, &mFilter, &endRef, NULL)) || !endRef)
{
Con::errorf("No NavMesh polygon near visit point (%g, %g, %g) of NavPath %s",
end.x, end.y, end.z, getIdString());
return false;
}
// Init sliced pathfind.
mStatus = mQuery->initSlicedFindPath(startRef, endRef, from, to, &mFilter);
if(dtStatusFailed(mStatus))
return false;
return true;
}
bool nav_mesh::get_random_pos(const float* center, float radius, float* pos)
{
dtPolyRef centerRef = 0;
float polyPickExt[3] = { 2, 4, 2 };
dtStatus status = query_->findNearestPoly(center, polyPickExt, &filter_, ¢erRef, nullptr);
if (dtStatusFailed(status))
{
return false;
}
dtPolyRef posRef = 0;
status = query_->findRandomPointAroundCircle(centerRef, center, radius, &filter_, frand, &posRef, pos);
if (dtStatusFailed(status))
{
return false;
}
return true;
}
bool nav_mesh::get_random_pos(float* pos)
{
dtPolyRef posRef = 0;
dtStatus status = query_->findRandomPoint(&filter_, frand, &posRef, pos);
if (dtStatusFailed(status))
{
return false;
}
return true;
}
bool BotFindNearestPoly( Bot_t *bot, rVec coord, dtPolyRef *nearestPoly, rVec &nearPoint )
{
rVec start( coord );
rVec extents( 640, 96, 640 );
dtStatus status;
dtNavMeshQuery* navQuery = bot->nav->query;
dtQueryFilter* navFilter = &bot->nav->filter;
status = navQuery->findNearestPoly( start, extents, navFilter, nearestPoly, nearPoint );
if ( dtStatusFailed( status ) || *nearestPoly == 0 )
{
//try larger extents
extents[ 1 ] += 900;
status = navQuery->findNearestPoly( start, extents, navFilter, nearestPoly, nearPoint );
if ( dtStatusFailed( status ) || *nearestPoly == 0 )
{
return false; // failed
}
}
return true;
}
bool nav_mesh::get_height(dtPolyRef posRef, float* pos)
{
float h = 0;
dtStatus status = query_->getPolyHeight(posRef, pos, &h);
if (dtStatusFailed(status))
{
return false;
}
pos[1] = h;
return true;
}
bool NavPath::init()
{
mStatus = DT_FAILURE;
// Check that all the right data is provided.
if(!mMesh || !mMesh->getNavMesh())
return false;
if(!(mFromSet && mToSet) && !(mWaypoints && mWaypoints->size()))
return false;
// Initialise our query.
if(dtStatusFailed(mQuery->init(mMesh->getNavMesh(), MaxPathLen)))
return false;
mPoints.clear();
mFlags.clear();
mVisitPoints.clear();
mLength = 0.0f;
if(isServerObject())
setMaskBits(PathMask);
// Add points we need to visit in reverse order.
if(mWaypoints && mWaypoints->size())
{
if(mIsLooping && mFromSet)
mVisitPoints.push_back(mFrom);
if(mToSet)
mVisitPoints.push_front(mTo);
for(S32 i = mWaypoints->size() - 1; i >= 0; i--)
{
SceneObject *s = dynamic_cast<SceneObject*>(mWaypoints->at(i));
if(s)
{
mVisitPoints.push_back(s->getPosition());
// This is potentially slow, but safe.
if(!i && mIsLooping && !mFromSet)
mVisitPoints.push_front(s->getPosition());
}
}
if(mFromSet)
mVisitPoints.push_back(mFrom);
}
else
{
if(mIsLooping)
mVisitPoints.push_back(mFrom);
mVisitPoints.push_back(mTo);
mVisitPoints.push_back(mFrom);
}
return true;
}
dtNavMesh* Sample_TileMesh::loadAll(const char* path)
{
FILE* fp = fopen(path, "rb");
if (!fp) return 0;
// Read header.
NavMeshSetHeader header;
fread(&header, sizeof(NavMeshSetHeader), 1, fp);
if (header.magic != NAVMESHSET_MAGIC)
{
fclose(fp);
return 0;
}
if (header.version != NAVMESHSET_VERSION)
{
fclose(fp);
return 0;
}
dtNavMesh* mesh = dtAllocNavMesh();
if (!mesh)
{
fclose(fp);
return 0;
}
dtStatus status = mesh->init(&header.params);
if (dtStatusFailed(status))
{
fclose(fp);
return 0;
}
// Read tiles.
for (int i = 0; i < header.numTiles; ++i)
{
NavMeshTileHeader tileHeader;
fread(&tileHeader, sizeof(tileHeader), 1, fp);
if (!tileHeader.tileRef || !tileHeader.dataSize)
break;
unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
if (!data) break;
memset(data, 0, tileHeader.dataSize);
fread(data, tileHeader.dataSize, 1, fp);
mesh->addTile(data, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, 0);
}
fclose(fp);
return mesh;
}
bool nav_mesh::is_valid(const float* pos)
{
dtPolyRef posRef = 0;
float polyPickExt[3] = { 2, 4, 2 };
dtStatus status = query_->findNearestPoly(pos, polyPickExt, &filter_, &posRef, nullptr);
if (dtStatusFailed(status))
{
return false;
}
return true;
}
bool nav_mesh::adjust_pos(float* pos)
{
dtPolyRef posRef = 0;
float polyPickExt[3] = { 2, 4, 2 };
dtStatus status = query_->findNearestPoly(pos, polyPickExt, &filter_, &posRef, pos);
if (dtStatusFailed(status))
{
return false;
}
return true;
}
void Sample_TileMesh::buildAllTiles()
{
if (!m_geom) return;
if (!m_navMesh) return;
const float* bmin = m_geom->getNavMeshBoundsMin();
const float* bmax = m_geom->getNavMeshBoundsMax();
int gw = 0, gh = 0;
rcCalcGridSize(bmin, bmax, m_cellSize, &gw, &gh);
const int ts = (int)m_tileSize;
const int tw = (gw + ts-1) / ts;
const int th = (gh + ts-1) / ts;
const float tcs = m_tileSize*m_cellSize;
// Start the build process.
m_ctx->startTimer(RC_TIMER_TEMP);
for (int y = 0; y < th; ++y)
{
for (int x = 0; x < tw; ++x)
{
m_lastBuiltTileBmin[0] = bmin[0] + x*tcs;
m_lastBuiltTileBmin[1] = bmin[1];
m_lastBuiltTileBmin[2] = bmin[2] + y*tcs;
m_lastBuiltTileBmax[0] = bmin[0] + (x+1)*tcs;
m_lastBuiltTileBmax[1] = bmax[1];
m_lastBuiltTileBmax[2] = bmin[2] + (y+1)*tcs;
int dataSize = 0;
unsigned char* data = buildTileMesh(x, y, m_lastBuiltTileBmin, m_lastBuiltTileBmax, dataSize);
if (data)
{
// Remove any previous data (navmesh owns and deletes the data).
m_navMesh->removeTile(m_navMesh->getTileRefAt(x,y,0),0,0);
// Let the navmesh own the data.
dtStatus status = m_navMesh->addTile(data,dataSize,DT_TILE_FREE_DATA,0,0);
if (dtStatusFailed(status))
dtFree(data);
}
}
}
// Start the build process.
m_ctx->stopTimer(RC_TIMER_TEMP);
m_totalBuildTimeMs = m_ctx->getAccumulatedTime(RC_TIMER_TEMP)/1000.0f;
}
dtStatus dtTileCache::buildNavMeshTilesAt(const int tx, const int ty, dtNavMesh* navmesh)
{
const int MAX_TILES = 32;
dtCompressedTileRef tiles[MAX_TILES];
const int ntiles = getTilesAt(tx,ty,tiles,MAX_TILES);
for (int i = 0; i < ntiles; ++i)
{
dtStatus status = buildNavMeshTile(tiles[i], navmesh);
if (dtStatusFailed(status))
return status;
}
return DT_SUCCESS;
}
int fixupShortcuts(dtPolyRef* path, int npath, dtNavMeshQuery* navQuery)
{
if (npath < 3)
return npath;
// Get connected polygons
static const int maxNeis = 16;
dtPolyRef neis[maxNeis];
int nneis = 0;
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
if (dtStatusFailed(navQuery->getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly)))
return npath;
for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
{
const dtLink* link = &tile->links[k];
if (link->ref != 0)
{
if (nneis < maxNeis)
neis[nneis++] = link->ref;
}
}
// If any of the neighbour polygons is within the next few polygons
// in the path, short cut to that polygon directly.
static const int maxLookAhead = 6;
int cut = 0;
for (int i = dtMin(maxLookAhead, npath) - 1; i > 1 && cut == 0; i--) {
for (int j = 0; j < nneis; j++)
{
if (path[i] == neis[j]) {
cut = i;
break;
}
}
}
if (cut > 1)
{
int offset = cut - 1;
npath -= offset;
for (int i = 1; i < npath; i++)
path[i] = path[i + offset];
}
return npath;
}
bool OgreDetourTileCache::TileCacheBuild(InputGeom *inputGeom)
{
// Init configuration for specified geometry
configure(inputGeom);
dtStatus status;
// Preprocess tiles.
// Prepares navmesh tiles in a 2D intermediary format that allows quick conversion to a 3D navmesh
m_cacheLayerCount = 0;
m_cacheCompressedSize = 0;
m_cacheRawSize = 0;
for (int y = 0; y < m_th; ++y)
{
for (int x = 0; x < m_tw; ++x)
{
TileCacheData tiles[MAX_LAYERS];
memset(tiles, 0, sizeof(tiles));
int ntiles = rasterizeTileLayers(m_geom, x, y, m_cfg, tiles, MAX_LAYERS); // This is where the tile is built
for (int i = 0; i < ntiles; ++i)
{
TileCacheData* tile = &tiles[i];
status = m_tileCache->addTile(tile->data, tile->dataSize, DT_COMPRESSEDTILE_FREE_DATA, 0); // Add compressed tiles to tileCache
if (dtStatusFailed(status))
{
dtFree(tile->data);
tile->data = 0;
continue;
}
m_cacheLayerCount++;
m_cacheCompressedSize += tile->dataSize;
m_cacheRawSize += calcLayerBufferSize(m_tcparams.width, m_tcparams.height);
}
}
}
buildInitialNavmesh();
return true;
}
bool nav_mesh::los(const float* start, const float* end, float* hitPos)
{
float t = 0;
float m_hitNormal[3] = { 0, };
dtPolyRef startRef = 0;
float polyPickExt[3] = { 2, 4, 2 };
dtStatus status = query_->findNearestPoly(start, polyPickExt, &filter_, &startRef, nullptr);
if (dtStatusFailed(status))
{
return false;
}
dtPolyRef polys[MAX_POLYS] = { 0, };
int npolys = 0;
query_->raycast(startRef, start, end, &filter_, &t, m_hitNormal, polys, &npolys, sizeof(polys) / sizeof(polys[0]));
if (t > 1)
{
dtVcopy(hitPos, end);
return true;
}
else
{
if (npolys > 0)
{
dtVlerp(hitPos, start, end, t);
float temp[3] = { 0, };
dtStatus statu = query_->closestPointOnPoly(polys[npolys - 1], hitPos, temp, nullptr);
if (dtStatusSucceed(statu))
{
dtVcopy(hitPos, temp);
return false;
}
}
}
dtVcopy(hitPos, start);
return false;
}
void Sample_TileMesh::buildTile(const float* pos)
{
if (!m_geom) return;
if (!m_navMesh) return;
const float* bmin = m_geom->getNavMeshBoundsMin();
const float* bmax = m_geom->getNavMeshBoundsMax();
const float ts = m_tileSize*m_cellSize;
const int tx = (int)((pos[0] - bmin[0]) / ts);
const int ty = (int)((pos[2] - bmin[2]) / ts);
m_lastBuiltTileBmin[0] = bmin[0] + tx*ts;
m_lastBuiltTileBmin[1] = bmin[1];
m_lastBuiltTileBmin[2] = bmin[2] + ty*ts;
m_lastBuiltTileBmax[0] = bmin[0] + (tx+1)*ts;
m_lastBuiltTileBmax[1] = bmax[1];
m_lastBuiltTileBmax[2] = bmin[2] + (ty+1)*ts;
m_tileCol = duRGBA(255,255,255,64);
m_ctx->resetLog();
int dataSize = 0;
unsigned char* data = buildTileMesh(tx, ty, m_lastBuiltTileBmin, m_lastBuiltTileBmax, dataSize);
// Remove any previous data (navmesh owns and deletes the data).
m_navMesh->removeTile(m_navMesh->getTileRefAt(tx,ty,0),0,0);
// Add tile, or leave the location empty.
if (data)
{
// Let the navmesh own the data.
dtStatus status = m_navMesh->addTile(data,dataSize,DT_TILE_FREE_DATA,0,0);
if (dtStatusFailed(status))
dtFree(data);
}
m_ctx->dumpLog("Build Tile (%d,%d):", tx,ty);
}
bool NavMesh::createNavMesh(dtNavMeshCreateParams ¶ms)
{
unsigned char *tileData = NULL;
S32 tileDataSize = 0;
if(!dtCreateNavMeshData(¶ms, &tileData, &tileDataSize))
{
Con::errorf("Could not construct NavMeshData for NavMesh %s", getIdString());
return false;
}
tnm = dtAllocNavMesh();
if(!tnm)
{
Con::errorf("Out of memory allocating dtNavMesh for NavMesh %s", getIdString());
return false;
}
dtStatus s = tnm->init(tileData, tileDataSize, DT_TILE_FREE_DATA);
if(dtStatusFailed(s))
{
Con::errorf("Could not initialise dtNavMesh for NavMesh %s", getIdString());
return false;
}
// Initialise all flags to something helpful.
for(U32 i = 0; i < tnm->getMaxTiles(); ++i)
{
const dtMeshTile* tile = ((const dtNavMesh*)tnm)->getTile(i);
if(!tile->header) continue;
const dtPolyRef base = tnm->getPolyRefBase(tile);
for(U32 j = 0; j < tile->header->polyCount; ++j)
{
const dtPolyRef ref = base | j;
unsigned short f = 0;
tnm->getPolyFlags(ref, &f);
tnm->setPolyFlags(ref, f | 1);
}
}
return true;
}
EXPORT_API dtStatus dtnmInitTiledNavMesh(dtNavMeshParams* params
, dtNavMesh** ppNavMesh)
{
if (!params)
return DT_FAILURE + DT_INVALID_PARAM;
dtNavMesh* pNavMesh = dtAllocNavMesh();
if (!pNavMesh)
return DT_FAILURE + DT_OUT_OF_MEMORY;
dtStatus status =
pNavMesh->init(params);
if (dtStatusFailed(status))
{
dtFreeNavMesh(pNavMesh);
return status;
}
*ppNavMesh = pNavMesh;
return DT_SUCCESS;
}
dtStatus dtNavMesh::init(unsigned char* data, const int dataSize, const int flags)
{
// Make sure the data is in right format.
dtMeshHeader* header = (dtMeshHeader*)data;
if (header->magic != DT_NAVMESH_MAGIC)
return DT_FAILURE | DT_WRONG_MAGIC;
if (header->version != DT_NAVMESH_VERSION)
return DT_FAILURE | DT_WRONG_VERSION;
dtNavMeshParams params;
dtVcopy(params.orig, header->bmin);
params.tileWidth = header->bmax[0] - header->bmin[0];
params.tileHeight = header->bmax[2] - header->bmin[2];
params.maxTiles = 1;
params.maxPolys = header->polyCount;
dtStatus status = init(¶ms);
if (dtStatusFailed(status))
return status;
return addTile(data, dataSize, flags, 0, 0);
}
bool NavigationMesh::InitializeQuery()
{
if (!navMesh_ || !node_)
return false;
if (navMeshQuery_)
return true;
navMeshQuery_ = dtAllocNavMeshQuery();
if (!navMeshQuery_)
{
LOGERROR("Could not create navigation mesh query");
return false;
}
if (dtStatusFailed(navMeshQuery_->init(navMesh_, MAX_POLYS)))
{
LOGERROR("Could not init navigation mesh query");
return false;
}
return true;
}
bool dtPathQueue::init(const int maxPathSize, const int maxSearchNodeCount, dtNavMesh* nav)
{
purge();
m_navquery = dtAllocNavMeshQuery();
if (!m_navquery)
return false;
if (dtStatusFailed(m_navquery->init(nav, maxSearchNodeCount)))
return false;
m_maxPathSize = maxPathSize;
for (int i = 0; i < MAX_QUEUE; ++i)
{
m_queue[i].ref = DT_PATHQ_INVALID;
m_queue[i].path = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathSize, DT_ALLOC_PERM);
if (!m_queue[i].path)
return false;
}
m_queueHead = 0;
return true;
}
bool PointInPolyExtents( Bot_t *bot, dtPolyRef ref, rVec point, rVec extents )
{
rVec closest;
if ( dtStatusFailed( bot->nav->query->closestPointOnPolyBoundary( ref, point, closest ) ) )
{
return false;
}
// use the bot's bbox as an epsilon because the navmesh is always at least that far from a boundry
float maxRad = std::max( extents[ 0 ], extents[ 1 ] ) + 1;
if ( fabsf( point[ 0 ] - closest[ 0 ] ) > maxRad )
{
return false;
}
if ( fabsf( point[ 2 ] - closest[ 2 ] ) > maxRad )
{
return false;
}
return true;
}
bool PathFinder::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 || dtStatusFailed(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;
}
static void getPolyCenter(dtNavMesh* navMesh, dtPolyRef ref, float* center)
{
center[0] = 0;
center[1] = 0;
center[2] = 0;
const dtMeshTile* tile = 0;
const dtPoly* poly = 0;
dtStatus status = navMesh->getTileAndPolyByRef(ref, &tile, &poly);
if (dtStatusFailed(status))
return;
for (int i = 0; i < (int)poly->vertCount; ++i)
{
const float* v = &tile->verts[poly->verts[i]*3];
center[0] += v[0];
center[1] += v[1];
center[2] += v[2];
}
const float s = 1.0f / poly->vertCount;
center[0] *= s;
center[1] *= s;
center[2] *= s;
}
void OgreDetourTileCache::drawDetail(const int tx, const int ty)
{
if (!DEBUG_DRAW)
return; // Don't debug draw for huge performance gain!
struct TileCacheBuildContext
{
inline TileCacheBuildContext(struct dtTileCacheAlloc* a) : layer(0), lcset(0), lmesh(0), alloc(a) {}
inline ~TileCacheBuildContext() { purge(); }
void purge()
{
dtFreeTileCacheLayer(alloc, layer);
layer = 0;
dtFreeTileCacheContourSet(alloc, lcset);
lcset = 0;
dtFreeTileCachePolyMesh(alloc, lmesh);
lmesh = 0;
}
struct dtTileCacheLayer* layer;
struct dtTileCacheContourSet* lcset;
struct dtTileCachePolyMesh* lmesh;
struct dtTileCacheAlloc* alloc;
};
dtCompressedTileRef tiles[MAX_LAYERS];
const int ntiles = m_tileCache->getTilesAt(tx,ty,tiles,MAX_LAYERS);
dtTileCacheAlloc* talloc = m_tileCache->getAlloc();
dtTileCacheCompressor* tcomp = m_tileCache->getCompressor();
const dtTileCacheParams* params = m_tileCache->getParams();
for (int i = 0; i < ntiles; ++i)
{
const dtCompressedTile* tile = m_tileCache->getTileByRef(tiles[i]);
talloc->reset();
TileCacheBuildContext bc(talloc);
const int walkableClimbVx = (int)(params->walkableClimb / params->ch);
dtStatus status;
// Decompress tile layer data.
status = dtDecompressTileCacheLayer(talloc, tcomp, tile->data, tile->dataSize, &bc.layer);
if (dtStatusFailed(status))
return;
// Build navmesh
status = dtBuildTileCacheRegions(talloc, *bc.layer, walkableClimbVx);
if (dtStatusFailed(status))
return;
//TODO this part is replicated from navmesh tile building in DetourTileCache. Maybe that can be reused. Also is it really necessary to do an extra navmesh rebuild from compressed tile just to draw it? Can't I just draw it somewhere where the navmesh is rebuilt?
bc.lcset = dtAllocTileCacheContourSet(talloc);
if (!bc.lcset)
return;
status = dtBuildTileCacheContours(talloc, *bc.layer, walkableClimbVx,
params->maxSimplificationError, *bc.lcset);
if (dtStatusFailed(status))
return;
bc.lmesh = dtAllocTileCachePolyMesh(talloc);
if (!bc.lmesh)
return;
status = dtBuildTileCachePolyMesh(talloc, *bc.lcset, *bc.lmesh);
if (dtStatusFailed(status))
return;
// Draw navmesh
Ogre::String tileName = Ogre::StringConverter::toString(tiles[i]);
// Ogre::LogManager::getSingletonPtr()->logMessage("Drawing tile: "+tileName);
// TODO this is a dirty quickfix that should be gone as soon as there is a rebuildTile(tileref) method
if(m_recast->m_pSceneMgr->hasManualObject("RecastMOWalk_"+tileName))
return;
drawPolyMesh(tileName, *bc.lmesh, tile->header->bmin, params->cs, params->ch, *bc.layer);
}
}
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;
}
bool OgreDetourTileCache::loadAll(Ogre::String filename)
{
FILE* fp = fopen(filename.data(), "rb");
if (!fp) {
Ogre::LogManager::getSingletonPtr()->logMessage("Error: OgreDetourTileCache::loadAll("+filename+"). Could not open file.");
return false;
}
// Read header.
TileCacheSetHeader header;
fread(&header, sizeof(TileCacheSetHeader), 1, fp);
if (header.magic != TILECACHESET_MAGIC)
{
fclose(fp);
Ogre::LogManager::getSingletonPtr()->logMessage("Error: OgreDetourTileCache::loadAll("+filename+"). File does not appear to contain valid tilecache data.");
return false;
}
if (header.version != TILECACHESET_VERSION)
{
fclose(fp);
Ogre::LogManager::getSingletonPtr()->logMessage("Error: OgreDetourTileCache::loadAll("+filename+"). File contains a different version of the tilecache data format ("+Ogre::StringConverter::toString(header.version)+" instead of "+Ogre::StringConverter::toString(TILECACHESET_VERSION)+").");
return false;
}
m_recast->m_navMesh = dtAllocNavMesh();
if (!m_recast->m_navMesh)
{
fclose(fp);
Ogre::LogManager::getSingletonPtr()->logMessage("Error: OgreDetourTileCache::loadAll("+filename+"). Could not allocate navmesh.");
return false;
}
dtStatus status = m_recast->m_navMesh->init(&header.meshParams);
if (dtStatusFailed(status))
{
fclose(fp);
Ogre::LogManager::getSingletonPtr()->logMessage("Error: OgreDetourTileCache::loadAll("+filename+"). Could not init navmesh.");
return false;
}
m_tileCache = dtAllocTileCache();
if (!m_tileCache)
{
fclose(fp);
Ogre::LogManager::getSingletonPtr()->logMessage("Error: OgreDetourTileCache::loadAll("+filename+"). Could not allocate tilecache.");
return false;
}
status = m_tileCache->init(&header.cacheParams, m_talloc, m_tcomp, m_tmproc);
if (dtStatusFailed(status))
{
fclose(fp);
Ogre::LogManager::getSingletonPtr()->logMessage("Error: OgreDetourTileCache::loadAll("+filename+"). Could not init tilecache.");
return false;
}
memcpy(&m_cfg, &header.recastConfig, sizeof(rcConfig));
// Read tiles.
for (int i = 0; i < header.numTiles; ++i)
{
TileCacheTileHeader tileHeader;
fread(&tileHeader, sizeof(tileHeader), 1, fp);
if (!tileHeader.tileRef || !tileHeader.dataSize)
break;
unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
if (!data) break;
memset(data, 0, tileHeader.dataSize);
fread(data, tileHeader.dataSize, 1, fp);
dtCompressedTileRef tile = 0;
m_tileCache->addTile(data, tileHeader.dataSize, DT_COMPRESSEDTILE_FREE_DATA, &tile);
if (tile)
m_tileCache->buildNavMeshTile(tile, m_recast->m_navMesh);
}
fclose(fp);
// Init recast navmeshquery with created navmesh (in OgreRecast component)
m_recast->m_navQuery = dtAllocNavMeshQuery();
m_recast->m_navQuery->init(m_recast->m_navMesh, 2048);
// Config
// TODO handle this nicer, also inputGeom is not inited, making some functions crash
m_cellSize = m_cfg.cs;
m_tileSize = m_cfg.tileSize;
// cache bounding box
const float* bmin = m_cfg.bmin;
const float* bmax = m_cfg.bmax;
// Copy loaded config back to recast module
memcpy(&m_recast->m_cfg, &m_cfg, sizeof(rcConfig));
m_tileSize = m_cfg.tileSize;
m_cellSize = m_cfg.cs;
m_tcparams = header.cacheParams;
// Determine grid size (number of tiles) based on bounding box and grid cell size
int gw = 0, gh = 0;
rcCalcGridSize(bmin, bmax, m_cellSize, &gw, &gh); // Calculates total size of voxel grid
const int ts = m_tileSize;
const int tw = (gw + ts-1) / ts; // Tile width
const int th = (gh + ts-1) / ts; // Tile height
m_tw = tw;
m_th = th;
Ogre::LogManager::getSingletonPtr()->logMessage("Total Voxels: "+Ogre::StringConverter::toString(gw) + " x " + Ogre::StringConverter::toString(gh));
Ogre::LogManager::getSingletonPtr()->logMessage("Tilesize: "+Ogre::StringConverter::toString(m_tileSize)+" Cellsize: "+Ogre::StringConverter::toString(m_cellSize));
Ogre::LogManager::getSingletonPtr()->logMessage("Tiles: "+Ogre::StringConverter::toString(m_tw)+" x "+Ogre::StringConverter::toString(m_th));
// Max tiles and max polys affect how the tile IDs are caculated.
// There are 22 bits available for identifying a tile and a polygon.
int tileBits = rcMin((int)dtIlog2(dtNextPow2(tw*th*EXPECTED_LAYERS_PER_TILE)), 14);
if (tileBits > 14) tileBits = 14;
int polyBits = 22 - tileBits;
m_maxTiles = 1 << tileBits;
m_maxPolysPerTile = 1 << polyBits;
Ogre::LogManager::getSingletonPtr()->logMessage("Max Tiles: " + Ogre::StringConverter::toString(m_maxTiles));
Ogre::LogManager::getSingletonPtr()->logMessage("Max Polys: " + Ogre::StringConverter::toString(m_maxPolysPerTile));
// End config ////
buildInitialNavmesh();
return true;
}
