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); } }