basemultidestinationpathfinderquery.h

Go to the documentation of this file.

/*
* Copyright 2015 Autodesk, Inc. All rights reserved.
* Use of this software is subject to the terms of the Autodesk license agreement and any attachments or Appendices thereto provided at the time of installation or download,
* or which otherwise accompanies this software in either electronic or hard copy form, or which is signed by you and accepted by Autodesk.
*/



#ifndef Navigation_BaseMultiDestinationPathFinderQueryResult_H
#define Navigation_BaseMultiDestinationPathFinderQueryResult_H

#include "gwnavruntime/navmesh/identifiers/navtriangleptr.h"
#include "gwnavruntime/querysystem/workingmemcontainers/navdatachangeindexingrid.h"
#include "gwnavruntime/queries/ipathfinderquery.h"
#include "gwnavruntime/queries/utils/pathfinderqueryutils.h"
#include "gwnavruntime/queries/utils/pathrefinercontext.h"
#include "gwnavruntime/queries/utils/pathclampercontext.h"
#include "gwnavruntime/queries/utils/traversalparams.h"
#include "gwnavruntime/queries/utils/pathrefinerconfig.h"
#include "gwnavruntime/queries/utils/cellfilter.h"
#include "gwnavruntime/base/types.h"


namespace Kaim
{

class Path;

enum MultiDestinationPathFinderQueryResult
{
    MULTIDESTINATIONPATHFINDER_NOT_INITIALIZED = QUERY_NOT_INITIALIZED, 
    MULTIDESTINATIONPATHFINDER_NOT_PROCESSED   = QUERY_NOT_PROCESSED,   
    
    // ---------------------  Done once for all destinations ----------------------------
    MULTIDESTINATIONPATHFINDER_PROCESSING_TRAVERSAL,                    
    MULTIDESTINATIONPATHFINDER_PROCESSING_TRAVERSAL_DONE,               
    
    // ---------------------  Done for each destination ----------------------------
    MULTIDESTINATIONPATHFINDER_PROCESSING_CANGOQUERY,                   
    MULTIDESTINATIONPATHFINDER_PROCESSING_ABSTRACT_PATH,                
    MULTIDESTINATIONPATHFINDER_PROCESSING_REFINING_INIT,                
    MULTIDESTINATIONPATHFINDER_PROCESSING_REFINING,                     
    MULTIDESTINATIONPATHFINDER_PROCESSING_PATHCLAMPING_INIT,            
    MULTIDESTINATIONPATHFINDER_PROCESSING_PATHCLAMPING,                 
    MULTIDESTINATIONPATHFINDER_PROCESSING_PATHBUILDING,                 
    MULTIDESTINATIONPATHFINDER_PROCESSING_CHANNEL_INIT,                 
    MULTIDESTINATIONPATHFINDER_PROCESSING_CHANNEL_COMPUTE,              

    // ---------------------  Possible results at the end of query  ----------------------------
    MULTIDESTINATIONPATHFINDER_DONE_START_OUTSIDE,                      
    MULTIDESTINATIONPATHFINDER_DONE_START_NAVTAG_FORBIDDEN,             
    MULTIDESTINATIONPATHFINDER_DONE_ENDS_ARE_UNREACHABLE,               
    MULTIDESTINATIONPATHFINDER_DONE_DESTS_ARE_START_NO_PATH,            
    MULTIDESTINATIONPATHFINDER_DONE_NO_PATH_FOUND,                      

    MULTIDESTINATIONPATHFINDER_DONE_NAVDATA_CHANGED,                    
    MULTIDESTINATIONPATHFINDER_DONE_ERROR_LACK_OF_WORKING_MEMORY,       
    MULTIDESTINATIONPATHFINDER_DONE_COMPUTATION_ERROR,                  
    MULTIDESTINATIONPATHFINDER_DONE_CHANNELCONFIG_ERROR,                
    MULTIDESTINATIONPATHFINDER_DONE_CHANNELCOMPUTATION_ERROR,           
    MULTIDESTINATIONPATHFINDER_DONE_COMPUTATION_CANCELED,               

    MULTIDESTINATIONPATHFINDER_DONE_SOME_PATHS_FOUND,                     

    // ---------------------  Used individually for m_resultPerDestination ----------------------------
    MULTIDESTINATIONPATHFINDER_TOPROCESS_FROM_CANGOQUERY,                
    MULTIDESTINATIONPATHFINDER_PROCESSING,                              
    MULTIDESTINATIONPATHFINDER_DONE_END_OUTSIDE,                        
    MULTIDESTINATIONPATHFINDER_DONE_END_NAVTAG_FORBIDDEN,               
    MULTIDESTINATIONPATHFINDER_DONE_PATH_NOT_FOUND,                     
    MULTIDESTINATIONPATHFINDER_DONE_DEST_IS_START_NO_PATH,              
    MULTIDESTINATIONPATHFINDER_DONE_PATH_FOUND,                         
};

enum MultiDestinationPathFinderTryCanGoMode
{
    MULTIDESTINATIONPATHFINDER_DO_NOT_TRY_RAYCANGO = 0,
    MULTIDESTINATIONPATHFINDER_DO_TRY_RAYCANGO     = 1
};

enum MultiDestinationPathFinderFinalOutput
{
    MULTIDESTINATIONPATHFINDER_BUILD_LOWEST_COST_PATH_ONLY = 0, 
    MULTIDESTINATIONPATHFINDER_COMPUTE_RAW_COSTS = 1 << 0, 
    MULTIDESTINATIONPATHFINDER_BUILD_RAW_PATHS   = 1 << 1, 
    MULTIDESTINATIONPATHFINDER_BUILD_PATHS       = 1 << 2, 
};


static const AStarNodeIndex AStarNodeIndex_FirstDestNode = 1;

class PathFinderPoint
{
public:
    PathFinderPoint() { Clear(); }

    void Clear()
    {
        m_pos3f.Set(KyFloat32MAXVAL, KyFloat32MAXVAL, KyFloat32MAXVAL);
        m_trianglePtr.Invalidate();
        m_navGraphEdgePtr[0].Invalidate();
        m_navGraphEdgePtr[1].Invalidate();
        m_navGraphVertexPtr.Invalidate();
        m_navGraphVertexPtr.Invalidate();
    }   

    void SetPosition(const Vec3f& pos3f);
    void SetTrianglePtr(const NavTrianglePtr& destTrianglePtr);
    void SetNavGraphEdgePtr(const NavGraphEdgePtr& startNavGraphEdgePtr, NavGraphEdgeDirection navGraphEdgePathfindMode);
    void SetNavGraphVertexPtr(const NavGraphVertexPtr& startNavGraphVertexPtr);

public:
    NavTrianglePtr m_trianglePtr;
    NavGraphEdgePtr m_navGraphEdgePtr[2];
    NavGraphVertexPtr m_navGraphVertexPtr;
    Vec3f m_pos3f;        
    Vec3f m_insidePos3f;  
};

class BaseMultiDestinationPathFinderQuery : public ITimeSlicedQuery
{
public:
    static  QueryType            GetStaticType() { return TypeMultiDestinationPathFinderQuery; }
    virtual QueryType            GetType() const { return TypeMultiDestinationPathFinderQuery; }
    virtual void                 BuildQueryBlob(BaseBlobHandler* blobHandler);
    virtual void                 InitFromQueryBlob(World* world, void* blob);
    virtual Ptr<BaseBlobHandler> CreateQueryBlobHandler() { return CreateStaticQueryBlobHandler(); }
    static Ptr<BaseBlobHandler>  CreateStaticQueryBlobHandler();

    // ---------------------------------- Main API functions ----------------------------------

    MultiDestinationPathFinderQueryResult  GetResult()                                const;
    KyFloat32                              GetFromOutsideNavMeshDistance()            const;
    KyFloat32                              GetToOutsideNavMeshDistance()              const;

    Ptr<CellFilter>                        GetPropagationCellFilter()                 const;

    const Vec3f&                           GetStartPos()                              const;
    const NavTrianglePtr&                  GetStartTrianglePtr()                      const;
    const NavGraphEdgePtr&                 GetStartNavGraphEdgePtr()                  const;
    const NavGraphVertexPtr&               GetStartNavGraphVertexPtr()                const;

    KyUInt32                               GetDestCount()                             const;
    const Vec3f&                           GetDestPos(KyUInt32 destIdx)               const;
    const NavTrianglePtr&                  GetDestTrianglePtr(KyUInt32 destIdx)       const;
    const NavGraphEdgePtr&                 GetDestNavGraphEdgePtr(KyUInt32 destIdx)   const;
    const NavGraphVertexPtr&               GetDestNavGraphVertexPtr(KyUInt32 destIdx) const;
    MultiDestinationPathFinderQueryResult  GetDestResult(KyUInt32 destIdx)            const;

    MultiDestinationPathFinderTryCanGoMode GetTryCanGoMode()                          const;
    PathFinderQueryComputeChannelMode      GetComputeChannelMode()                    const;
    const ChannelComputerConfig&           GetChannelComputerConfig()                 const;
    KyUInt32                               GetNumberOfProcessedNodePerFrame()         const;
    Path*                                  GetPath(KyUInt32 destIdx)                  const;
    Path*                                  GetAbstractPath(KyUInt32 destIdx)          const;
    KyFloat32                              GetRawCost(KyUInt32 destIdx)               const;

    const PositionSpatializationRange&     GetPositionSpatializationRange() const;

    void ComputePathRefinerConfigInternals();
    const PathRefinerConfig& GetPathRefinerConfig() const;
    void SetPathClamperFlagMask(PathClamperFlagMask pathClamperFlagMask); // Call this method after BindToDatabase()

    KyUInt32 GetNavigationProfileId() const;

    KyUInt32 GetFinalOutputMode() const; 
    
public:
    void BindToDatabase(Database* database);
    void Initialize(const PathFinderPoint& start);
    void AddDestination(const PathFinderPoint& additionalDest);

    void SetFinalOutputMode(KyUInt32 finalOutputMode); 
    void SetTryCanGoMode(MultiDestinationPathFinderTryCanGoMode tryCanGoMode);
    void SetComputeChannelMode(PathFinderQueryComputeChannelMode computeChannelMode);
    void SetChannelComputerConfig(const ChannelComputerConfig& channelComputerConfig);
    void SetPropagationFilter(Ptr<CellFilter> propagationFilter);
    void SetPositionSpatializationRange(const PositionSpatializationRange& positionSpatializationRange);
    void SetFromOutsideNavMeshDistance(KyFloat32 fromOutsideNavMeshDistance);
    void SetToOutsideNavMeshDistance(KyFloat32 toOutsideNavMeshDistance);
    void SetNumberOfProcessedNodePerFrame(KyUInt32 numberOfProcessedNodePerFrame);
    void SetAbstractGraphTraversalMode(PathFinderAbstractGraphTraversalMode abstractGraphTraversalMode);
    void SetNavigationProfileId(KyUInt32 navigationProfileId);

protected :
    BaseMultiDestinationPathFinderQuery();
    virtual ~BaseMultiDestinationPathFinderQuery() {}
    void SetResult(MultiDestinationPathFinderQueryResult result);

    void InitCellFilterFromAbstractGraph();

public: // internal
    Path* GetRawPath(KyUInt32 destIdx) const; // For Debug only.

protected :
    KyUInt32 m_navigationProfileId;

    MultiDestinationPathFinderTryCanGoMode m_tryCanGoMode;
    PathFinderQueryComputeChannelMode m_computeChannelMode; 
    ChannelComputerConfig m_channelComputerConfig; 
    KyUInt32 m_finalOutputMode; 
    KyUInt32 m_dataBaseChangeIdx;     

    PathFinderPoint m_start;
    KyArray<PathFinderPoint> m_destinations;

    TraversalParameters m_traversalParams;
    PathRefinerConfig m_pathRefinerConfig;
    PathClamperFlagMask m_pathClamperFlagMask;

    Ptr<CellFilter> m_propagationCellFilter;

    KyArray<Ptr<Path> > m_paths;
    KyArray<Ptr<Path> > m_abstractPaths;
    KyArray<Ptr<Path> > m_rawPaths; // For Debug only.
    KyArrayPOD<KyFloat32>  m_rawCosts;

    KyArray<MultiDestinationPathFinderQueryResult> m_resultPerDestinations;

    MultiDestinationPathFinderQueryResult m_result; 

protected:
    Ptr<Path> m_currentRawPath;

public:
    bool m_enableAdvancedVisualDebug;
};


KY_INLINE void PathFinderPoint::SetPosition(const Vec3f& pos3f) { m_pos3f = pos3f; }

KY_INLINE void PathFinderPoint::SetTrianglePtr(const NavTrianglePtr& trianglePtr)
{
    m_trianglePtr = trianglePtr;
    KY_LOG_WARNING_IF(m_navGraphEdgePtr[0].IsValid(), ("You are setting a NavTrianglePtr whereas a NavGraphEdgePtr was set too."));
    KY_LOG_WARNING_IF(m_navGraphVertexPtr.IsValid(), ("You are setting a NavTrianglePtr whereas a NavGraphVertexPtr was set too."));
    m_navGraphEdgePtr[0].Invalidate();
    m_navGraphEdgePtr[1].Invalidate();
    m_navGraphVertexPtr.Invalidate();
}

KY_INLINE void PathFinderPoint::SetNavGraphEdgePtr(const NavGraphEdgePtr& navGraphEdgePtr, NavGraphEdgeDirection navGraphEdgePathfindMode)
{
    m_navGraphEdgePtr[0] = navGraphEdgePtr; 
    if(navGraphEdgePathfindMode == NAVGRAPHEDGE_BIDIRECTIONAL)
        m_navGraphEdgePtr[1] = navGraphEdgePtr.GetOppositeNavGraphEdgePtr();
    else
        m_navGraphEdgePtr[1].Invalidate();

    KY_LOG_WARNING_IF(m_trianglePtr.IsValid(), ("You are setting a NavGraphEdgePtr whereas a NavTrianglePtr was set too."));
    KY_LOG_WARNING_IF(m_navGraphVertexPtr.IsValid(), ("You are setting a NavGraphEdgePtr whereas a NavGraphVertexPtr was set too."));
    m_trianglePtr.Invalidate();
    m_navGraphVertexPtr.Invalidate();
}

KY_INLINE void PathFinderPoint::SetNavGraphVertexPtr(const NavGraphVertexPtr& navGraphVertexPtr)
{
    m_navGraphVertexPtr = navGraphVertexPtr; 
    KY_LOG_WARNING_IF(m_trianglePtr.IsValid(), ("You are setting a NavGraphVertexPtr whereas a NavTrianglePtr was set too."));
    KY_LOG_WARNING_IF(m_navGraphEdgePtr[0].IsValid(), ("You are setting a NavGraphVertexPtr whereas a NavGraphEdgePtr was set too."));
    m_trianglePtr.Invalidate();
    m_navGraphEdgePtr[0].Invalidate();
    m_navGraphEdgePtr[1].Invalidate();
}

KY_INLINE void BaseMultiDestinationPathFinderQuery::SetFinalOutputMode(KyUInt32 finalOutputMode)                                 { m_finalOutputMode       = finalOutputMode;       }
KY_INLINE void BaseMultiDestinationPathFinderQuery::SetTryCanGoMode(MultiDestinationPathFinderTryCanGoMode tryCanGoMode)         { m_tryCanGoMode          = tryCanGoMode;          }
KY_INLINE void BaseMultiDestinationPathFinderQuery::SetResult(MultiDestinationPathFinderQueryResult result)                      { m_result                = result;                }
KY_INLINE void BaseMultiDestinationPathFinderQuery::SetComputeChannelMode(PathFinderQueryComputeChannelMode computeChannelMode)  { m_computeChannelMode    = computeChannelMode;    }
KY_INLINE void BaseMultiDestinationPathFinderQuery::SetChannelComputerConfig(const ChannelComputerConfig& channelComputerConfig) { m_channelComputerConfig = channelComputerConfig; }
KY_INLINE void BaseMultiDestinationPathFinderQuery::SetPropagationFilter(Ptr<CellFilter> propagationFilter)                      { m_propagationCellFilter  = propagationFilter;    }

KY_INLINE MultiDestinationPathFinderQueryResult BaseMultiDestinationPathFinderQuery::GetResult() const { return m_result; }

KY_INLINE KyUInt32 BaseMultiDestinationPathFinderQuery::GetFinalOutputMode() const  { return m_finalOutputMode; }

KY_INLINE const Vec3f&              BaseMultiDestinationPathFinderQuery::GetStartPos()                const { return m_start.m_pos3f;              }
KY_INLINE const NavTrianglePtr&     BaseMultiDestinationPathFinderQuery::GetStartTrianglePtr()        const { return m_start.m_trianglePtr;        }
KY_INLINE const NavGraphEdgePtr&    BaseMultiDestinationPathFinderQuery::GetStartNavGraphEdgePtr()    const { return m_start.m_navGraphEdgePtr[0]; }
KY_INLINE const NavGraphVertexPtr&  BaseMultiDestinationPathFinderQuery::GetStartNavGraphVertexPtr()  const { return m_start.m_navGraphVertexPtr;  }

KY_INLINE KyUInt32                  BaseMultiDestinationPathFinderQuery::GetDestCount()                               const { return m_destinations.GetCount();                    }
KY_INLINE const Vec3f&              BaseMultiDestinationPathFinderQuery::GetDestPos(KyUInt32 destIdx)                 const { return m_destinations[destIdx].m_pos3f;              }
KY_INLINE const NavTrianglePtr&     BaseMultiDestinationPathFinderQuery::GetDestTrianglePtr(KyUInt32 destIdx)         const { return m_destinations[destIdx].m_trianglePtr;        }
KY_INLINE const NavGraphEdgePtr&    BaseMultiDestinationPathFinderQuery::GetDestNavGraphEdgePtr(KyUInt32 destIdx)     const { return m_destinations[destIdx].m_navGraphEdgePtr[0]; }
KY_INLINE const NavGraphVertexPtr&  BaseMultiDestinationPathFinderQuery::GetDestNavGraphVertexPtr(KyUInt32 destIdx)   const { return m_destinations[destIdx].m_navGraphVertexPtr;  }
KY_INLINE MultiDestinationPathFinderQueryResult  BaseMultiDestinationPathFinderQuery::GetDestResult(KyUInt32 destIdx) const { return m_resultPerDestinations[destIdx];             }

KY_INLINE MultiDestinationPathFinderTryCanGoMode BaseMultiDestinationPathFinderQuery::GetTryCanGoMode()                  const { return m_tryCanGoMode;           }
KY_INLINE Path*                                  BaseMultiDestinationPathFinderQuery::GetPath(KyUInt32 destIdx)          const { return m_paths[destIdx];         }
KY_INLINE Path*                                  BaseMultiDestinationPathFinderQuery::GetAbstractPath(KyUInt32 destIdx)  const { return m_abstractPaths[destIdx]; }
KY_INLINE Path*                                  BaseMultiDestinationPathFinderQuery::GetRawPath(KyUInt32 destIdx)       const { return m_rawPaths[destIdx];      }
KY_INLINE KyFloat32                              BaseMultiDestinationPathFinderQuery::GetRawCost(KyUInt32 destIdx)       const { return m_rawCosts[destIdx];      }
KY_INLINE PathFinderQueryComputeChannelMode      BaseMultiDestinationPathFinderQuery::GetComputeChannelMode()            const { return m_computeChannelMode;     }
KY_INLINE const ChannelComputerConfig&           BaseMultiDestinationPathFinderQuery::GetChannelComputerConfig()         const { return m_channelComputerConfig;  }
KY_INLINE Ptr<CellFilter>                        BaseMultiDestinationPathFinderQuery::GetPropagationCellFilter()         const { return m_propagationCellFilter;  }


KY_INLINE KyFloat32 BaseMultiDestinationPathFinderQuery::GetFromOutsideNavMeshDistance()    const { return m_traversalParams.GetFromOutsideNavMeshDistance();    }
KY_INLINE KyFloat32 BaseMultiDestinationPathFinderQuery::GetToOutsideNavMeshDistance()      const { return m_traversalParams.GetToOutsideNavMeshDistance();      }
KY_INLINE KyUInt32  BaseMultiDestinationPathFinderQuery::GetNumberOfProcessedNodePerFrame() const { return m_traversalParams.GetNumberOfProcessedNodePerFrame(); }


KY_INLINE void BaseMultiDestinationPathFinderQuery::SetFromOutsideNavMeshDistance(KyFloat32 fromOutsideNavMeshDistance)
{
    m_traversalParams.SetFromOutsideNavMeshDistance(fromOutsideNavMeshDistance);
}
KY_INLINE void BaseMultiDestinationPathFinderQuery::SetToOutsideNavMeshDistance(KyFloat32 toOutsideNavMeshDistance)
{
    m_traversalParams.SetToOutsideNavMeshDistance(toOutsideNavMeshDistance);
}
KY_INLINE void BaseMultiDestinationPathFinderQuery::SetNumberOfProcessedNodePerFrame(KyUInt32 numberOfProcessedNodePerFrame)
{
    m_traversalParams.SetNumberOfProcessedNodePerFrame(numberOfProcessedNodePerFrame);
}
KY_INLINE void BaseMultiDestinationPathFinderQuery::SetPositionSpatializationRange(const PositionSpatializationRange& positionSpatializationRange)
{
    m_traversalParams.SetPositionSpatializationRange(positionSpatializationRange);
}

KY_INLINE void BaseMultiDestinationPathFinderQuery::SetAbstractGraphTraversalMode(PathFinderAbstractGraphTraversalMode abstractGraphTraversalMode)
{
    m_traversalParams.SetAbstractGraphTraversalMode(abstractGraphTraversalMode);
}


KY_INLINE const PositionSpatializationRange& BaseMultiDestinationPathFinderQuery::GetPositionSpatializationRange() const { return m_traversalParams.GetPositionSpatializationRange(); }

KY_INLINE void BaseMultiDestinationPathFinderQuery::ComputePathRefinerConfigInternals()              { m_pathRefinerConfig.ComputeInternals();}
KY_INLINE const PathRefinerConfig& BaseMultiDestinationPathFinderQuery::GetPathRefinerConfig() const { return m_pathRefinerConfig;}
KY_INLINE void BaseMultiDestinationPathFinderQuery::SetPathClamperFlagMask(PathClamperFlagMask pathClamperFlagMask) { m_pathClamperFlagMask = pathClamperFlagMask; }

KY_INLINE void     BaseMultiDestinationPathFinderQuery::SetNavigationProfileId(KyUInt32 navigationProfileId) { m_navigationProfileId = navigationProfileId; }
KY_INLINE KyUInt32 BaseMultiDestinationPathFinderQuery::GetNavigationProfileId() const { return m_navigationProfileId; }

}

#endif //Navigation_BaseMultiDestinationPathFinderQueryResult_H