DetourNavMeshQuery.h

//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty.  In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
//    claim that you wrote the original software. If you use this software
//    in a product, an acknowledgment in the product documentation would be
//    appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
//    misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
 
#ifndef DETOURNAVMESHQUERY_H
#define DETOURNAVMESHQUERY_H
 
#include <AtomsUtils/Globals.h>
#include <AtomsUtils/NavigationMesh/Detour/DetourAlloc.h>
#include <AtomsUtils/NavigationMesh/Detour/DetourNavMesh.h>
#include <AtomsUtils/NavigationMesh/Detour/DetourCommon.h>
 
namespace AtomsUtils {
 
 
#define WITH_FIXED_AREA_ENTERING_COST 1
 
#define DT_UNWALKABLE_POLY_COST FLT_MAX
 
 
// By default dtQueryFilter will use virtual calls.
// On certain platforms indirect or virtual function call is expensive. The default
// setting is to use non-virtual functions, the actual implementations of the functions
// are declared as inline for maximum speed. 
// To avoid virtual calls create dtQueryFilter with inIsVirtual = false.
 
// Special link filter is custom filter run only for offmesh links with assigned UserId
// Used by smart navlinks in UE4
//
struct ATOMSUTILS_EXPORT dtQuerySpecialLinkFilter
{
    virtual ~dtQuerySpecialLinkFilter() {}
 
    virtual bool isLinkAllowed(const int UserId) const { return true; }
 
    virtual void initialize() {}
};
 
 
struct ATOMSUTILS_EXPORT dtQueryFilterData
{
    float m_areaCost[DT_MAX_AREAS];     
#if WITH_FIXED_AREA_ENTERING_COST
    float m_areaFixedCost[DT_MAX_AREAS];
#endif // WITH_FIXED_AREA_ENTERING_COST
    float heuristicScale;               
    float lowestAreaCost;
 
    unsigned short m_includeFlags;      
    unsigned short m_excludeFlags;      
 
    bool m_isBacktracking;
    
    bool m_shouldIgnoreClosedNodes;
    
 
    dtQueryFilterData();
    
    bool equals(const dtQueryFilterData* other) const;
    void copyFrom(const dtQueryFilterData* source);
};
 
class ATOMSUTILS_EXPORT dtQueryFilter
{
protected:
    dtQueryFilterData data;
    const bool isVirtual;
    
public:
    dtQueryFilter(bool inIsVirtual = true) : isVirtual(inIsVirtual)
    {}
    virtual ~dtQueryFilter() {}
    
protected:
 
    inline bool passInlineFilter(const dtPolyRef ref,
        const dtMeshTile* tile,
        const dtPoly* poly) const
    {
        return (poly->flags & data.m_includeFlags) != 0 && (poly->flags & data.m_excludeFlags) == 0
            && (data.m_areaCost[poly->getArea()] < DT_UNWALKABLE_POLY_COST)
#if WITH_FIXED_AREA_ENTERING_COST
            && (data.m_areaFixedCost[poly->getArea()] < DT_UNWALKABLE_POLY_COST)
#endif // WITH_FIXED_AREA_ENTERING_COST
            ;
    }
 
    virtual bool passVirtualFilter(const dtPolyRef ref,
        const dtMeshTile* tile,
        const dtPoly* poly) const
    {
        return passInlineFilter(ref, tile, poly);
    }
 
public:
    inline bool passFilter(const dtPolyRef ref,
                            const dtMeshTile* tile,
                            const dtPoly* poly) const
    {
        return !isVirtual ? passInlineFilter(ref, tile, poly) : passVirtualFilter(ref, tile, poly);
    }
 
protected:
 
    inline float getInlineCost(const float* pa, const float* pb,
        const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly,
        const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly,
        const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const
    {
#if WITH_FIXED_AREA_ENTERING_COST
        const float areaChangeCost = nextPoly != 0 && nextPoly->getArea() != curPoly->getArea()
            ? data.m_areaFixedCost[nextPoly->getArea()] : 0.f;
 
        return dtVdist(pa, pb) * data.m_areaCost[curPoly->getArea()] + areaChangeCost;
#else
        return dtVdist(pa, pb) * data.m_areaCost[curPoly->getArea()];
#endif // #if WITH_FIXED_AREA_ENTERING_COST
    }
 
    virtual float getVirtualCost(const float* pa, const float* pb,
        const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly,
        const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly,
        const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const
    {
        return getInlineCost(pa, pb,
            prevRef, prevTile, prevPoly,
            curRef, curTile, curPoly,
            nextRef, nextTile, nextPoly);
    }
 
public:
    inline float getCost(const float* pa, const float* pb,
                          const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly,
                          const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly,
                          const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const
    {
        return !isVirtual ? getInlineCost(pa, pb, prevRef, prevTile, prevPoly, curRef, curTile, curPoly, nextRef, nextTile, nextPoly)
            : getVirtualCost(pa, pb, prevRef, prevTile, prevPoly, curRef, curTile, curPoly, nextRef, nextTile, nextPoly);
    }
 
 
    inline float getAreaCost(const int i) const { return data.m_areaCost[i]; }
 
    inline void setAreaCost(const int i, const float cost) { data.m_areaCost[i] = cost; data.lowestAreaCost = dtMin(data.lowestAreaCost, cost); }
 
 
    inline const float* getAllAreaCosts() const { return data.m_areaCost; }
    
#if WITH_FIXED_AREA_ENTERING_COST
    inline float getAreaFixedCost(const int i) const { return data.m_areaFixedCost[i]; }
 
    inline void setAreaFixedCost(const int i, const float cost) { data.m_areaFixedCost[i] = cost; }
 
    inline const float* getAllFixedAreaCosts() const { return data.m_areaFixedCost; }
#endif // WITH_FIXED_AREA_ENTERING_COST
 
    inline float getModifiedHeuristicScale() const { return data.heuristicScale * ((data.lowestAreaCost > 0) ? data.lowestAreaCost : 1.0f); }
 
    inline float getHeuristicScale() const { return data.heuristicScale; }
 
    inline void setHeuristicScale(const float newScale) { data.heuristicScale = newScale; }
 
    inline bool isValidLinkSide(const unsigned char side) const 
    { 
        return (side & DT_LINK_FLAG_OFFMESH_CON) == 0 || (side & DT_LINK_FLAG_OFFMESH_CON_BIDIR) != 0
            || (data.m_isBacktracking ? (side & DT_LINK_FLAG_OFFMESH_CON_BACKTRACKER) != 0
                : (side & DT_LINK_FLAG_OFFMESH_CON_BACKTRACKER) == 0);
    }
 
    inline void setIsBacktracking(const bool isBacktracking) { data.m_isBacktracking = isBacktracking; }
 
    inline bool getIsBacktracking() const { return data.m_isBacktracking; }
 
    inline void setShouldIgnoreClosedNodes(const bool shouldIgnore) { data.m_shouldIgnoreClosedNodes = shouldIgnore; }
 
    inline bool getShouldIgnoreClosedNodes() const { return data.m_shouldIgnoreClosedNodes; }
 
 
    inline unsigned short getIncludeFlags() const { return data.m_includeFlags; }
 
    inline void setIncludeFlags(const unsigned short flags) { data.m_includeFlags = flags; }
 
    inline unsigned short getExcludeFlags() const { return data.m_excludeFlags; }
 
    inline void setExcludeFlags(const unsigned short flags) { data.m_excludeFlags = flags; }
 
 
    inline bool equals(const dtQueryFilter* other) const { return data.equals(&(other->data)); }
    inline bool equals(const dtQueryFilter& other) const { return data.equals(&(other.data)); }
 
    inline void copyFrom(const dtQueryFilter* other) { data.copyFrom(&(other->data)); }
    inline void copyFrom(const dtQueryFilter& other) { data.copyFrom(&(other.data)); }
 
};
 
struct dtQueryResultPack
{
    dtPolyRef ref;
    float cost;
    float pos[3];
    unsigned int flag;
 
    dtQueryResultPack() : ref(0), cost(0), flag(0) {}
    dtQueryResultPack(dtPolyRef inRef, float inCost, const float* inPos, unsigned int inFlag);
};
 
struct ATOMSUTILS_EXPORT dtQueryResult
{
    inline void reserve(int n) { data.resize(n); data.resize(0); }
    inline int size() const { return data.size(); }
 
    inline dtPolyRef getRef(int idx) const { return data[idx].ref; }
    inline float getCost(int idx) const { return data[idx].cost; }
    inline const float* getPos(int idx) const { return data[idx].pos; }
    inline unsigned int getFlag(int idx) const { return data[idx].flag; }
    void getPos(int idx, float* pos);
 
    void copyRefs(dtPolyRef* refs, int nmax);
    void copyCosts(float* costs, int nmax);
    void copyPos(float* pos, int nmax);
    void copyFlags(unsigned char* flags, int nmax);
    void copyFlags(unsigned int* flags, int nmax);
 
protected:
    dtChunkArray<dtQueryResultPack> data;
 
    inline int addItem(dtPolyRef ref, float cost, const float* pos, unsigned int flag) { data.push(dtQueryResultPack(ref, cost, pos, flag)); return data.size() - 1; }
 
    inline void setRef(int idx, dtPolyRef ref) { data[idx].ref = ref; }
    inline void setCost(int idx, float cost) { data[idx].cost = cost; }
    inline void setFlag(int idx, unsigned int flag) { data[idx].flag = flag; }
    void setPos(int idx, const float* pos);
 
    friend class dtNavMeshQuery;
};
 
class ATOMSUTILS_EXPORT dtNavMeshQuery
{
public:
    dtNavMeshQuery();
    ~dtNavMeshQuery();
    
    dtStatus init(const dtNavMesh* nav, const int maxNodes, dtQuerySpecialLinkFilter* linkFilter = 0);
 
    void updateLinkFilter(dtQuerySpecialLinkFilter* linkFilter);
    
    // /@{
 
    dtStatus findPath(dtPolyRef startRef, dtPolyRef endRef,
                      const float* startPos, const float* endPos, const float costLimit, 
                      const dtQueryFilter* filter,
                      dtQueryResult& result, float* totalCost) const;
    
    dtStatus testClusterPath(dtPolyRef startRef, dtPolyRef endRef) const; 
 
    dtStatus findStraightPath(const float* startPos, const float* endPos,
                              const dtPolyRef* path, const int pathSize,
                              dtQueryResult& result, const int options = 0) const;
 
 
    dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef,
                                const float* startPos, const float* endPos, const float costLimit, 
                                const dtQueryFilter* filter);
 
    dtStatus updateSlicedFindPath(const int maxIter, int* doneIters);
 
    dtStatus finalizeSlicedFindPath(dtPolyRef* path, int* pathCount, const int maxPath);
    
    dtStatus finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize,
                                           dtPolyRef* path, int* pathCount, const int maxPath);
 
 
    dtStatus findPolysAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius,
                                   const dtQueryFilter* filter,
                                   dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
                                   int* resultCount, const int maxResult) const;
    
    dtStatus findPolysAroundShape(dtPolyRef startRef, const float* verts, const int nverts,
                                  const dtQueryFilter* filter,
                                  dtPolyRef* resultRef, dtPolyRef* resultParent, float* resultCost,
                                  int* resultCount, const int maxResult) const;
    
    
    dtStatus findPolysInPathDistance(dtPolyRef startRef, const float* centerPos, const float pathDistance,
                                    const dtQueryFilter* filter, dtPolyRef* resultRef,
                                    int* resultCount, const int maxResult) const;
 
 
    dtStatus findNearestPoly(const float* center, const float* extents,
                             const dtQueryFilter* filter,
                             dtPolyRef* nearestRef, float* nearestPt, const float* referencePt = 0) const;
 
    dtStatus findNearestPoly2D(const float* center, const float* extents,
                            const dtQueryFilter* filter,
                            dtPolyRef* outProjectedRef, float* outProjectedPt,
                            const float* referencePt = 0, float tolerance = 0) const;
 
    dtStatus findNearestContainingPoly(const float* center, const float* extents,
                                       const dtQueryFilter* filter,
                                       dtPolyRef* nearestRef, float* nearestPt) const;
    
 
    dtStatus queryPolygons(const float* center, const float* extents,
                           const dtQueryFilter* filter,
                           dtPolyRef* polys, int* polyCount, const int maxPolys) const;
 
    dtStatus findLocalNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius,
                                    const dtQueryFilter* filter,
                                    dtPolyRef* resultRef, dtPolyRef* resultParent,
                                    int* resultCount, const int maxResult) const;
 
    dtStatus findWallsInNeighbourhood(dtPolyRef startRef, const float* centerPos, const float radius,
                                      const dtQueryFilter* filter, 
                                      dtPolyRef* neiRefs, int* neiCount, const int maxNei,
                                      float* resultWalls, dtPolyRef* resultRefs, int* resultCount, const int maxResult) const;
 
    dtStatus moveAlongSurface(dtPolyRef startRef, const float* startPos, const float* endPos,
                              const dtQueryFilter* filter,
                              float* resultPos, dtPolyRef* visited, int* visitedCount, const int maxVisitedSize) const;
    
    dtStatus raycast(dtPolyRef startRef, const float* startPos, const float* endPos,
                     const dtQueryFilter* filter,
                     float* t, float* hitNormal, dtPolyRef* path, int* pathCount, const int maxPath) const;
    
    dtStatus findDistanceToWall(dtPolyRef startRef, const float* centerPos, const float maxRadius,
                                const dtQueryFilter* filter,
                                float* hitDist, float* hitPos, float* hitNormal) const;
    
    dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
                                 float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount,
                                 const int maxSegments) const;
 
    dtStatus findRandomPoint(const dtQueryFilter* filter, int randomSeed,
                             dtPolyRef* randomRef, float* randomPt) const;
 
    dtStatus findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius,
                                         const dtQueryFilter* filter, int randomSeed,
                                         dtPolyRef* randomRef, float* randomPt) const;
 
    dtStatus findRandomPointInCluster(dtClusterRef clusterRef, int randomSeed,
                                      dtPolyRef* randomRef, float* randomPt) const;
    
    dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const;
    
    dtStatus closestPointOnPolyBoundary(dtPolyRef ref, const float* pos, float* closest) const;
 
    dtStatus projectedPointOnPoly(dtPolyRef ref, const float* pos, float* projected) const;
    
    dtStatus isPointInsidePoly(dtPolyRef ref, const float* pos, bool& result) const;
 
    dtStatus getPolyHeight(dtPolyRef ref, const float* pos, float* height) const;
 
    dtStatus getPolyCluster(dtPolyRef polyRef, dtClusterRef& clusterRef) const;
 
 
    bool isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const;
 
    bool isInClosedList(dtPolyRef ref) const;
    
    bool wasClusterLinkSearched(dtPolyRef cFrom, dtPolyRef cTo) const;
 
    class dtNodePool* getNodePool() const { return m_nodePool; }
    
    const dtNavMesh* getAttachedNavMesh() const { return m_nav; }
 
    void getCurrentBestResult(struct dtNode*& bestNode, float& bestCost) const { bestNode = m_query.lastBestNode; bestCost = m_query.lastBestNodeCost; }
 
    int getQueryNodes() const { return m_queryNodes; }
    
private:
    
    dtMeshTile* getNeighbourTileAt(int x, int y, int side) const;
 
    int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter,
                            dtPolyRef* polys, const int maxPolys) const;
    dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents,
                                    const dtQueryFilter* filter, float* nearestPt) const;
    void closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const;
 
    dtStatus projectedPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* projected) const;
    
    
    // exposing function to be able to generate navigation corridors as sequence of point pairs
public:
    dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right,
                             unsigned char& fromType, unsigned char& toType) const;
    dtStatus getPortalPoints(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
                             dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
                             float* left, float* right) const;
    
    dtStatus getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mid) const;
    dtStatus getEdgeMidPoint(dtPolyRef from, const dtPoly* fromPoly, const dtMeshTile* fromTile,
                             dtPolyRef to, const dtPoly* toPoly, const dtMeshTile* toTile,
                             float* mid) const;
private:
    
 
    // Appends vertex to a straight path
    dtStatus appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
                          dtQueryResult& result) const;
 
    // Appends intermediate portal points to a straight path.
    dtStatus appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path,
                           dtQueryResult& result, const int options) const;
    
    inline bool passLinkFilterByRef(const dtMeshTile* tile, const dtPolyRef ref) const
    {
        return passLinkFilter(tile, m_nav->decodePolyIdPoly(ref));
    }
 
    inline bool passLinkFilter(const dtMeshTile* tile, const int polyIdx) const
    {
        const int linkIdx = polyIdx - tile->header->offMeshBase;
 
        return !(m_linkFilter && polyIdx >= tile->header->offMeshBase
            && linkIdx < tile->header->offMeshConCount
            && tile->offMeshCons[linkIdx].userId != 0
            && m_linkFilter->isLinkAllowed(tile->offMeshCons[linkIdx].userId) == false);
    }
 
    const dtNavMesh* m_nav;                         
    dtQuerySpecialLinkFilter* m_linkFilter;         
 
    struct dtQueryData
    {
        dtStatus status;
        struct dtNode* lastBestNode;
        float lastBestNodeCost;
        dtPolyRef startRef, endRef;
        float startPos[3], endPos[3];
        float costLimit;                     
        const dtQueryFilter* filter;
    };
    dtQueryData m_query;                
 
    class dtNodePool* m_tinyNodePool;   
    class dtNodePool* m_nodePool;       
    class dtNodeQueue* m_openList;      
 
    mutable int m_queryNodes;
};
 
ATOMSUTILS_EXPORT dtNavMeshQuery* dtAllocNavMeshQuery();
 
ATOMSUTILS_EXPORT void dtFreeNavMeshQuery(dtNavMeshQuery* query);
 
}
 
#endif // DETOURNAVMESHQUERY_H