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::GetTileGeometry(NavBuildData* build, Vector<NavigationGeometryInfo>& geometryList, BoundingBox& box) { Matrix3x4 inverse = node_->GetWorldTransform().Inverse(); for (unsigned i = 0; i < geometryList.Size(); ++i) { if (box.IsInsideFast(geometryList[i].boundingBox_) != OUTSIDE) { const Matrix3x4& transform = geometryList[i].transform_; if (geometryList[i].component_->GetType() == OffMeshConnection::GetTypeStatic()) { OffMeshConnection* connection = static_cast<OffMeshConnection*>(geometryList[i].component_); Vector3 start = inverse * connection->GetNode()->GetWorldPosition(); Vector3 end = inverse * connection->GetEndPoint()->GetWorldPosition(); build->offMeshVertices_.Push(start); build->offMeshVertices_.Push(end); build->offMeshRadii_.Push(connection->GetRadius()); build->offMeshFlags_.Push((unsigned short)connection->GetMask()); build->offMeshAreas_.Push((unsigned char)connection->GetAreaID()); build->offMeshDir_.Push((unsigned char)(connection->IsBidirectional() ? DT_OFFMESH_CON_BIDIR : 0)); continue; } else if (geometryList[i].component_->GetType() == NavArea::GetTypeStatic()) { NavArea* area = static_cast<NavArea*>(geometryList[i].component_); NavAreaStub stub; stub.areaID_ = (unsigned char)area->GetAreaID(); stub.bounds_ = area->GetWorldBoundingBox(); build->navAreas_.Push(stub); continue; } #ifdef ATOMIC_PHYSICS CollisionShape* shape = dynamic_cast<CollisionShape*>(geometryList[i].component_); if (shape) { switch (shape->GetShapeType()) { case SHAPE_TRIANGLEMESH: { Model* model = shape->GetModel(); if (!model) continue; unsigned lodLevel = shape->GetLodLevel(); for (unsigned j = 0; j < model->GetNumGeometries(); ++j) AddTriMeshGeometry(build, model->GetGeometry(j, lodLevel), transform); } break; case SHAPE_CONVEXHULL: { ConvexData* data = static_cast<ConvexData*>(shape->GetGeometryData()); if (!data) continue; unsigned numVertices = data->vertexCount_; unsigned numIndices = data->indexCount_; unsigned destVertexStart = build->vertices_.Size(); for (unsigned j = 0; j < numVertices; ++j) build->vertices_.Push(transform * data->vertexData_[j]); for (unsigned j = 0; j < numIndices; ++j) build->indices_.Push(data->indexData_[j] + destVertexStart); } break; case SHAPE_BOX: { unsigned destVertexStart = build->vertices_.Size(); build->vertices_.Push(transform * Vector3(-0.5f, 0.5f, -0.5f)); build->vertices_.Push(transform * Vector3(0.5f, 0.5f, -0.5f)); build->vertices_.Push(transform * Vector3(0.5f, -0.5f, -0.5f)); build->vertices_.Push(transform * Vector3(-0.5f, -0.5f, -0.5f)); build->vertices_.Push(transform * Vector3(-0.5f, 0.5f, 0.5f)); build->vertices_.Push(transform * Vector3(0.5f, 0.5f, 0.5f)); build->vertices_.Push(transform * Vector3(0.5f, -0.5f, 0.5f)); build->vertices_.Push(transform * Vector3(-0.5f, -0.5f, 0.5f)); const unsigned indices[] = { 0, 1, 2, 0, 2, 3, 1, 5, 6, 1, 6, 2, 4, 5, 1, 4, 1, 0, 5, 4, 7, 5, 7, 6, 4, 0, 3, 4, 3, 7, 1, 0, 4, 1, 4, 5 }; for (unsigned j = 0; j < 36; ++j) build->indices_.Push(indices[j] + destVertexStart); } break; default: break; } continue; } #endif Drawable* drawable = dynamic_cast<Drawable*>(geometryList[i].component_); if (drawable) { const Vector<SourceBatch>& batches = drawable->GetBatches(); for (unsigned j = 0; j < batches.Size(); ++j) AddTriMeshGeometry(build, drawable->GetLodGeometry(j, geometryList[i].lodLevel_), transform); } } } }
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); } }
Vector<WorldCoordinates>* PathFinderManager::findPathFromWorldToWorld(const WorldCoordinates& pointA, Vector<WorldCoordinates>& endPoints, Zone* zone, bool allowPartial) { Vector<WorldCoordinates>* finalpath = new Vector<WorldCoordinates>(); float finalLengthSq = FLT_MAX; #ifdef PROFILE_PATHING Timer t; t.start(); #endif for (const WorldCoordinates& pointB : endPoints) { const Vector3& startTemp = pointA.getPoint(); const Vector3& targetTemp = pointB.getPoint(); SortedVector<ManagedReference<NavArea*> > areas; Vector3 mid = startTemp + ((targetTemp-startTemp) * 0.5f); zone->getInRangeNavMeshes(mid.getX(), mid.getY(), &areas, true); SortedVector<NavCollision*> collisions; getNavMeshCollisions(&collisions, &areas, pointA.getWorldPosition(), pointB.getWorldPosition()); // Collisions are sorted by distance from the start of the line. This is done so that we can chain our path from // one navmesh to another if a path spans multiple meshes. Vector<WorldCoordinates> *path = new Vector<WorldCoordinates>(); float len = 0.0f; try { int collisionSize = collisions.size(); if (collisionSize == 1) { // we're entering/exiting a navmesh NavCollision* collision = collisions.get(0); NavArea *area = collision->getNavArea(); Vector3 position = collision->getPosition(); position.setZ(CollisionManager::getWorldFloorCollision(position.getX(), position.getY(), zone, true)); if (area->containsPoint(startTemp.getX(), startTemp.getY())) { if (!getRecastPath(startTemp, position, area, path, len, allowPartial)) { // exiting navmesh delete collision; if (path != NULL) delete path; continue; } path->add(pointB); } else { path->add(pointA); if (!getRecastPath(position, targetTemp, area, path, len, allowPartial)) { // entering navmesh delete collision; if (path != NULL) delete path; continue; } } if (len > 0 && len < finalLengthSq) { if (finalpath) delete finalpath; finalLengthSq = len; finalpath = path; path = NULL; } } else if (collisionSize == 0) { // we're already inside a navmesh (or there are no navmeshes around) for (int i = 0; i < areas.size(); i++) { if (!getRecastPath(startTemp, targetTemp, areas.get(i), path, len, allowPartial)) { continue; } if (len > 0 && len < finalLengthSq) { if (finalpath) delete finalpath; finalLengthSq = len; finalpath = path; path = new Vector<WorldCoordinates>(); } } } else if (collisionSize == 2) { // we're crossing over a mesh or dealing with multiple meshes NavCollision* collision1 = collisions.get(0); NavArea *area1 = collision1->getNavArea(); NavCollision* collision2 = collisions.get(1); NavArea *area2 = collision2->getNavArea(); if (area1 == area2) { // crossing same mesh path->add(pointA); Vector3 position1 = collision1->getPosition(); position1.setZ(CollisionManager::getWorldFloorCollision(position1.getX(), position1.getY(), zone, false)); Vector3 position2 = collision2->getPosition(); position2.setZ(CollisionManager::getWorldFloorCollision(position2.getX(), position2.getY(), zone, false)); if (!getRecastPath(position1, position2, area1, path, len, allowPartial)) { delete collision1; delete collision2; if (path != NULL) delete path; continue; } path->add(pointB); if (len > 0 && len < finalLengthSq) { if (finalpath) delete finalpath; finalLengthSq = len; finalpath = path; path = NULL; } } else { // TODO: handle multiple meshes } } else { // TODO: handle multiple meshes } } catch (...) { error("Unhandled pathing exception"); for (int i=collisions.size()-1; i>=0; i--) { NavCollision *collision = collisions.remove(i); delete collision; } delete path; path = NULL; } for (int i=collisions.size()-1; i>=0; i--) { NavCollision *collision = collisions.remove(i); delete collision; } if (path != NULL) delete path; } if (finalpath && finalpath->size() < 2) { // path could not be evaluated, just return the start/end position finalpath->removeAll(); finalpath->add(pointA); finalpath->add(endPoints.get(0)); } #ifdef PROFILE_PATHING t.stop(); totalTime.add(t.getElapsedTime()); info("Spent " + String::valueOf(totalTime.get()) + " in recast", true); #endif return finalpath; }