PImplRecastNavMesh.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
//
// Private implementation for communication with Recast library
// 
// All functions should be called through RecastNavMesh actor to make them thread safe!
//
 
#pragma once 
 
#if UE_ENABLE_INCLUDE_ORDER_DEPRECATED_IN_5_4
#include "CoreMinimal.h"
#endif
#include "UObject/WeakObjectPtr.h"
#if UE_ENABLE_INCLUDE_ORDER_DEPRECATED_IN_5_4
#include "NavFilters/NavigationQueryFilter.h"
#endif
#include "AI/Navigation/NavigationTypes.h"
#include "NavMesh/RecastNavMesh.h"
#include "NavMesh/RecastQueryFilter.h"
#include "AI/NavigationSystemBase.h"
#include "VisualLogger/VisualLogger.h"
 
#if RECAST_INTERNAL_DEBUG_DATA
#include "NavMesh/RecastInternalDebugData.h"
#endif
 
#if WITH_RECAST
#include "Detour/DetourNavMesh.h"
#include "Detour/DetourNavMeshQuery.h"
#endif
 
class FRecastNavMeshGenerator;
struct FNavLinkId;
 
#if WITH_RECAST
 
#define RECAST_VERY_SMALL_AGENT_RADIUS 0.0f
 
class FPImplRecastNavMesh
{
public:
 
    NAVIGATIONSYSTEM_API FPImplRecastNavMesh(ARecastNavMesh* Owner);
 
    NAVIGATIONSYSTEM_API ~FPImplRecastNavMesh();
 
    NAVIGATIONSYSTEM_API void Serialize(FArchive& Ar, int32 NavMeshVersion);
 
    /* Gather debug geometry.
     * @params OutGeometry Output geometry.
     * @params TileIndex Used to collect geometry for a specific tile, INDEX_NONE will gather all tiles.
     * @return True if done collecting.
     */
    UE_DEPRECATED(5.5, "Use the version of this function that takes a FNavTileRef instead.")
    NAVIGATIONSYSTEM_API bool GetDebugGeometryForTile(FRecastDebugGeometry& OutGeometry, int32 TileIndex) const;
 
    /* Gather debug geometry.
     * @params OutGeometry Output geometry.
     * @params TileRef Used to collect geometry for a specific tile, an invalid FNavTileRef will gather all tiles.
     * @return True if done collecting.
     */
    NAVIGATIONSYSTEM_API bool GetDebugGeometryForTile(FRecastDebugGeometry& OutGeometry, FNavTileRef TileRef) const;
    
    NAVIGATIONSYSTEM_API FBox GetNavMeshBounds() const;
 
    UE_DEPRECATED(5.5, "Use the version of this function on ARecastNavMesh that takes a FNavTileRef instead.")
    NAVIGATIONSYSTEM_API FBox GetNavMeshTileBounds(int32 TileIndex) const;
 
    UE_DEPRECATED(5.5, "Use the version of this function on ARecastNavMesh that takes a FNavTileRef instead.")
    NAVIGATIONSYSTEM_API bool GetNavMeshTileXY(int32 TileIndex, int32& OutX, int32& OutY, int32& OutLayer) const;
 
    NAVIGATIONSYSTEM_API bool GetNavMeshTileXY(const FVector& Point, int32& OutX, int32& OutY) const;
 
    UE_DEPRECATED(5.5, "Use the version of this function on ARecastNavMesh that takes an array of FNavTileRefs instead.")
    NAVIGATIONSYSTEM_API void GetNavMeshTilesAt(int32 TileX, int32 TileY, TArray<int32>& Indices) const;
 
    UE_DEPRECATED(5.5, "Use the version of the function that takes an array of FNavTileRefs instead")
    NAVIGATIONSYSTEM_API void GetNavMeshTilesIn(const TArray<FBox>& InclusionBounds, TArray<int32>& Indices) const;
 
    NAVIGATIONSYSTEM_API void GetNavMeshTilesIn(const TArray<FBox>& InclusionBounds, TArray<FNavTileRef>& OutRefs) const;
 
    inline int32 GetNavMeshTilesCount() const { return DetourNavMesh ? DetourNavMesh->getMaxTiles() : 0; }
 
    NAVIGATIONSYSTEM_API ENavigationQueryResult::Type FindPath(const FVector& StartLoc, const FVector& EndLoc, const FVector::FReal CostLimit, const bool bRequireNavigableEndLocation, FNavMeshPath& Path, const FNavigationQueryFilter& Filter, const UObject* Owner) const;
 
    NAVIGATIONSYSTEM_API ENavigationQueryResult::Type TestPath(const FVector& StartLoc, const FVector& EndLoc, const bool bRequireNavigableEndLocation, const FNavigationQueryFilter& Filter, const UObject* Owner, int32* NumVisitedNodes = 0) const;
 
    template< typename TRecastAStar, typename TRecastAStartGraph, typename TRecastGraphAStarFilter, typename TRecastAStarResult >
    ENavigationQueryResult::Type FindPathCustomAStar(TRecastAStartGraph& RecastGraphWrapper, TRecastAStar& AStarAlgo, const FVector& StartLoc, const FVector& EndLoc, const FVector::FReal CostLimit, FNavMeshPath& Path, const FNavigationQueryFilter& Filter, const UObject* Owner) const;
 
    NAVIGATIONSYSTEM_API void Raycast(const FVector& StartLoc, const FVector& EndLoc, const FNavigationQueryFilter& InQueryFilter, const UObject* Owner,
        ARecastNavMesh::FRaycastResult& RaycastResult, NavNodeRef StartNode = INVALID_NAVNODEREF) const;
 
    NAVIGATIONSYSTEM_API int32 DebugPathfinding(const FVector& StartLoc, const FVector& EndLoc, const FVector::FReal CostLimit, const bool bRequireNavigableEndLocation, const FNavigationQueryFilter& Filter, const UObject* Owner, TArray<FRecastDebugPathfindingData>& Steps);
 
    NAVIGATIONSYSTEM_API FNavLocation GetRandomPoint(const FNavigationQueryFilter& Filter, const UObject* Owner) const;
 
#if WITH_NAVMESH_CLUSTER_LINKS
    NAVIGATIONSYSTEM_API ENavigationQueryResult::Type TestClusterPath(const FVector& StartLoc, const FVector& EndLoc, int32* NumVisitedNodes = 0) const;
 
    NAVIGATIONSYSTEM_API bool GetRandomPointInCluster(NavNodeRef ClusterRef, FNavLocation& OutLocation) const;
#endif // WITH_NAVMESH_CLUSTER_LINKS
 
    NAVIGATIONSYSTEM_API bool FindMoveAlongSurface(const FNavLocation& StartLocation, const FVector& TargetPosition, FNavLocation& OutLocation, const FNavigationQueryFilter& Filter, const UObject* Owner) const;
 
    NAVIGATIONSYSTEM_API bool ProjectPointToNavMesh(const FVector& Point, FNavLocation& Result, const FVector& Extent, const FNavigationQueryFilter& Filter, const UObject* Owner) const;
    
    NAVIGATIONSYSTEM_API bool ProjectPointMulti(const FVector& Point, TArray<FNavLocation>& OutLocations, const FVector& Extent,
        FVector::FReal MinZ, FVector::FReal MaxZ, const FNavigationQueryFilter& Filter, const UObject* Owner) const;
 
    NAVIGATIONSYSTEM_API NavNodeRef FindNearestPoly(FVector const& Loc, FVector const& Extent, const FNavigationQueryFilter& Filter, const UObject* Owner) const;
 
    NAVIGATIONSYSTEM_API bool FindPolysAroundCircle(const FVector& CenterPos, const NavNodeRef CenterNodeRef, const FVector::FReal Radius, const FNavigationQueryFilter& Filter, const UObject* Owner, TArray<NavNodeRef>* OutPolys = nullptr, TArray<NavNodeRef>* OutPolysParent = nullptr, TArray<float>* OutPolysCost = nullptr, int32* OutPolysCount = nullptr) const;
 
    NAVIGATIONSYSTEM_API bool GetPolysWithinPathingDistance(FVector const& StartLoc, const FVector::FReal PathingDistance,
        const FNavigationQueryFilter& Filter, const UObject* Owner,
        TArray<NavNodeRef>& FoundPolys, FRecastDebugPathfindingData* DebugData) const;
 
    //@todo document
    NAVIGATIONSYSTEM_API void GetEdgesForPathCorridor(const TArray<NavNodeRef>* PathCorridor, TArray<FNavigationPortalEdge>* PathCorridorEdges) const;
 
    NAVIGATIONSYSTEM_API bool FindStraightPath(const FVector& StartLoc, const FVector& EndLoc, const TArray<NavNodeRef>& PathCorridor, TArray<FNavPathPoint>& PathPoints, TArray<FNavLinkId>* CustomLinks = NULL) const;
 
    NAVIGATIONSYSTEM_API bool FilterPolys(TArray<NavNodeRef>& PolyRefs, const FRecastQueryFilter* Filter, const UObject* Owner) const;
 
    UE_DEPRECATED(5.5, "Use the version of this function in ARecastNavMesh that takes a FNavTileRef instead.")
    NAVIGATIONSYSTEM_API bool GetPolysInTile(int32 TileIndex, TArray<FNavPoly>& Polys) const;
 
    NAVIGATIONSYSTEM_API void UpdateNavigationLinkArea(FNavLinkId UserId, uint8 AreaType, uint16 PolyFlags) const;
 
#if WITH_NAVMESH_SEGMENT_LINKS
    NAVIGATIONSYSTEM_API void UpdateSegmentLinkArea(int32 UserId, uint8 AreaType, uint16 PolyFlags) const;
 
    NAVIGATIONSYSTEM_API void ProcessSegmentLinksForTile(FNavTileRef TileRef) const;
 
    NAVIGATIONSYSTEM_API void ProcessSegmentLinksForTile(FNavTileRef TileRef, uint32 MaxSkippedNeigborTiles, dtTileRef* OutSkippedNeigborTiles, uint32& OutNumSkippedNeigborTiles) const;
#endif // WITH_NAVMESH_SEGMENT_LINKS
 
    NAVIGATIONSYSTEM_API bool GetPolyCenter(NavNodeRef PolyID, FVector& OutCenter) const;
    NAVIGATIONSYSTEM_API bool GetPolyVerts(NavNodeRef PolyID, TArray<FVector>& OutVerts) const;
    NAVIGATIONSYSTEM_API bool GetRandomPointInPoly(NavNodeRef PolyID, FVector& OutPoint) const;
    NAVIGATIONSYSTEM_API FVector::FReal GetPolySurfaceArea(NavNodeRef PolyID) const;
    NAVIGATIONSYSTEM_API bool GetPolyData(NavNodeRef PolyID, uint16& Flags, uint8& AreaType) const;
    NAVIGATIONSYSTEM_API uint32 GetPolyAreaID(NavNodeRef PolyID) const;
    NAVIGATIONSYSTEM_API void SetPolyAreaID(NavNodeRef PolyID, uint8 AreaID);
    NAVIGATIONSYSTEM_API bool GetPolyNeighbors(NavNodeRef PolyID, TArray<FNavigationPortalEdge>& Neighbors) const;
    NAVIGATIONSYSTEM_API bool GetPolyNeighbors(NavNodeRef PolyID, TArray<NavNodeRef>& Neighbors) const;
    NAVIGATIONSYSTEM_API bool GetPolyEdges(NavNodeRef PolyID, TArray<FNavigationPortalEdge>& Edges) const;
    NAVIGATIONSYSTEM_API bool GetPolyWallSegments(NavNodeRef PolyID, const FNavigationQueryFilter& InQueryFilter, const UObject* QueryOwner, TArray<FNavigationPortalEdge>& OutNeighbors) const;
    NAVIGATIONSYSTEM_API bool GetClosestPointOnPoly(NavNodeRef PolyID, const FVector& TestPt, FVector& PointOnPoly) const;
    NAVIGATIONSYSTEM_API bool GetPolyTileIndex(NavNodeRef PolyID, uint32& PolyIndex, uint32& TileIndex) const;
    NAVIGATIONSYSTEM_API bool GetPolyTileRef(NavNodeRef PolyId, uint32& OutPolyIndex, FNavTileRef& OutTileRef) const;
    UE_DEPRECATED(5.3, "Please use GetNavLinkUserId() instead. This function only returns Invalid.")
    uint32 GetLinkUserId(NavNodeRef LinkPolyID) const
    {
        return static_cast<int32>(FNavLinkId::Invalid.GetId());
    }
    NAVIGATIONSYSTEM_API FNavLinkId GetNavLinkUserId(NavNodeRef LinkPolyID) const;
    NAVIGATIONSYSTEM_API bool GetLinkEndPoints(NavNodeRef LinkPolyID, FVector& PointA, FVector& PointB) const;
    NAVIGATIONSYSTEM_API bool IsCustomLink(NavNodeRef PolyRef) const;
 
#if WITH_NAVMESH_CLUSTER_LINKS
    NAVIGATIONSYSTEM_API bool GetClusterBounds(NavNodeRef ClusterRef, FBox& OutBounds) const;
    NAVIGATIONSYSTEM_API NavNodeRef GetClusterRefFromPolyRef(const NavNodeRef PolyRef) const;
#endif // WITH_NAVMESH_CLUSTER_LINKS
 
    uint32 GetTileIndexFromPolyRef(const NavNodeRef PolyRef) const { return DetourNavMesh != NULL ? DetourNavMesh->decodePolyIdTile(PolyRef) : uint32(INDEX_NONE); }
 
    static NAVIGATIONSYSTEM_API uint16 GetFilterForbiddenFlags(const FRecastQueryFilter* Filter);
    static NAVIGATIONSYSTEM_API void SetFilterForbiddenFlags(FRecastQueryFilter* Filter, uint16 ForbiddenFlags);
 
    NAVIGATIONSYSTEM_API void OnAreaCostChanged();
 
public:
    dtNavMesh const* GetRecastMesh() const { return DetourNavMesh; };
    dtNavMesh* GetRecastMesh() { return DetourNavMesh; };
    NAVIGATIONSYSTEM_API void ReleaseDetourNavMesh();
 
    NAVIGATIONSYSTEM_API void RemoveTileCacheLayers(int32 TileX, int32 TileY);
    NAVIGATIONSYSTEM_API void RemoveTileCacheLayer(int32 TileX, int32 TileY, int32 LayerIdx);
    NAVIGATIONSYSTEM_API void AddTileCacheLayers(int32 TileX, int32 TileY, const TArray<FNavMeshTileData>& Layers);
    NAVIGATIONSYSTEM_API void AddTileCacheLayer(int32 TileX, int32 TileY, int32 LayerIdx, const FNavMeshTileData& LayerData);
    NAVIGATIONSYSTEM_API void MarkEmptyTileCacheLayers(int32 TileX, int32 TileY);
    NAVIGATIONSYSTEM_API FNavMeshTileData GetTileCacheLayer(int32 TileX, int32 TileY, int32 LayerIdx) const;
    NAVIGATIONSYSTEM_API TArray<FNavMeshTileData> GetTileCacheLayers(int32 TileX, int32 TileY) const;
    NAVIGATIONSYSTEM_API bool HasTileCacheLayers(int32 TileX, int32 TileY) const;
 
    NAVIGATIONSYSTEM_API void SetRecastMesh(dtNavMesh* NavMesh);
 
    NAVIGATIONSYSTEM_API SIZE_T GetTotalDataSize() const;
 
#if !UE_BUILD_SHIPPING
    NAVIGATIONSYSTEM_API int32 GetCompressedTileCacheSize();
#endif
 
    NAVIGATIONSYSTEM_API void ApplyWorldOffset(const FVector& InOffset, bool bWorldShift);
 
    NAVIGATIONSYSTEM_API FVector::FReal CalcSegmentCostOnPoly(NavNodeRef PolyID, const dtQueryFilter* Filter, const FVector& StartLoc, const FVector& EndLoc) const;
 
    ARecastNavMesh* NavMeshOwner;
    
    dtNavMesh* DetourNavMesh;
 
    TMap<FIntPoint, TArray<FNavMeshTileData> > CompressedTileCacheLayers;
 
#if RECAST_INTERNAL_DEBUG_DATA
    TMap<FIntPoint, FRecastInternalDebugData> DebugDataMap;
#endif
 
    mutable dtNavMeshQuery SharedNavQuery;
 
    static NAVIGATIONSYSTEM_API void SerializeRecastMeshTile(FArchive& Ar, int32 NavMeshVersion, unsigned char*& TileData, int32& TileDataSize);
 
    static NAVIGATIONSYSTEM_API void SerializeCompressedTileCacheData(FArchive& Ar, int32 NavMeshVersion, unsigned char*& CompressedData, int32& CompressedDataSize);
 
    NAVIGATIONSYSTEM_API bool InitPathfinding(const FVector& UnrealStart, const FVector& UnrealEnd, 
        const dtNavMeshQuery& Query, const dtQueryFilter* Filter,
        FVector& RecastStart, dtPolyRef& StartPoly,
        FVector& RecastEnd, dtPolyRef& EndPoly,
        FNavMeshPath* OutPath = nullptr) const;
 
    NAVIGATIONSYSTEM_API void PostProcessPath(dtStatus PathfindResult, FNavMeshPath& Path,
        const dtNavMeshQuery& Query, const dtQueryFilter* Filter,
        NavNodeRef StartNode, NavNodeRef EndNode,
        FVector UnrealStart, FVector UnrealEnd,
        FVector RecastStart, FVector RecastEnd,
        dtQueryResult& PathResult) const;
 
    UE_DEPRECATED(5.5, "Use GetTilePolyEdges instead.")
    NAVIGATIONSYSTEM_API void GetDebugPolyEdges(const dtMeshTile& Tile, bool bInternalEdges, bool bNavMeshEdges, TArray<FVector>& InternalEdgeVerts, TArray<FVector>& NavMeshEdgeVerts) const;
 
    NAVIGATIONSYSTEM_API void GetTilePolyEdges(const dtMeshTile& Tile, bool bGatherInteriorPolyEdges, bool bGatherExteriorNavMeshEdges, TArray<FVector>& OutInteriorPolyEdgeVerts, TArray<FVector>& OutExteriorNavMeshEdgeVerts) const;
 
    NAVIGATIONSYSTEM_API void GetEdgesForPathCorridorImpl(const TArray<NavNodeRef>* PathCorridor, TArray<FNavigationPortalEdge>* PathCorridorEdges, const dtNavMeshQuery& NavQuery) const;
 
    NAVIGATIONSYSTEM_API void GetTilePolyEdgesForFilter(const dtMeshTile& Tile, const FNavigationQueryFilter& InQueryFilter, TArray<FNavigationWallEdge>& OutEdges) const;
 
protected:
    NAVIGATIONSYSTEM_API int32 GetTilesDebugGeometry(const FRecastNavMeshGenerator* Generator, const dtMeshTile& Tile, int32 VertBase, FRecastDebugGeometry& OutGeometry, int32 TileIdx = INDEX_NONE, uint16 ForbiddenFlags = 0) const;
 
    NAVIGATIONSYSTEM_API ENavigationQueryResult::Type PostProcessPathInternal(dtStatus FindPathStatus, FNavMeshPath& Path, 
        const dtNavMeshQuery& NavQuery, const dtQueryFilter* QueryFilter, 
        NavNodeRef StartPolyID, NavNodeRef EndPolyID, 
        const FVector& RecastStartPos, const FVector& RecastEndPos, 
        dtQueryResult& PathResult) const;
};
 
template< typename TRecastAStar, typename TRecastAStartGraph, typename TRecastGraphAStarFilter, typename TRecastAStarResult >
ENavigationQueryResult::Type FPImplRecastNavMesh::FindPathCustomAStar(TRecastAStartGraph& RecastGraphWrapper, TRecastAStar& AStarAlgo, const FVector& StartLoc, const FVector& EndLoc, const FVector::FReal CostLimit, FNavMeshPath& Path, const FNavigationQueryFilter& InQueryFilter, const UObject* Owner) const
{
    const FRecastQueryFilter* FilterImplementation = (const FRecastQueryFilter*)(InQueryFilter.GetImplementation());
    if (FilterImplementation == nullptr)
    {
        UE_VLOG(NavMeshOwner, LogNavigation, Error, TEXT("FPImplRecastNavMesh::FindPath failed due to passed filter having NULL implementation!"));
        return ENavigationQueryResult::Error;
    }
 
    const dtQueryFilter* QueryFilter = FilterImplementation->GetAsDetourQueryFilter();
    if (QueryFilter == nullptr)
    {
        UE_VLOG(NavMeshOwner, LogNavigation, Warning, TEXT("FPImplRecastNavMesh::FindPath failing due to QueryFilter == NULL"));
        return ENavigationQueryResult::Error;
    }
 
    // initialize recast wrapper with the NavMeshOwner, not multithread safe!
    RecastGraphWrapper.Initialize(NavMeshOwner);
    TRecastGraphAStarFilter AStarFilter(RecastGraphWrapper, *FilterImplementation, InQueryFilter.GetMaxSearchNodes(), CostLimit, Owner);
 
    FVector RecastStartPos, RecastEndPos;
    NavNodeRef StartPolyID, EndPolyID;
    const bool bCanSearch = InitPathfinding(StartLoc, EndLoc, RecastGraphWrapper.GetRecastQuery(), QueryFilter, RecastStartPos, StartPolyID, RecastEndPos, EndPolyID, &Path);
    if (!bCanSearch)
    {
        return ENavigationQueryResult::Error;
    }
 
    typename TRecastAStar::FSearchNode StartNode(StartPolyID, RecastStartPos);
    typename TRecastAStar::FSearchNode EndNode(EndPolyID, RecastEndPos);
    StartNode.Initialize(RecastGraphWrapper);
    EndNode.Initialize(RecastGraphWrapper);
    TRecastAStarResult PathResult;
    auto AStarResult = AStarAlgo.FindPath(StartNode, EndNode, AStarFilter, PathResult);
 
    dtStatus FindPathStatus = RecastGraphWrapper.ConvertToRecastStatus(AStarAlgo, AStarFilter, AStarResult);
    return PostProcessPathInternal(FindPathStatus, Path, RecastGraphWrapper.GetRecastQuery(), FilterImplementation, StartNode.NodeRef, EndNode.NodeRef, 
        FVector(StartNode.Position[0], StartNode.Position[1], StartNode.Position[2]), 
        FVector(EndNode.Position[0], EndNode.Position[1], EndNode.Position[2]),
        PathResult);
}
 
#endif  // WITH_RECAST