bubbletocornerfunnel.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_BubbleToCornerFunnel_H
#define Navigation_BubbleToCornerFunnel_H

#include "gwnavruntime/channel/funnel.h"
#include "gwnavruntime/channel/bubblearray.h"
#include "gwnavruntime/visualsystem/visualcolor.h"


namespace Kaim
{

class BubbleArray;
class BubbleToCornerFunnel;
class ScopedDisplayList;

// Internal: represents a BubbleFunnel border with all necessary internal references.
class BubbleFunnelBorder
{
public:
    BubbleFunnelBorder()
        : m_isInitialized(false)
    {}

    BubbleFunnelBorder(KyUInt32 bubbleIdx, const Vec3f& segmentStart, const Vec3f& segmentEnd)
    {
        Set(bubbleIdx, segmentStart, segmentEnd);
    }

    void Clear()
    {
        m_isInitialized = false;
    }

    void Set(KyUInt32 bubbleIdx, const Vec3f& segmentStart, const Vec3f& segmentEnd)
    {
        m_segmentStart = segmentStart;
        m_segmentEnd   = segmentEnd;

        const Vec2f segmentVec2d = segmentEnd.Get2d() - segmentStart.Get2d();
        m_segmentLength2d = segmentVec2d.GetNormalized(m_segmentDir2d);

        m_bubbleIdx     = bubbleIdx;
        m_isInitialized = true;
    }

    const Vec3f&   GetSegmentStart()    const { return m_segmentStart;    }
    const Vec3f&   GetSegmentEnd()      const { return m_segmentEnd;      }
    const Vec2f&   GetSegmentDir2d()    const { return m_segmentDir2d;    }
    KyFloat32      GetSegmentLength2d() const { return m_segmentLength2d; }
    KyUInt32       GetBubbleIdx()       const { return m_bubbleIdx;       }
    bool           IsInitialized()      const { return m_isInitialized;   }

private:
    friend class BubbleToCornerFunnel;

    Vec3f     m_segmentStart;
    Vec3f     m_segmentEnd;
    Vec2f     m_segmentDir2d;
    KyFloat32 m_segmentLength2d;
    KyUInt32  m_bubbleIdx;
    bool      m_isInitialized;
};


class BubbleToCornerFunnel
{
public:
    BubbleToCornerFunnel(const BubbleArray* bubbleArray)
        : m_bubbleArray(bubbleArray)
        , m_previousResult(FunnelUnchanged)
        , m_leftBorderDisplayList(KY_NULL)
        , m_rightBorderDisplayList(KY_NULL)
    {}

    void Initialize(KyUInt32 apexIdx, const Vec2f& entryDir, KyFloat32 fullTurnToleranceSin);
    FunnelResult UpdateAccordinglyToCorner(KyUInt32 cornerIdx, KyArrayPOD<KyUInt32>& indicesOfTheCornersIncludedInCurrentPivot);

    KyUInt32                  GetApexIdx()        const { return m_apexIdx;        }
    const Bubble&             GetApexBubble()     const { return m_apexBubble;     }
    const BubbleFunnelBorder& GetLeftBorder()     const { return m_leftBorder;     }
    const BubbleFunnelBorder& GetRightBorder()    const { return m_rightBorder;    }
    FunnelResult              GetPreviousResult() const { return m_previousResult; }

private:
    bool IsStrictlyInTurnRange(const Vec2f& entryDir, const Vec2f& exitDir, RotationDirection rotDir, const Vec2f& dir2d) const;
    bool IsOutsideOnLeft(const Vec2f& axis2d) const;
    bool IsOutsideOnRight(const Vec2f& axis2d) const;

    bool IsOutsideOnLeft_Standard(const Vec2f& axis2d) const;
    bool IsOutsideOnRight_Standard(const Vec2f& axis2d) const;
    bool IsStrictlyInHalfPlan(const Vec2f& refDir, KyFloat32 rotSign, const Vec2f& axis2d) const;
    bool IsInHalfPlan(const Vec2f& refDir, KyFloat32 rotSign, const Vec2f& axis2d) const;

    enum IsInsideFunnelResult
    {
        FunnelNotInitialized,
        LeftBorderNotInitialized,
        RightBorderNotInitialized,
        SmallFunnelRange,
        NearLeftBorder,
        NearRightBorder,
        StrictlyInsideFunnel,
        OutsideFunnel
    };
    IsInsideFunnelResult IsInsideFunnel(const Vec2f& axis2d, KyFloat32 fullTurnToleranceSin) const;

    bool IsInsideFunnel(const Vec2f& axis2d) const;

    void TightenLeftBorder(const BubbleFunnelBorder& border);
    void TightenRightBorder(const BubbleFunnelBorder& border);
    void PushBorderAsLineInDisplayList(const BubbleFunnelBorder& border, ScopedDisplayList* displayList, VisualColor color);

private:
    // Inputs
    const BubbleArray* m_bubbleArray;

    KyUInt32 m_apexIdx;
    Bubble m_apexBubble;
    RotationDirection m_apexRotDir;
    FunnelSide m_apexSide;
    Vec2f m_entryDir;

    KyFloat32 m_fullTurnToleranceSin;

    // Keep radius-related funnel information.
    BubbleFunnelBorder m_leftBorder;
    BubbleFunnelBorder m_rightBorder;

    // Output
    FunnelResult m_previousResult;

    // Internals
    Bubble m_lastAddedCornerBubble; // Just to avoid allocating a Bubble on each iteration.
    bool m_entryDirPassed;
    bool m_hasQuitApexBubble;

public: // internal - debug
    ScopedDisplayList* m_leftBorderDisplayList;
    ScopedDisplayList* m_rightBorderDisplayList;
};


KY_INLINE void BubbleToCornerFunnel::TightenLeftBorder(const BubbleFunnelBorder& border)
{
    if (m_leftBorderDisplayList)
        PushBorderAsLineInDisplayList(border, m_leftBorderDisplayList, VisualColor::Green);

    m_leftBorder = border;
    m_previousResult = FunnelTightened;
}

KY_INLINE void BubbleToCornerFunnel::TightenRightBorder(const BubbleFunnelBorder& border)
{
    if (m_rightBorderDisplayList)
        PushBorderAsLineInDisplayList(border, m_rightBorderDisplayList, VisualColor::DarkRed);

    m_rightBorder = border;
    m_previousResult = FunnelTightened;
}


KY_INLINE bool BubbleToCornerFunnel::IsOutsideOnLeft_Standard(const Vec2f& axis2d) const
{
    return IsStrictlyInHalfPlan(m_leftBorder.GetSegmentDir2d(), 1.0f, axis2d);
}

KY_INLINE bool BubbleToCornerFunnel::IsOutsideOnRight_Standard(const Vec2f& axis2d) const
{
    return IsStrictlyInHalfPlan(m_rightBorder.GetSegmentDir2d(), -1.0f, axis2d);
}

} // namespace Kaim

#endif // Navigation_BubbleToCornerFunnel_H