rayqueryutils.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/database/database.h"
#include "gwnavruntime/navmesh/intersections.h"
#include "gwnavruntime/queries/querydynamicoutput.h"
#include "gwnavruntime/queries/trianglefromposandtriangleseedquery.h"
#include "gwnavruntime/querysystem/workingmemcontainers/workingmemarray.h"

namespace Kaim
{

class Box2f;
class NavTrianglePtr;
class NavTriangleRawPtr;
class NavHalfEdgePtr;
class NavHalfEdgeRawPtr;
class TriangleFromPosAndTriangleSeedQuery;
class WorldIntegerPos;
class LocalStoreActiveDataWorkspaceContext;
class RawNavTagSubSegment;
class WorkingMemory;
class Path;
template <class T>
class WorkingMemArray;

enum ComputeCostMode
{
    QUERY_DO_NOT_COMPUTE_COST,       
    QUERY_COMPUTE_COST_ALONG_3DAXIS, 
};

class CompositeSubSegment
{
public:
    CompositeSubSegment() {}
    CompositeSubSegment(const Vec3f& startPos3f, const NavTriangleRawPtr& startTriangle, KyFloat32 costMultiplier)
    {
        m_rawNavTagSubSegment.m_navTriangleRawPtr = startTriangle;
        m_rawNavTagSubSegment.m_entrancePos3f = startPos3f;
        m_costMultiplierSubSegment.m_costMultiplier = costMultiplier;
        m_costMultiplierSubSegment.m_entrancePos3f = startPos3f;
    }

public:
    RawNavTagSubSegment m_rawNavTagSubSegment;
    CostMultiplierSubSegment m_costMultiplierSubSegment;
};

class RayQueryUtils
{
    RayQueryUtils();

public:
    RayQueryUtils(const Vec3f& startPos3f, const Vec3f& destPos3f, void* traverseLogicUserData, ScopedDynamicOutput& scopeAutoSaveDynamicOutput,
                  DynamicOutputMode savingMode = QUERY_SAVE_NOTHING, KyFloat32 firstCostMultiplier = 1.f, ComputeCostMode computeCostMode = QUERY_DO_NOT_COMPUTE_COST);

    KyResult StorePropagationData(KyFloat32 integerPrecision, const NavHalfEdgeRawPtr& currentPairHalfEdgeRawPtr, CompositeSubSegment& subSegment, const CoordPos64& cellOrigin,
                                  const NavFloorBlob* navFloorBlob);

    template <class TraverseLogic>
    KyResult StoreOrComputeCostFromPropagationData(KyFloat32 integerPrecision, const NavHalfEdgeRawPtr& currentPairHalfEdgeRawPtr, CompositeSubSegment& currentSubSegment,
                                                   const CoordPos64& cellOrigin, const NavFloorBlob* navFloorBlob, KyFloat32 nextCostMultiplier, KyFloat32& cost);

    KyInt32 GetFirstEdgeToCrossInStartTriangle(const NavTriangleRawPtr& startTriangleRawPtr, const CoordPos64& startCoordPos, const CoordPos64& destCoordPos);

    bool IsEdgeBeyondDestPos(const CoordPos64& cellOrigin, const NavFloorBlob& navFloorBlob, NavHalfEdgeIdx navHalfEdgeIdx, const CoordPos64& segmentStartCoordPos,
                             const CoordPos64& segmentDestCoordPos, const CoordPos64& destinationCoordPos, const KyInt64& startToDestSquareDist);

    void UpdateCostAlongAxis(const Vec3f& endOfCostMultiplierSegment, KyFloat32& cost);

    template <class TLogic>
    static KY_INLINE bool NeedToCommputeOrStoreDataFromCost(DynamicOutputMode dynamicOutputMode, ComputeCostMode computeCostMode = QUERY_DO_NOT_COMPUTE_COST)
    {
        return NeedToCommputeOrStoreDataFromCost(typename TLogic::CostMultiplierUsage(), dynamicOutputMode, computeCostMode);
    }

    KyResult PushTriangleInDynamicOutput(DynamicOutputMode dynamicOutputMode, const NavTriangleRawPtr& startTriangleRawPtr);
    static KyResult FinalizeLastSubSegment(DynamicOutputMode dynamicOutputMode, ScopedDynamicOutput& scopedDynamicOutput, const Vec3f& lastPos, CompositeSubSegment& subSegment);

protected:
    KyResult StoreTriangle(const NavHalfEdgeRawPtr& currentPairHalfEdgeRawPtr);
    KyResult ComputeAndStoreEdgeIntersection(KyFloat32 integerPrecision, const NavHalfEdgeRawPtr& currentPairHalfEdgeRawPtr, const CoordPos64& cellOrigin,
                                             const NavFloorBlob* navFloorBlob, Vec3f& intersectionWithEdge);

    KyResult FinalizeAndStoreNavTagSubSegment(const Vec3f& intersectionWithEdge, const NavHalfEdgeRawPtr& currentPairHalfEdgeRawPtr, RawNavTagSubSegment& navTagSubSegment);
    KyResult FinalizeAndStoreCostMultiplierSubSegment(const Vec3f& intersectionWithEdge, KyFloat32 nextCostMultiplier, CostMultiplierSubSegment& costMultSubSegment);

    bool EdgeIsANavTagBoundary(const NavHalfEdgeRawPtr& currentPairHalfEdgeRawPtr, RawNavTagSubSegment& navTagSubSegment);

    void ComputeIntersectionWithEdge(KyFloat32 integerPrecision, const NavHalfEdgeRawPtr& currentPairHalfEdgeRawPtr, const CoordPos64& cellOrigin, const NavFloorBlob* navFloorBlob,
                                     Vec3f& intersection);

    KyResult StoreOrComputeCostFromPropagationData(KyFloat32 integerPrecision, const NavHalfEdgeRawPtr& currentPairHalfEdgeRawPtr, CompositeSubSegment& currentSubSegment,
                                                   const CoordPos64& cellOrigin, const NavFloorBlob* navFloorBlob, KyFloat32 nextCostMultiplier, bool updateDataFromCostLimit,
                                                   KyFloat32& cost);

    static KY_INLINE bool NeedToCommputeOrStoreDataFromCost(const LogicWithNoCustomCost&, DynamicOutputMode, ComputeCostMode) { return false; }
    static KY_INLINE bool NeedToCommputeOrStoreDataFromCost(const LogicWithCostPerNavTag&, DynamicOutputMode dynamicOutputMode, ComputeCostMode computeCostMode)
    {
        return NeedToCommputeOrStoreDataFromCost_Impl(dynamicOutputMode, computeCostMode);
    }
    static KY_INLINE bool NeedToCommputeOrStoreDataFromCost(const LogicWithCostPerTriangle&, DynamicOutputMode dynamicOutputMode, ComputeCostMode computeCostMode)
    {
        return NeedToCommputeOrStoreDataFromCost_Impl(dynamicOutputMode, computeCostMode);
    }
    static KY_INLINE bool NeedToCommputeOrStoreDataFromCost_Impl(DynamicOutputMode dynamicOutputMode, ComputeCostMode computeCostMode)
    {
        return (dynamicOutputMode & QUERY_SAVE_COSTMULTIPLIERSUBSEGMENTS) != 0 || computeCostMode == QUERY_COMPUTE_COST_ALONG_3DAXIS;
        ;
    }

public:
    Vec3f m_startPos3f;
    Vec3f m_destPos3f;
    void* m_traverseLogicUserData;

    DynamicOutputMode m_dynamicOutputMode;
    ComputeCostMode m_computeCostMode;
    ScopedDynamicOutput* m_scopedDynamicOutput;

    KyFloat32 m_dist3DOverSquareDist2d;
    Vec3f m_currentCostMultiplierStartPos;
    KyFloat32 m_currentCostMultiplier;
};

template <class TLogic>
KY_INLINE KyResult RayQueryUtils::StoreOrComputeCostFromPropagationData(KyFloat32 integerPrecision, const NavHalfEdgeRawPtr& currentPairHalfEdgeRawPtr,
                                                                        CompositeSubSegment& currentSubSegment, const CoordPos64& cellOrigin, const NavFloorBlob* navFloorBlob,
                                                                        KyFloat32 nextCostMultiplier, KyFloat32& cost)
{
    bool updateDataFromCostLimit = NeedToCommputeOrStoreDataFromCost<TLogic>(m_dynamicOutputMode, m_computeCostMode);
    return StoreOrComputeCostFromPropagationData(integerPrecision, currentPairHalfEdgeRawPtr, currentSubSegment, cellOrigin, navFloorBlob, nextCostMultiplier,
                                                 updateDataFromCostLimit, cost);
}
}