basemultidestinationpathfinderquery.h

Go to the documentation of this file.

/*
* Copyright 2016 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.
*/



#pragma once

#include "gwnavruntime/navmesh/identifiers/navtriangleptr.h"
#include "gwnavruntime/querysystem/workingmemcontainers/navdatachangeindexingrid.h"
#include "gwnavruntime/queries/ipathfinderquery.h"
#include "gwnavruntime/queries/utils/pathfinderquerytrycangomode.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_NEXT_DESTINATION,             
    MULTIDESTINATIONPATHFINDER_PROCESSING_ABSTRACT_PATH,                
    MULTIDESTINATIONPATHFINDER_PROCESSING_REFINING_INIT,                
    MULTIDESTINATIONPATHFINDER_PROCESSING_REFINING_RESETCOST,           
    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 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) { m_pos3f = 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();

    MultiDestinationPathFinderQueryResult  GetResult()                                const { return m_result; }
    KyFloat32                              GetFromOutsideNavMeshDistance()            const { return m_traversalParams.m_fromOutsideNavMeshDistance; }
    KyFloat32                              GetToOutsideNavMeshDistance()              const { return m_traversalParams.m_toOutsideNavMeshDistance;   }

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

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

    PathFinderQueryTryCanGoMode            GetTryCanGoMode()                          const { return m_tryCanGoMode;           }
    PathFinderQueryComputeChannelMode      GetComputeChannelMode()                    const { return m_computeChannelMode;     }
    const ChannelComputerConfig&           GetChannelComputerConfig()                 const { return m_channelComputerConfig;  }
    KyUInt32                               GetNumberOfProcessedNodePerFrame()         const { return m_traversalParams.m_numberOfVisitedNodePerAdvance; }
    Path*                                  GetPath(KyUInt32 destIdx)                  const { return m_paths[destIdx];         }
    Path*                                  GetAbstractPath(KyUInt32 destIdx)          const { return m_abstractPaths[destIdx]; }
    KyFloat32                              GetRawCost(KyUInt32 destIdx)               const { return m_rawCosts[destIdx];      }

    const PositionSpatializationRange&     GetPositionSpatializationRange()           const { return m_traversalParams.m_positionSpatializationRange; }

    void ComputePathRefinerConfigInternals() { m_pathRefinerConfig.ComputeInternals();}
    const PathRefinerConfig& GetPathRefinerConfig() const { return m_pathRefinerConfig; }
    void SetPathClamperFlagMask(PathClamperFlagMask pathClamperFlagMask) { m_pathClamperFlagMask = pathClamperFlagMask; } // Call this method after BindToDatabase()

    KyUInt32 GetNavigationProfileId() const { return m_navigationProfileId; }

    KyUInt32 GetFinalOutputMode() const { return m_finalOutputMode; } 

    void BindToDatabase(Database* database);
    void Initialize(const PathFinderPoint& start);
    void AddDestination(const PathFinderPoint& additionalDest);

    void SetFinalOutputMode(KyUInt32 finalOutputMode)                                 { m_finalOutputMode       = finalOutputMode;       }
    void SetTryCanGoMode(PathFinderQueryTryCanGoMode tryCanGoMode)                    { m_tryCanGoMode          = tryCanGoMode;          }
    void SetComputeChannelMode(PathFinderQueryComputeChannelMode computeChannelMode)  { m_computeChannelMode    = computeChannelMode;    }
    void SetChannelComputerConfig(const ChannelComputerConfig& channelComputerConfig) { m_channelComputerConfig = channelComputerConfig; }
    void SetPropagationFilter(Ptr<CellFilter> propagationFilter)                      { m_propagationCellFilter  = propagationFilter;    }
    void SetPositionSpatializationRange(const PositionSpatializationRange& positionSpatializationRange) { m_traversalParams.m_positionSpatializationRange = positionSpatializationRange; }
    void SetFromOutsideNavMeshDistance(KyFloat32 fromOutsideNavMeshDistance)                            { m_traversalParams.m_fromOutsideNavMeshDistance  = fromOutsideNavMeshDistance; }
    void SetToOutsideNavMeshDistance(KyFloat32 toOutsideNavMeshDistance)                                { m_traversalParams.m_toOutsideNavMeshDistance    = toOutsideNavMeshDistance; }
    void SetNumberOfProcessedNodePerFrame(KyUInt32 numberOfProcessedNodePerFrame)                       { m_traversalParams.m_numberOfVisitedNodePerAdvance = numberOfProcessedNodePerFrame; }
    void SetAbstractGraphTraversalMode(PathFinderAbstractGraphTraversalMode abstractGraphTraversalMode) { m_abstractGraphTraversalMode  = abstractGraphTraversalMode; }
    void SetNavigationProfileId(KyUInt32 navigationProfileId) { m_navigationProfileId = navigationProfileId; }

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

    void InitTraversalParamsAndPropagationCellFilter();

    void ReplaceNodesOfShortestPathOnMiddleOfNavHalfEdge(WorkingMemory* workingMemory);

public: // internal
    Path* GetRawPath(KyUInt32 destIdx) const { return m_rawPaths[destIdx]; }// For Debug only.

protected:
    KyUInt32 m_navigationProfileId;

    // configuration
    PathFinderQueryTryCanGoMode m_tryCanGoMode;
    PathFinderQueryComputeChannelMode m_computeChannelMode; 
    ChannelComputerConfig m_channelComputerConfig; 

    KyUInt32 m_finalOutputMode;   
    KyUInt32 m_lastAdvanceDatabaseChangeIdx; // used to test if there was a change in the database since last call to Advance

    bool m_isStartTriangleHooked; // computed
    PathFinderPoint m_start; // start
    KyArray<PathFinderPoint> m_destinations; // destinations

    TraversalParameters m_traversalParams;// computed
    PathRefinerConfig m_pathRefinerConfig; // configuration
    PathClamperFlagMask m_pathClamperFlagMask; // configuration
    PathFinderAbstractGraphTraversalMode m_abstractGraphTraversalMode; // configuration

    Ptr<CellFilter> m_propagationCellFilter; // computed

    // results
    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; // computed Updated during processing to indicate the result of the PathFinderQueryResult query.

protected:
    Ptr<Path> m_currentRawPath; // computed

public:
    FullDebug m_fullDebug; // configuration
};

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

}