channelsectionwidener.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_ChannelSectionWidener_H
#define Navigation_ChannelSectionWidener_H

#include "gwnavruntime/channel/channelbordersimplifier.h"
#include "gwnavruntime/navmesh/identifiers/navtriangleptr.h"
#include "gwnavruntime/containers/sharedpoollist.h"
#include "gwnavruntime/math/vec3f.h"
#include "gwnavruntime/visualsystem/displaylist.h"



namespace Kaim
{

class Database;
class Channel;
class Gate;
class ScopedDisplayList;
class ChannelComputerConfig;

class ChannelSectionWidener
{
public:
    ChannelSectionWidener();

    void Clear();

    void SetConfig(const ChannelComputerConfig& config);
    void SetStartGatePosTrianglePtr(const NavTrianglePtr& sectionStartNavTrianglePtr);

    void InitializeWidener(Database* database, Channel* channel, const Vec2f& pathEdgeDir2d, KyFloat32 pathEdgeLength,
        const Vec3f& edgeStartPos, const Vec3f& edgeEndPos, const Vec2f& startGateDirOnLeft,
        const Vec2f& endGateDirOnLeft, GateType endGateType, const Vec3f& endGateFixedPos);

    enum WideningSide
    {
        WideningSide_LeftAndRight,
        WideningSide_LeftOnly,
        WideningSide_RightOnly
    };

    template<class TLogic>
    KyResult WidenChannel(WorkingMemory* workingMem, void* traverseLogicUserData, QueryDynamicOutput* queryDynamicOutput, WideningSide wideningSide);

public: //Internal
    void ConfigureIntersector(PolylineCastIntersector &intersector);
    KyResult ProcessPolylineCastResult(WorkingMemory* workingMem, PolylineCastIntersector &intersector, WideningSide wideningSide);

    KyResult AdaptExtremityGatesToPolyline(WideningSide wideningSide);
    KyResult AddIntermediaryGatesFromPolylines();


    void MoveIteratorsBeyondX(SharedPoolList<Vec2f>::Iterator& currentLeftIt, SharedPoolList<Vec2f>::Iterator& currentRightIt, KyFloat32 xValue);

    KyResult FillChannelWithIntermediarygates(WorkingMemory* workingMemory, WideningSide wideningSide);

    KyResult FixStartGateWidth(WideningSide wideningSide);
    KyResult AddLastGate(WideningSide wideningSide);

    void ComputeTrapezoid();
    void FillExtremityContext(PolylineExtremityContext& context, bool isOnLeft);

    class GateCandidate
    {
    public :
        GateCandidate() : m_x(KyFloat32MAXVAL), m_yOnLeft(KyFloat32MAXVAL), m_yOnRight(KyFloat32MAXVAL) {}

    public :
        KyFloat32 m_x;
        KyFloat32 m_yOnLeft;
        KyFloat32 m_yOnRight;
    };

    void ComputeGateYValuesNoConstraint(const SharedPoolList<Vec2f>::Iterator& currentLeftIt,
        const SharedPoolList<Vec2f>::Iterator& currentRightIt, GateCandidate& newGate);

    KyResult ComputeGateYValuesWithXandYDiffConstraint(const SharedPoolList<Vec2f>::Iterator& currentLeftIt,
        const SharedPoolList<Vec2f>::Iterator& currentRightIt, GateCandidate& currentGate, GateCandidate& newGate);

    KyResult ComputeGateYValuesWithXConstraintFromCurrentGate(const SharedPoolList<Vec2f>::Iterator& currentLeftIt,
        const SharedPoolList<Vec2f>::Iterator& currentRightIt, GateCandidate& currentGate, GateCandidate& newGate,
        KyFloat32* differenceFromBorder = KY_NULL, KyFloat32* moreConstraintXForMaxDist = KY_NULL);
    KyResult ComputeGateYValuesWithXConstraintFromCurrentGateOnLeft(const SharedPoolList<Vec2f>::Iterator& currentLeftIt,
        GateCandidate &currentGate, GateCandidate &newGate, KyFloat32& differenceFromBorder, KyFloat32& moreConstraintX);
    KyResult ComputeGateYValuesWithXConstraintFromCurrentGateOnRight(const SharedPoolList<Vec2f>::Iterator& currentRightIt,
        GateCandidate &currentGate, GateCandidate &newGate, KyFloat32& differenceFromBorder, KyFloat32& moreConstraintX);

    KyResult ComputeYValuesOfCurrentGateAndEndGate(const SharedPoolList<Vec2f>::Iterator& currentLeftIt, const SharedPoolList<Vec2f>::Iterator& currentRightIt,
        GateCandidate& currentGate, KyFloat32* differenceFromBorder = KY_NULL, KyFloat32* moreConstraintXForMaxDist = KY_NULL);
    KyResult ComputeYValuesOfCurrentGateAndEndGateOnLeft(const SharedPoolList<Vec2f>::Iterator& currentRightIt, GateCandidate& currentGate,
        KyFloat32& differenceFromBorder, KyFloat32& moreConstraintX);
    KyResult ComputeYValuesOfCurrentGateAndEndGateOnRight(const SharedPoolList<Vec2f>::Iterator& currentRightIt, GateCandidate& currentGate,
        KyFloat32& differenceFromBorder, KyFloat32& moreConstraintX);

    KyResult ComputelastIntermediaryGateAndEndGate(SharedPoolList<Vec2f>::Iterator &currentLeftIt, SharedPoolList<Vec2f>::Iterator &currentRightIt, GateCandidate& currentGate);
    KyResult AddIntermediaryGateAtX(SharedPoolList<Vec2f>::Iterator& currentLeftIt, SharedPoolList<Vec2f>::Iterator& currentRightIt, GateCandidate& currentGate, KyFloat32 xValue);

    void AddGatesToRespectMaxDistanceBetweenGates(const SharedPoolList<Vec2f>::Iterator& currentLeftIt,
        const SharedPoolList<Vec2f>::Iterator& currentRightIt, GateCandidate& currentGate, GateCandidate& newGate);

    SharedPoolList<Vec2f>::Iterator GetMoreConstraintSlopeForward(const Vec2f& point, SharedPoolList<Vec2f>& polyline,
        const SharedPoolList<Vec2f>::Iterator& currentIt, const SharedPoolList<Vec2f>::Iterator& firstIteratorNotToTest);
    SharedPoolList<Vec2f>::Iterator GetMoreConstraintSlopeBackward(const Vec2f& point, SharedPoolList<Vec2f>& polyline,
        const SharedPoolList<Vec2f>::Iterator& currentIt, const SharedPoolList<Vec2f>::Iterator& firstIteratorNotToTest);

    KY_INLINE bool CanMoveLeftStartPos()  const { return m_startType !=   LeftTurnEnd; }
    KY_INLINE bool CanMoveRightStartPos() const { return m_startType !=   RightTurnEnd; }
    KY_INLINE bool CanMoveLeftEndPos()    const { return   m_endType !=  LeftTurnStart; }
    KY_INLINE bool CanMoveRightEndPos()   const { return   m_endType != RightTurnStart; }
public:
    ChannelBorderSimplifier m_simplifier;
    Database* m_database;
    Channel* m_channel;

    // input
    Vec3f m_edgeStartPos;
    Vec3f m_edgeEndPos;
    Vec2f m_startGateDirOnLeft;
    Vec2f m_endGateDirOnLeft;
    Vec3f m_endGateFixedPos; // not relevant for endGate
    GateType m_startType;
    GateType m_endType;

    NavTrianglePtr m_sectionStartNavTriangle;
    WorldIntegerPos m_edgeStartIntegerPos;

    // input & output
    Vec2f m_startGateLeftPos;
    Vec2f m_startGateRightPos;

    // working members
    KyFloat32 m_margin;
    Vec3f m_trapezoidStartLeftPos;
    Vec3f m_trapezoidEndLeftPos;
    Vec3f m_trapezoidStartRightPos;
    Vec3f m_trapezoidEndRightPos;
    KyArray<GateCandidate> m_gates;

    SharedPoolList<GateCandidate>::NodePool m_gatePool;
    SharedPoolList<GateCandidate> m_gateCandidates;

    Vec2f m_startLeftLocal;
    Vec2f m_startRightLocal;
    Vec2f m_endLeftLocal;
    Vec2f m_endRightLocal;
    KyFloat32 m_startGateMaxX;
    KyFloat32 m_endGateMinX;

    KyFloat32 m_tooFarFromBorderThreshold;
    KyFloat32 m_internalMinDistFromBorder;
    ScopedDisplayList* m_displayList;
};

// Class for scoped object defined in ChannelSectionWidener::WidenChannel
// do not use KY_EXIT_SCOPE_BEGIN inside function to avoid link error on vc8 with scoped object declared in template function
KY_EXIT_CLASS_SCOPE_BEGIN(ChannelSectionWidener, WidenChannel)
KY_DELETE_AND_SET_NULL(self->m_displayList);
KY_EXIT_CLASS_SCOPE_END

template<class TLogic>
inline KyResult ChannelSectionWidener::WidenChannel(WorkingMemory* workingMem, void* traverseLogicUserData, QueryDynamicOutput* queryDynamicOutput, WideningSide wideningSide)
{
    KY_EXIT_SCOPE_INSTANCE(ChannelSectionWidener, WidenChannel);

    if (m_simplifier.m_advancedVisualDebuggingEnabled)
    {
        m_displayList = KY_NEW ScopedDisplayList(m_simplifier.m_database->GetWorld());
        m_displayList->InitSingleFrameLifespan("temp Gates", "ChannelSectionWidener");
    }

    // collect borders
    PolylineCastIntersector intersector;
    ConfigureIntersector(intersector);

    if (PolylineCastHelper::RunPolylineCast<TLogic>(workingMem, m_database, traverseLogicUserData, queryDynamicOutput, intersector) == KY_ERROR)
        return KY_ERROR;

    return ProcessPolylineCastResult(workingMem, intersector, wideningSide);
}

} // namespace Kaim

#endif // Navigation_ChannelSectionWidener_H