multidestinationpathfinderquery.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/queries/utils/basemultidestinationpathfinderquery.h"
#include "gwnavruntime/queries/utils/astartraversal.h"
#include "gwnavruntime/queries/utils/astartraversalcontext.h"
#include "gwnavruntime/queries/raycangoquery.h"
#include "gwnavruntime/database/database.h"
#include "gwnavruntime/path/path.h"
#include "gwnavruntime/channel/channelarraycomputer.h"
#include "gwnavruntime/math/box2iiterator.h"

namespace Kaim
{

template<class TraverseLogic>
class MultiDestinationPathFinderQuery : public BaseMultiDestinationPathFinderQuery
{
public:

    // ------------------------------ Functions -----------------------------

    MultiDestinationPathFinderQuery();
    virtual ~MultiDestinationPathFinderQuery();

    // ---------------------------------- Query framework functions ----------------------------------

    virtual void Advance(WorkingMemory* workingMemory);

    virtual void ReleaseWorkingMemoryOnCancelDuringProcess(WorkingMemory* workingMemory = nullptr);

public: // internal
    bool TraversalHasVisitedNavDataChanged();

protected:
    void SetFinish(WorkingMemory* workingMemory);
    KyResult ComputeStartGraphEdgeOrGraphVertexOrTriangle(PathFinderQueryUtils& queryUtils);
    KyResult ComputeStartTriangle(PathFinderQueryUtils& queryUtils);
    KyResult ComputeDestTriangleIfNeeded(PathFinderQueryUtils& queryUtils);
    KyResult InitializeContextAndStartNode(PathFinderQueryUtils& queryUtils);
    KyResult InitAllDestinationNodes();
    KyResult InitDestinationNodeFromDestNavGraphEdge(PathFinderPoint& dest);
    KyResult InitDestinationNodeFromDestNavGraphVertex(PathFinderPoint& dest);
    KyResult InitDestinationNodeFromDestNavTriangle(PathFinderPoint& dest);
    KyResult InitTraversalFromStartNode(PathFinderQueryUtils& queryUtils);
    void InitPropagationBounds();

    KyResult CheckNavDataChangeDuringTraversal(WorkingMemory* workingMemory);
    KyResult CheckNavDataChangeAfterTraversal(WorkingMemory* workingMemory);

    bool ShouldReturnAfterTryingCanGo(WorkingMemory* workingMemory);

    bool HasExplorationReachedAllDestNodes();
    bool HasExplorationReachedAtLeastADestNode();

    void ComputeRawCosts();
    void BuildRawPaths();
    PathEdgeType GetPathEdgeType(const AStarNode& predecessorNode, const AStarNode& node);

    bool ProcessFirstDestination();
    bool UpdateResultAndSelectFirstDestination();
    bool ProcessNextDestination(WorkingMemory* workingMemory);

    KyUInt32 CountAbstractGraphNodes(WorkingMemory* workingMemory, AStarNodeIndex& concreteDestAStarNodeIndex);
    void BuildAbstractPath(WorkingMemory* workingMemory, AStarNodeIndex& concreteDestAStarNodeIndex);

protected:
    class TraversalCustomizer
    {
    public:
        typedef TraverseLogic TLogic;

        TraversalCustomizer() : m_query(nullptr), m_traverseLogicUserData(nullptr) {}

        bool CanTraverse(const NavTriangleRawPtr& triangleRawPtr, KyFloat32* costMultiplier)       { return triangleRawPtr.CanBeTraversed<TLogic>(m_traverseLogicUserData, costMultiplier); }
        bool CanTraverse(const NavGraphEdgeRawPtr& edgeRawPtr, KyFloat32* costMultiplier)          { return edgeRawPtr.CanBeTraversed<TLogic>(m_traverseLogicUserData, costMultiplier); }
        bool CanEnterNavTag(const NavTag& exitNavTag, const NavTag& enterNavTag, const Vec3f& pos) { return TLogic::CanEnterNavTag(m_traverseLogicUserData, exitNavTag, enterNavTag, pos); }
        KyFloat32 GetHeuristicFromNodePosition(const Vec3f&) { return 0.0f; }

        void* GetTraverseLogicUserData() const { return m_traverseLogicUserData; }
        bool ShouldVisitNode(AStarTraversalContext* aStarTraversalContext, AStarNodeIndex nodeIndex);
        bool IsNodeOutsidePropagationBounds(AStarTraversalContext* aStarTraversalContext, AStarNodeIndex nodeIndex);

        bool ShouldOpenHalfEdgeNode(const NavHalfEdgeRawPtr& halfEdge, NavHalfEdgeRawPtr& pairHalfEdgeOfNewNode);
        bool ShouldOpenGraphVertexNode(const NavGraphVertexRawPtr&)      { return true; }
        bool ShouldOpenAbstractGraphNode(const AbstractGraphNodeRawPtr&) { return true; }
        bool ShouldOpenNavTriangleUnderAbstractGraphNode(const AbstractGraphNodeRawPtr& abstractGraphNodeRawPtr);

        Vec3f ComputeNodePositionOnHalfEdge(AStarTraversalContext* aStarTraversalContext, const Vec3f& startPosOfEdge, const Vec3f& endPosOfEdge, AStarNodeIndex predecessorNodeIndex);
        KyResult OnNavTriangleExplored(AStarTraversalContext* aStarTraversalContext, const NavTriangleRawPtr& navTriangleRawPtr, AStarNodeIndex currentNodeIndex);
        KyResult OnNavGraphEdgeExplored(AStarTraversalContext* aStarTraversalContext, const NavGraphEdgeRawPtr& navGraphEdgeRawPtr, AStarNodeIndex currentNodeIndex);

        MultiDestinationPathFinderQuery<TraverseLogic>* m_query;
        void* m_traverseLogicUserData;
    };

    AStarTraversal<TraversalCustomizer> m_traversal;
    KyArray<MonodirectionalRayCanGoQuery<TraverseLogic> > m_rayCanGoQueries;
    KyUInt32 m_currentProcessedDestIdx;
};

}

#include "gwnavruntime/queries/multidestinationpathfinderquery.inl"