channelsectionwidener.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/channel/channelbordersimplifier.h"
#include "gwnavruntime/navmesh/identifiers/navtriangleptr.h"
#include "gwnavruntime/containers/sharedpoollist.h"
#include "gwnavruntime/math/vec3f.h"
#include "gwnavruntime/visualsystem/displaylist.h"
#include "gwnavruntime/queries/fulldebug.h"
#include "channelcomputerconfig.h"

namespace Kaim
{

class Database;
class Channel;
class Gate;
class DisplayList;
class ChannelComputerConfig;

struct GateSide { enum Enum { Left, Right }; };

// currIsStartGate==true         currIsStartGate==false         //
// NOT ORTHOGONAL                ALWAYS ORTHOGONAL              //
// P=startPos                       P=(currGate.m_x,currGate_y) //
//  \                               |                           //
//   O--------                   ---O--------                   //
//   O=(0,0)                        O=(currGate.m_x,0)
struct GateCandidate
{
    enum Type { Start, Mid, End };

    GateCandidate(KyFloat32 x = 0.0f) : type(Mid), L(x, 0.0f), O(x, 0.0f), R(x, 0.0f), LCanMove(true), RCanMove(true) {}

    bool IsStart() const { return type == Start; }
    bool IsEnd()   const { return type == End; }

    KyFloat32 MinX() const { return Kaim::Min(L.x, R.x); }
    KyFloat32 MaxX() const { return Kaim::Max(L.x, R.x); }

    Type type;
    Vec2f L; // local, left
    Vec2f O; // local, on edge
    Vec2f R; // local, right
    bool LCanMove;
    bool RCanMove;
};

struct GateFitConstraint
{
    enum Type { PerfectFit_ = 0, ShrinkFit_, CollapseFit_, NoFit_ };

    GateFitConstraint() : fitType(NoFit_), shrinkY(0.0f), side(GateSide::Left) {}
    GateFitConstraint(Type fitType_, KyFloat32 shrinkY_, const Vec2f& C_, GateSide::Enum side_) : fitType(fitType_), shrinkY(shrinkY_), side(side_), C(C_) {}

    static GateFitConstraint PerfectFit() { return GateFitConstraint(PerfectFit_ , 0.0f, Vec2f(0.0f, 0.0f), GateSide::Left); }
    static GateFitConstraint ShrinkNewGateFit(KyFloat32 shrinkY_, const Vec2f& C_, GateSide::Enum side_)   { return GateFitConstraint(ShrinkFit_  , shrinkY_, C_, side_); }
    static GateFitConstraint ShrinkBothGatesFit(KyFloat32 shrinkY_, const Vec2f& C_, GateSide::Enum side_) { return GateFitConstraint(ShrinkFit_  , shrinkY_, C_, side_); }
    static GateFitConstraint CollapseFit(const Vec2f& C_, GateSide::Enum side_)                            { return GateFitConstraint(CollapseFit_,     0.0f, C_, side_); }
    static GateFitConstraint NoFit(const Vec2f& C_, GateSide::Enum side_)                                  { return GateFitConstraint(NoFit_      ,     0.0f, C_, side_); }

    bool HasFit() const { return fitType != NoFit_; }

    bool IsLessConstraining(const GateFitConstraint& other) const
    {
        if (fitType < other.fitType) return true;
        if (fitType > other.fitType) return false;
        return shrinkY < other.shrinkY;
    }

    Type fitType;
    KyFloat32 shrinkY;
    GateSide::Enum side;
    Vec2f C; // constraining point (local)
};

struct GateFit
{
    enum Type { Mid, End };
    GateFit() : m_type(Mid) {}
    bool HasFit() const             { return m_constraint.HasFit(); }
    KyFloat32 GetConstrainX() const { return m_constraint.C.x; }

    Type m_type;
    GateCandidate m_currGate;
    GateCandidate m_nextGate;
    GateFitConstraint m_constraint;
};

struct NextGateX
{
    enum Mode { Mid_Constrain, Mid_No_Constrain, End };
    NextGateX(KyFloat32 x_, Mode mode_) : x(x_), mode(mode_) {}
    KyFloat32 x;
    Mode mode;
};

struct WidenInput
{
    WidenInput() : type(UndefinedGateType) {}
    WidenInput(const Vec3f pos_, const Vec2f left_dir_, GateType type_) : O(pos_), left_dir(left_dir_), type(type_) {}
    Vec3f O;
    Vec2f left_dir;
    GateType type;
};

class ChannelSectionWidener
{
public:
    ChannelSectionWidener();

    void Clear();

    void InitializeForSection(Database* database, const ChannelComputerConfig& channelConfig, Channel* channel, KyUInt32 sectionIndex, const WidenInput& startInput, const WidenInput& endInput, const Vec3f& endGateFixedPos,
                              const NavTrianglePtr& startNavTrianglePtr, const FullDebug& fullDebug);

    enum WidenSide { WidenLeft = 1, WidenRight = 2, WidenLeftRight = WidenLeft | WidenRight };

    template<class TLogic>
    KyResult WidenSection(WorkingMemory* workingMem, void* traverseLogicUserData, QueryDynamicOutput* queryDynamicOutput, WidenSide widenSide);

private: //Internal
    void ConfigureIntersector(PolylineCastIntersector& intersector);
    KyResult SimplifyPolylinesAndMakeGates(WorkingMemory* workingMem, PolylineCastIntersector& intersector, WidenSide widenSide);

    void FillExtremityContext(PolylineExtremityContext& context, bool isOnLeft);

    void ComputeTrapezoid();

    bool CanMoveLeftStartPos()  const { return m_startInput.type != LeftTurnEnd; }
    bool CanMoveRightStartPos() const { return m_startInput.type != RightTurnEnd; }
    bool CanMoveLeftEndPos()    const { return   m_endInput.type != LeftTurnStart; }
    bool CanMoveRightEndPos()   const { return   m_endInput.type != RightTurnStart; }

    void CollapseSimplifiedPolyline_Left();
    void CollapseSimplifiedPolyline_Right();

    void InitLeftAndRightArrays(WidenSide widenSide);

    // GateCandidates (Local) => Channel Gates (Global)
    KyResult MakeChannelGates(WorkingMemory* workingMemory, WidenSide widenSide);
    void ShrinkStartChannelGate(WidenSide widenSide);
    void AddEndChannelGate(WidenSide widenSide);

    NextGateX ComputeNextGateX();

    KyResult MakeGates_Trapeze();
    KyResult MakeGates_Triangle(WidenSide widenSide);

    void MakeGates_Mid_No_Constraint(KyFloat32 x);
    KyResult MakeGates_Mid(KyFloat32 x);
    KyResult MakeGates_End();

    bool IsGoodFit(const GateFit& gateFit);
    bool IsBetterFit(const GateFit& a, const GateFit& b);

    void ComputeGateFit_Mid(GateFit& gateFit, KyFloat32 x) const;
    GateFitConstraint ComputeGateFit_Mid_Left(const Vec2f* leftIt, GateFit& gateFit) const;
    GateFitConstraint ComputeGateFit_Mid_Right(const Vec2f* rightIt, GateFit& gateFit) const;

    void ComputeGateFit_End(GateFit& gateFit) const;
    GateFitConstraint ComputeGateFit_End_Left(const Vec2f* leftIt, GateFit& gateFit) const;
    GateFitConstraint ComputeGateFit_End_Right(const Vec2f* rightIt, GateFit& gateFit) const;

    void InitGateFit_Mid(GateFit& gateFit, KyFloat32 nextX) const;
    void InitGateFit_End(GateFit& gateFit) const;

    void PushGate_Mid(const GateFit& gateFit);
    void PushGate_End(const GateFit& gateFit);
    void AddGateSampling(const GateFit& gateFit);
    void AdvanceToLastOrStrictlyAfterX(KyFloat32 x);
    void UpdateStartLocal(const GateCandidate& currGate); // update m_startLeftLocal and m_startRightLocal
    void UpdateEndLocal(const GateCandidate& endGate);    // update m_endLeftLocal and m_endRightLocal

    void InitGateCandidate_Start(GateCandidate& gateCandidate) const;
    void InitGateCandidate_End(GateCandidate& gateCandidate) const;

    void PushGateCandidate(const GateCandidate& gateCandidate);

    const SharedPoolList<Vec2f>& SimplifiedLeft()  const { return m_simplifier.m_leftPolyline; }
    const SharedPoolList<Vec2f>& SimplifiedRight() const { return m_simplifier.m_rightPolyline; }
    SharedPoolList<Vec2f>& SimplifiedLeft()  { return m_simplifier.m_leftPolyline; }
    SharedPoolList<Vec2f>& SimplifiedRight() { return m_simplifier.m_rightPolyline; }
    KyFloat32 ForceDeltaX() const { return m_forceDeltaX; } // 10 cm
    KyFloat32 MinDeltaX()        const { return m_channelConfig.m_advancedConfig.m_minDistBetweenIntermediaryGates; } // 50 cm
    KyFloat32 MaxDeltaX()        const { return m_channelConfig.m_advancedConfig.m_maxDistBetweenIntermediaryGates; } // 10 m
    KyFloat32 GetChannelRadius() const { return m_channelConfig.m_channelRadius; }
    KyFloat32 EndX() const { return m_simplifier.m_edgeLength2d; }
    Vec2f EdgeDir() const { return m_simplifier.m_edgeDir2d; }

    const Vec2f* LeftBegin()  const { return m_left.GetDataPtr(); }
    const Vec2f* LeftLast()   const { return m_left.GetDataPtr() + m_left.GetCount() - 1; }
    const Vec2f* LeftEnd()    const { return m_left.GetDataPtr() + m_left.GetCount(); }
    const Vec2f* RightBegin() const { return m_right.GetDataPtr(); }
    const Vec2f* RightLast()  const { return m_right.GetDataPtr() + m_right.GetCount() - 1; }
    const Vec2f* RightEnd()   const { return m_right.GetDataPtr() + m_right.GetCount(); }

    Vec3f GlobalFromLocal_Left(const Vec2f& local) const  { return GlobalFromLocal(local, GateSide::Left); }
    Vec3f GlobalFromLocal_Right(const Vec2f& local) const { return GlobalFromLocal(local, GateSide::Right); }
    Vec3f GlobalFromLocal(const Vec2f& local, GateSide::Enum side) const;

    DisplayListManager* GetDisplayListManager() const;
    void DisplayTrapezoid() const;
    void DisplayTriangleAroundLeft(const Vec3f& I) const;
    void DisplayTriangleAroundRight(const Vec3f& I) const;
    void DisplaySectionInput() const;
    void DisplayGateFit(const GateFit& gateFit) const;

public:
    ChannelBorderSimplifier m_simplifier;
    Database* m_database;
    Channel* m_channel;

    // config
    ChannelComputerConfig m_channelConfig;
    KyFloat32 m_margin;
    const KyFloat32 m_maxShrinkY; // 10 cm
    const KyFloat32 m_forceDeltaX; // 20 cm

    // input start
    KyUInt32 m_sectionIndex; // only for debugging
    WidenInput m_startInput;
    Vec2f m_startGateLeftPos;  // m_startGlobal.L
    Vec2f m_startGateRightPos; // m_startGlobal.R
    NavTrianglePtr m_startNavTriangle;

    // input end
    WidenInput m_endInput;
    Vec3f m_endGateFixedPos; // not relevant for endGate

    // working members

    struct Trapezoid { Vec3f SL; Vec3f SR; Vec3f EL; Vec3f ER; };
    Trapezoid m_trapezoid;

    // 1. Init in m_startLeftLocal = Vec2f(extremities.m_startX, extremities.m_startY);
    // 2. changed in makes gates
    // 3. Channel.LastGate.L = GlobalFromLocal(m_startLeftLocal)
    Vec2f m_startLeftLocal;
    Vec2f m_startRightLocal;
    Vec2f m_endLeftLocal;
    Vec2f m_endRightLocal;

    // instead of m_startLeftLocal, m_startRightLocal, m_endLeftLocal, m_endRightLocal we may have
    // GateCandidate m_startLocal; GateCandidate m_endLocal;
    // -or- m_startInput.localL m_startInput.localR...

    KyArray<GateCandidate> m_gates;

    KyArray<Vec2f> m_left;
    KyArray<Vec2f> m_right;

    //                     //    +--------------------------+    //
    KyFloat32 m_gateMinX;  //   /|m_gateMinX      m_gateMaxX|\   //
    KyFloat32 m_gateMaxX;  //  +------------------------------+  //

    GateCandidate m_currGate;
    const Vec2f* m_leftIt;
    const Vec2f* m_rightIt;

    // debug
    FullDebug m_fullDebug;
};

template<class TLogic>
inline KyResult ChannelSectionWidener::WidenSection(WorkingMemory* workingMem, void* traverseLogicUserData, QueryDynamicOutput* queryDynamicOutput, WidenSide widenSide)
{
    // Run PolylineCast to collect the borders
    PolylineCastIntersector intersector;
    ConfigureIntersector(intersector);
    KyResult rc = PolylineCastHelper::RunPolylineCast<TLogic>(workingMem, m_database, traverseLogicUserData, queryDynamicOutput, intersector);

    if (rc == KY_SUCCESS)
        rc = SimplifyPolylinesAndMakeGates(workingMem, intersector, widenSide);

    if (m_fullDebug.Has(FullDebug::ChannelSectionWidener))
        GetDisplayListManager()->NewFrame(); // one frame per section

    return rc;
}

} // namespace Kaim