void NavigationMesh::CollectGeometries(Vector<NavigationGeometryInfo>& geometryList)
{
ATOMIC_PROFILE(CollectNavigationGeometry);
// Get Navigable components from child nodes, not from whole scene. This makes it possible to partition
// the scene into several navigation meshes
PODVector<Navigable*> navigables;
node_->GetComponents<Navigable>(navigables, true);
HashSet<Node*> processedNodes;
for (unsigned i = 0; i < navigables.Size(); ++i)
{
if (navigables[i]->IsEnabledEffective())
CollectGeometries(geometryList, navigables[i]->GetNode(), processedNodes, navigables[i]->IsRecursive());
}
// Get offmesh connections
Matrix3x4 inverse = node_->GetWorldTransform().Inverse();
PODVector<OffMeshConnection*> connections;
node_->GetComponents<OffMeshConnection>(connections, true);
for (unsigned i = 0; i < connections.Size(); ++i)
{
OffMeshConnection* connection = connections[i];
if (connection->IsEnabledEffective() && connection->GetEndPoint())
{
const Matrix3x4& transform = connection->GetNode()->GetWorldTransform();
NavigationGeometryInfo info;
info.component_ = connection;
info.boundingBox_ = BoundingBox(Sphere(transform.Translation(), connection->GetRadius())).Transformed(inverse);
geometryList.Push(info);
}
}
// Get nav area volumes
PODVector<NavArea*> navAreas;
node_->GetComponents<NavArea>(navAreas, true);
areas_.Clear();
for (unsigned i = 0; i < navAreas.Size(); ++i)
{
NavArea* area = navAreas[i];
if (area->IsEnabledEffective())
{
NavigationGeometryInfo info;
info.component_ = area;
info.boundingBox_ = area->GetWorldBoundingBox();
geometryList.Push(info);
areas_.Push(WeakPtr<NavArea>(area));
}
}
}
void DynamicNavigationMesh::DrawDebugGeometry(DebugRenderer* debug, bool depthTest)
{
if (!debug || !navMesh_ || !node_)
return;
const Matrix3x4& worldTransform = node_->GetWorldTransform();
const dtNavMesh* navMesh = navMesh_;
for (int z = 0; z < numTilesZ_; ++z)
{
for (int x = 0; x < numTilesX_; ++x)
{
// Get the layers from the tile-cache
const dtMeshTile* tiles[TILECACHE_MAXLAYERS];
int tileCount = navMesh->getTilesAt(x, z, tiles, TILECACHE_MAXLAYERS);
for (int i = 0; i < tileCount; ++i)
{
const dtMeshTile* tile = tiles[i];
if (!tile)
continue;
for (int i = 0; i < tile->header->polyCount; ++i)
{
dtPoly* poly = tile->polys + i;
for (unsigned j = 0; j < poly->vertCount; ++j)
{
debug->AddLine(
worldTransform * *reinterpret_cast<const Vector3*>(&tile->verts[poly->verts[j] * 3]),
worldTransform * *reinterpret_cast<const Vector3*>(&tile->verts[poly->verts[(j + 1) % poly->vertCount] * 3]),
Color::YELLOW,
depthTest
);
}
}
}
}
}
Scene* scene = GetScene();
if (scene)
{
// Draw Obstacle components
if (drawObstacles_)
{
PODVector<Node*> obstacles;
scene->GetChildrenWithComponent<Obstacle>(obstacles, true);
for (unsigned i = 0; i < obstacles.Size(); ++i)
{
Obstacle* obstacle = obstacles[i]->GetComponent<Obstacle>();
if (obstacle && obstacle->IsEnabledEffective())
obstacle->DrawDebugGeometry(debug, depthTest);
}
}
// Draw OffMeshConnection components
if (drawOffMeshConnections_)
{
PODVector<Node*> connections;
scene->GetChildrenWithComponent<OffMeshConnection>(connections, true);
for (unsigned i = 0; i < connections.Size(); ++i)
{
OffMeshConnection* connection = connections[i]->GetComponent<OffMeshConnection>();
if (connection && connection->IsEnabledEffective())
connection->DrawDebugGeometry(debug, depthTest);
}
}
// Draw NavArea components
if (drawNavAreas_)
{
PODVector<Node*> areas;
scene->GetChildrenWithComponent<NavArea>(areas, true);
for (unsigned i = 0; i < areas.Size(); ++i)
{
NavArea* area = areas[i]->GetComponent<NavArea>();
if (area && area->IsEnabledEffective())
area->DrawDebugGeometry(debug, depthTest);
}
}
}
}
void NavigationMesh::FindPath(PODVector<NavigationPathPoint>& dest, const Vector3& start, const Vector3& end,
const Vector3& extents, const dtQueryFilter* filter)
{
ATOMIC_PROFILE(FindPath);
dest.Clear();
if (!InitializeQuery())
return;
// Navigation data is in local space. Transform path points from world to local
const Matrix3x4& transform = node_->GetWorldTransform();
Matrix3x4 inverse = transform.Inverse();
Vector3 localStart = inverse * start;
Vector3 localEnd = inverse * end;
const dtQueryFilter* queryFilter = filter ? filter : queryFilter_.Get();
dtPolyRef startRef;
dtPolyRef endRef;
navMeshQuery_->findNearestPoly(&localStart.x_, &extents.x_, queryFilter, &startRef, 0);
navMeshQuery_->findNearestPoly(&localEnd.x_, &extents.x_, queryFilter, &endRef, 0);
if (!startRef || !endRef)
return;
int numPolys = 0;
int numPathPoints = 0;
navMeshQuery_->findPath(startRef, endRef, &localStart.x_, &localEnd.x_, queryFilter, pathData_->polys_, &numPolys,
MAX_POLYS);
if (!numPolys)
return;
Vector3 actualLocalEnd = localEnd;
// If full path was not found, clamp end point to the end polygon
if (pathData_->polys_[numPolys - 1] != endRef)
navMeshQuery_->closestPointOnPoly(pathData_->polys_[numPolys - 1], &localEnd.x_, &actualLocalEnd.x_, 0);
navMeshQuery_->findStraightPath(&localStart.x_, &actualLocalEnd.x_, pathData_->polys_, numPolys,
&pathData_->pathPoints_[0].x_, pathData_->pathFlags_, pathData_->pathPolys_, &numPathPoints, MAX_POLYS);
// Transform path result back to world space
for (int i = 0; i < numPathPoints; ++i)
{
NavigationPathPoint pt;
pt.position_ = transform * pathData_->pathPoints_[i];
pt.flag_ = (NavigationPathPointFlag)pathData_->pathFlags_[i];
// Walk through all NavAreas and find nearest
unsigned nearestNavAreaID = 0; // 0 is the default nav area ID
float nearestDistance = M_LARGE_VALUE;
for (unsigned j = 0; j < areas_.Size(); j++)
{
NavArea* area = areas_[j].Get();
if (area && area->IsEnabledEffective())
{
BoundingBox bb = area->GetWorldBoundingBox();
if (bb.IsInside(pt.position_) == INSIDE)
{
Vector3 areaWorldCenter = area->GetNode()->GetWorldPosition();
float distance = (areaWorldCenter - pt.position_).LengthSquared();
if (distance < nearestDistance)
{
nearestDistance = distance;
nearestNavAreaID = area->GetAreaID();
}
}
}
}
pt.areaID_ = (unsigned char)nearestNavAreaID;
dest.Push(pt);
}
}