diagonalstripcomputer.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/diagonalstrip.h"
#include "gwnavruntime/path/path.h"
#include "gwnavruntime/channel/channelarraycomputerstats.h"
#include "gwnavruntime/queries/querydynamicoutput.h"
#include "gwnavruntime/queries/trianglefromposquery.h"
#include "gwnavruntime/queries/utils/bestfirstsearchtraversal.h"
#include "gwnavruntime/queries/utils/polylinecasthelper.h"
#include "gwnavruntime/visualsystem/displaylist.h"
#include "gwnavruntime/queries/fulldebug.h"
#include "gwnavruntime/base/timeprofiler.h"

namespace Kaim
{

class DiagonalStripComputerContext;
class Path;

class DiagonalStripComputerConfig
{
public:
    DiagonalStripComputerConfig() { SetDefaults(); }
    void SetDefaults()
    {
        m_idealClearanceRadius = 2.5f;
        //m_idealPathDistFromCorners = 1.0f;
        m_fullDebug.Clear();
    }

    KyFloat32 m_idealClearanceRadius;
    //KyFloat32 m_idealPathDistFromCorners;
    FullDebug m_fullDebug;
};

enum DiagonalStripComputerResult
{
    DiagonalStripResult_NOT_INITIALIZED = QUERY_NOT_INITIALIZED, 
    DiagonalStripResult_NOT_PROCESSED   = QUERY_NOT_PROCESSED,   

    DiagonalStripResult_BuildDiagonalForPathEdge,
    DiagonalStripResult_ComputingMaxRadius_Init,
    DiagonalStripResult_ComputingMaxRadius,
    DiagonalStripResult_ComputingNeighborCompatibleRadius_Backward_Init,
    DiagonalStripResult_ComputingNeighborCompatibleRadius_Backward,
    DiagonalStripResult_ComputingNeighborCompatibleRadius_Forward_Init,
    DiagonalStripResult_ComputingNeighborCompatibleRadius_Forward,

    DiagonalStripResult_Done_DiagonalStripComputerError,
    DiagonalStripResult_Done_ComputeMaxRadiusError,
    DiagonalStripResult_Done_ComputeNeighborCompatibleRadiusError,

    DiagonalStripResult_Done
};

enum DiagonalStripJunctionType { JunctionType_Slanting, JunctionType_AlmostOrtho };

class DiagonalStripComputer
{
public:
    KY_DEFINE_NEW_DELETE_OPERATORS(MemStat_QueryWorkingMem)
public:
    DiagonalStripComputer();
    ~DiagonalStripComputer();

    void Clear();

    // database is redundant with path->m_database
    void Init(Database* database, const Path* path, KyUInt32 firstEdgeIdxOfSection, KyUInt32 lastEdgeIdxOfSection, DiagonalStrip* diagonalStrip, KyFloat32 cornerMaxRadius,
              FullDebug fullDebug);

    bool IsFinished() const { return m_result >= DiagonalStripResult_Done_DiagonalStripComputerError; }

    void* GetTraverseLogicUserData() const { return m_traverseLogicUserData; }
    void SetTraverseLogicUserData(void* traverseLogicUserData) { m_traverseLogicUserData = traverseLogicUserData; }

    DiagonalStripComputerResult GetResult() const { return m_result; }
    //DiagonalStripComputerConfig& GetComputerConfig() { return m_diagonalStripConfig; }

protected:
    KyResult ComputeCornerRadius(KyUInt32 cornerIdx);

    enum ComputeCompatibleRadiusResult { OutOfNeighborhood, Shrinked, Unchanged };

    KyResult ComputeForwardNeighborCompatibleCornerRadius(KyUInt32 cornerIdx);
    KyResult ComputeBackwardNeighborCompatibleCornerRadius(KyUInt32 cornerIdx);
    ComputeCompatibleRadiusResult ComputeCompatibleRadius(const Vec3f& cornerPos, CornerType cornerType, KyUInt32 otherIdx, KyFloat32& cornerRadius);

    void InitContext(DiagonalStripComputerContext& context, WorkingMemory* workingMemory);
    //void PostProcessPolylineCastResult(PolylineCastIntersector& clearancePolygonIntersector);
    KyResult FillDiagonalStripFromPolylineCastResult(PolylineCastIntersector& clearancePolygonIntersector);
    bool IsCCWQuadConvex(const Vec3f& A, const Vec3f& B, const Vec3f& C, const Vec3f& D);
    void ComputeBissectriceEndPoints(DiagonalStripComputerContext& context, const Vec3f& nextPointOnPath, KyFloat32 nextEdgelength2D, KyFloat32 dotProdWithNextEdge,
                                     KyFloat32 crossProdWithNextEdge);

    bool PushCorner(const Vec3f& pos, CornerType cornerType, bool force = false);
    //bool DiagonalStripComputerGetLastButOnCornerPos(CornerType cornerType, Vec3f& pos);
    KyResult MakeJunctionWithCurrentDiagonalStrip(PolylineCastIntersector& clearancePolygonIntersector);
    void PushEndPoint();

public:
    template <class TLogic> void Advance(WorkingMemory* workingMemory, ChannelArrayComputerStats& channelStats);

    template <class TLogic> KyResult ComputeDiagonalsFromPath(WorkingMemory* workingMemory, ChannelArrayComputerStats& stats);

    template <class TLogic> KyResult BuildDiagonalsForPathEdge(WorkingMemory* workingMemory);

    template <class TLogic> KyResult ProcessTheOneTrapezeStartingThePolygon(WorkingMemory* workingMemory, DiagonalStripComputerContext& context, KyFloat32 maxDotProdStart);

    template <class TLogic>
    KyResult ProcessPolygonWithTriangles(WorkingMemory* workingMemory, DiagonalStripComputerContext& context, Vec3f& bissectricIntersection, bool bissectriceIntersect,
                                         KyFloat32 maxDotProdStart, KyFloat32 minDotProdEnd);

    template <class TLogic>
    KyResult ProcessTwoOppositeTriangles(WorkingMemory* workingMemory, DiagonalStripComputerContext& context, const Vec3f& endPointLeft, const Vec3f& endPointRight,
                                         const Vec3f& endPointOnPath);

    template <class TLogic> KyResult EndPolygonFromAlmostOrthoStart(WorkingMemory* workingMemory, DiagonalStripComputerContext& context, KyFloat32 minDotProdEnd);

    template <class TLogic>
    KyResult BuildDiagonalsInTrapeze(WorkingMemory* workingMemory, const Vec3f& pathEdgeStart, const Vec3f& pathEdgeEnd, NavTrianglePtr& inAndOutTrianglePtr,
        const Vec3f& pointOnStartSideLeft, const Vec3f& pointOnEndSideLeft, const Vec3f& pointOnStartSideRight, const Vec3f& pointOnEndSideRight);

    template <class TLogic>
    KyResult BuildDiagonalsInTriangle(WorkingMemory* workingMemory, const Vec3f& pathEdgeStart, const Vec3f& pathEdgeEnd, NavTrianglePtr& inAndOutTrianglePtr,
                                      const Vec3f& pointOnStartSideLeft, const Vec3f& pointOnStartSideRight, const Vec3f& pointOnEndSideLeft, const Vec3f& pointOnEndSideRight,
                                      bool forceTrapeze = false);

public: // internal
    void ClearDisplayList();
    void CreateDisplayList();
    static ShapeColor GetVisualDebugQuadColor();
    static ShapeColor GetVisualDebugTriangleColor();
protected:
    Database* m_database;
    void* m_traverseLogicUserData;
    KyFloat32 m_channelRadius;
    DiagonalStripComputerResult m_result;
    const Path* m_rawPath;
    DiagonalStrip* m_diagonalStrip;

    // diagonal strip temp
    KyUInt32 m_currentPathEdgeIdx;
    KyUInt32 m_firstEdgeIdxOfSection;
    KyUInt32 m_lastEdgeIdxOfSection;
    KyInt32 m_currentCornerIdx;

    Vec3f m_startL;
    Vec3f m_startR;
    DiagonalStripJunctionType m_startJunctionType;
    Vec3f m_lastStripCornerLeft;
    Vec3f m_lastStripCornerRight;
    DisplayList* m_displayList; // TODO remove that and crate display list on the stack

    FullDebug m_fullDebug;
};


} // namespace Kaim



#include "gwnavruntime/channel/diagonalstripcomputer.inl"