circlearcsplinecomputer.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.
*/

// primary contact: LAPA - secondary contact: NOBODY
#ifndef Navigation_CircleArcSplineComputer_H
#define Navigation_CircleArcSplineComputer_H


#include "gwnavruntime/pathfollower/circlearcspline.h"
#include "gwnavruntime/pathfollower/circlearcsplinedisplay.h"
#include "gwnavruntime/pathfollower/circlearcsplineturn.h"
#include "gwnavruntime/pathfollower/splineinputblobdumper.h"
#include "gwnavruntime/pathfollower/circlearcsplinecomputationresult.h"
#include "gwnavruntime/pathfollower/splinetrajectoryconfig.h"
#include "gwnavruntime/pathfollower/radiusprofile.h"

#include "gwnavruntime/channel/channelsectionptr.h"


//#define DBG_CircleArcSplineComputer_PrintStatsOnSmallArcs


namespace Kaim
{


class RadiusProfileCircleArcSplineComputerOutput
{
public:
    ~RadiusProfileCircleArcSplineComputerOutput() { Clear(); }
    void Clear()
    {
        m_spline.Clear();
        m_result = CircleArcSplineComputation_NotComputed;
    }

    CircleArcSpline                  m_spline;
    CircleArcSplineComputationResult m_result;
    KyUInt32                         m_warningFlags;
};


class CircleArcSplineComputerVisualDebugConfig
{
public:
    CircleArcSplineComputerVisualDebugConfig()
        : m_world(KY_NULL)
        , m_visualDebugId(KyUInt32MAXVAL)
    {}

    World* m_world;
    KyUInt32 m_visualDebugId;
    String m_visualDebugPrefix;

    StringPulledBubbleListDisplayConfig m_rawBubbleListDisplayConfig;
    StringPulledBubbleListDisplayConfig m_adjustedBubbleListDisplayConfig;
    StringPulledBubbleListDisplayConfig m_simplifiedBubbleListDisplayConfig;
    CircleArcSplineSectionDisplayConfig m_circleArcSplineSectionDisplayConfig;

    // Activate this to track down error cases
    Ptr<SplineInputBlobDumpConfig> m_inputBlobDumpConfig;
};


// Internal: Class aggregating all necessary information about an edge going from one
// turn to the next one. Such edges are namely StringPulledEdge localized into a Channel.
class TurnListEdge
{
public:
    StringPulledEdge m_edge;
    KyUInt32 m_startSectionIdx;
    KyUInt32 m_endSectionIdx;
};

class SplineInputBlobDumpConfig;
class SplineInputBlobDumper;

class CircleArcSplineComputer
{
public:
    CircleArcSplineComputer();
    ~CircleArcSplineComputer();

    void Initialize(const SplineComputationConfig& splineConfig,
        const Vec3f& startPosition, const ChannelSectionPtr& startSection,
        const Vec3f& endPosition, const ChannelSectionPtr& endSection);

    void Clear();
    void ClearInternals();

    void SetStartConstraint(const Vec2f& constraint) { m_startConstraint = constraint; }
    void SetEndConstraint(const Vec2f& constraint) { m_endConstraint = constraint; }

    CircleArcSplineComputationResult Compute();

    KyUInt32 GetOutputCount() const { return m_profileOutputs.GetCount(); }
    const RadiusProfileCircleArcSplineComputerOutput& GetOutput(KyUInt32 index) const { return m_profileOutputs[index]; }


public: // internal: these are public only to be accessible from unit tests
    enum NextChannelTurnRangeSearchResult
    {
        NextChannelTurnFound,
        NoChannelTurnUpToEnd
    };

    enum ChangeTurnBubbleResult
    {
        TurnBubbleChangeRejected,
        TurnBubbleChangeAccepted,
        TurnBubbleChangeIgnored
    };

    enum NeighborTurnValidationResult
    {
        ValidNeighborTurn,
        ValidNeighborTurn_Small,
        InvalidNeighborTurn_ContinueSearch,
        InvalidNeighborTurn_StopSearch
    };

    enum NeighborTurnArcValidationMode
    {
        UseIsInTurnRangeForSameSideArcsAndCanGoForOppositeOnes,
        UseCanGoForAllArcs
    };

    // ---- Main computation function : calls all the subsequent ones
    CircleArcSplineComputationResult ComputeSpline(KyUInt32& overallWarningFlags);

    // ---- First steps: computed once for all RadiusProfiles

    KyResult CheckInputValidity();
    KyResult ComputeBubbleArray(BubbleArray& bubbleArray);
    KyResult ComputeStringPulledBubbleList(const BubbleArray& bubbleArray, StringPulledBubbleList& stringPulledBubbleList);

    // ---- Last steps: computed for each RadiusProfile

    // Compute all the last steps: Conversion to turn list, radius adjustment,
    // simplification & conversion into CircleArcSpline.
    void ComputeOptimalSpline(const StringPulledBubbleList& stringPulledBubbleList, RadiusProfileCircleArcSplineComputerOutput& output, const RadiusProfile& radiusProfile);

    // Convert the string pulled Bubble list into a TurnList.
    CircleArcSplineComputationResult ConvertStringPulledBubbleListIntoTurnList(const StringPulledBubbleList& stringPulledBubbleList, TurnList& turnList);

    // Enlarge turns to fit preferred turn radii and respecting neighbor turns constraints.
    CircleArcSplineComputationResult AdjustTurnListToRadiusProfile(const RadiusProfile& radiusProfile, TurnList& turnList, KyUInt32& warningFlags);

    // Convert the TurnList into a CircleArcSpline.
    CircleArcSplineComputationResult ConvertTurnListIntoCircleArcSpline(const TurnList& turnList, CircleArcSpline& m_circleArcSpline, KyUInt32& warningFlags);


    // BubbleArray computation

    NextChannelTurnRangeSearchResult GetNextChannelTurnRange(const Channel* channel, KyUInt32 currentIdx, KyUInt32 lastIdx, KyUInt32& nextTurnStartIdx, KyUInt32& nextTurnEndIdx);
    KyResult UpdateBubbleMaxRadius(const Vec3f& center, const Vec3f& constrainingPosition, const Vec3f& oppositeCorner, KyFloat32& maxBubbleRadius) const;
    KyResult CapBubblesRadiiAccordinglyToStartAndEndPositions(const Channel* channel, KyUInt32 gateIdx, KyFloat32& leftCornerMaxRadius, KyFloat32& rightCornerMaxRadius) const;
    void AddCornerBubble(const Vec3f& channelCorner, KyFloat32 BubbleRadius, RotationDirection rotationDirection, BubbleArray& bubbleArray);
    KyResult AddIntermediaryGateBubbles(const Channel* channel, KyUInt32 gateIdx, BubbleArray& bubbleArray);
    KyResult AddNextChannelTurnBubbles(const Channel* channel, KyUInt32 nextTurnStartIdx, KyUInt32 nextTurnEndIdx, BubbleArray& bubbleArray);

    // Adjusts bubble radius to neighbors so that the bubble array is string-pullable
    void EnsureBubbleArrayIsStringPullable(BubbleArray& bubbleArray);

    void AdjustBubbleRadiusRelativelyToNearbyBubble(const Bubble* nearbyBubble, Bubble& bubble);
    void EnsureBubbleArrayIsStringPullable_ForwardPass(BubbleArray& bubbleArray, const KyUInt32 firstCornerBubbleIdx, const KyUInt32 lastCornerBubbleIdx);
    void EnsureBubbleArrayIsStringPullable_BackwardPass(BubbleArray& bubbleArray, const KyUInt32 firstCornerBubbleIdx, const KyUInt32 lastCornerBubbleIdx);


    // TurnList atomic modifications

    void ComputeInitialTurnsForStartAndEndConstraints(TurnList& turnList, KyFloat32 startRadius, KyFloat32 endRadius, CircleArcSplineComputationResult& result, KyUInt32& warningFlags);
    bool AdjustFirstTurnToStartConstraintAndThisRadius(TurnList& turnList, const RadiusProfile& radiusProfile, KyUInt32 radiusIdx, CircleArcSplineComputationResult& result, KyUInt32& warningFlags);
    bool AdjustLastTurnToEndConstraintAndThisRadius(TurnList& turnList, const RadiusProfile& radiusProfile, KyUInt32 radiusIdx, CircleArcSplineComputationResult& result, KyUInt32& warningFlags);

    // Check the candidate Bubble is valid if used for the specified turn. If so, it then:
    // 1. replace the turn Bubble by the candidate one
    // 2. update turn entry and exit positions
    // 3. update the neighbor turns:
    //      - remove the ones that are no more in touch with the pulled string
    //      - update the previous turn exit position
    //      - update the next turn entry position
    ChangeTurnBubbleResult ChangeTurnBubble(TurnList& turnList, TurnList::Iterator turnIt, const Bubble& candidate,
        const RadiusProfile& radiusProfile, KyUInt32 radiusIndexInProfile, CircleArcSplineComputationResult& result, KyUInt32& warningFlags);

    KyResult FindNewPrevTurn(TurnList& turnList, const TurnList::Iterator& turnIt, const CircleArcSplineTurn& curTurn, const Bubble& turnBubbleCdt,
        const RotationDirection rotDir, ScopedDisplayList* displayListPtr, TurnList::Iterator& prevTurnIt, TurnListEdge& incomingEdge,
        const RadiusProfile& radiusProfile, KyUInt32 radiusIndexInProfile, Bubble& newStartBubble, KyUInt32& newStartBubbleRadiusIndexInProfile);

    KyResult FindNewNextTurn(TurnList& turnList, const TurnList::Iterator& turnIt, const CircleArcSplineTurn& curTurn, const Bubble& turnBubbleCdt, const TurnListEdge& incomingEdge,
        const RotationDirection rotDir, ScopedDisplayList* displayListPtr, TurnList::Iterator& nextTurnIt, TurnListEdge& outgoingEdge,
        const RadiusProfile& radiusProfile, KyUInt32 radiusIndexInProfile, Bubble& newEndBubble, KyUInt32& newEndBubbleRadiusIndexInProfile);

    KyResult TryFirstTurnWithRelaxedStartConstraint(
        Bubble& newStartBubble, KyUInt32& newStartBubbleRadiusIndexInProfile, const CircleArcSplineTurn& curTurn,
        StringPulledEdge& edge, const Bubble& turnBubbleCdt, const RotationDirection rotDir, ScopedDisplayList* displayListPtr,
        TurnListEdge& incomingEdge, TurnList::Iterator& prevTurnIt, TurnList::Iterator prevTurnCdtIt);

    KyResult TryLastTurnCandidateWithRelaxedEndConstraint(
        Bubble& newEndBubble, KyUInt32& newEndBubbleRadiusIndexInProfile, const CircleArcSplineTurn& curTurn,
        StringPulledEdge& edge, const Bubble& turnBubbleCdt, const RotationDirection rotDir, ScopedDisplayList* displayListPtr,
        const TurnListEdge& incomingEdge, TurnListEdge& outgoingEdge, TurnList::Iterator& nextTurnIt, TurnList::Iterator nextTurnCdtIt);

    NeighborTurnValidationResult ValidatePrevTurnCandidate(const CircleArcSplineTurn& curTurn, const Bubble& turnBubbleCdt, const CircleArcSplineTurn& prevTurnCdtTurn,
        const RotationDirection rotDir, ScopedDisplayList* displayListPtr, TurnListEdge& incomingEdge,
        KyFloat32& skippedTurnAngleRadSum, NeighborTurnArcValidationMode neighborTurnArcValidationMode);

    NeighborTurnValidationResult ValidateNextTurnCandidate(const CircleArcSplineTurn& curTurn, const Bubble& turnBubbleCdt, const CircleArcSplineTurn& nextTurnCdtTurn,
        const RotationDirection rotDir, ScopedDisplayList* displayListPtr, const TurnListEdge& incomingEdge, TurnListEdge& outgoingEdge,
        KyFloat32& skippedTurnAngleRadSum, NeighborTurnArcValidationMode neighborTurnArcValidationMode);

    bool DoesSegmentRespectDistanceToChannelBorder(const Channel* channel,
        const Vec3f& from, KyUInt32 fromSectionIdx, const Vec3f& to, KyUInt32& toSectionIdx) const;

    bool DoesCircleArcRespectDistanceToChannelBorder(const Channel* channel, const Bubble& circleArcBubble,
        const Vec3f& from, KyUInt32 fromSectionIdx, const Vec3f& to, KyUInt32 toSectionIdx) const;

    // ---- to be removed -----
    bool TestCircleArcCanGoWithSampling(const Bubble &circleArcBubble, const Vec3f &from, const Vec3f &to, const KyFloat32 circleRadius, KyUInt32 fromSectionIdx,
        const Channel* channel, KyUInt32 toSectionIdx) const;


public: // Internal - intermediary computation result accessors and other internal functions useful for unit tests
    const TurnList& GetTurnList() const { return m_turnList; }

    // Adjusts a channel width accordingly to m_margin.
    KyFloat32 GetReducedChannelWidth(KyFloat32 channelWidth) const;

    enum VerboseMode {Verbose, Silent};
    bool IsSplineValid(const CircleArcSpline& spline, CircleArcSplineComputationResult result, KyUInt32 warningFlags, VerboseMode verboseMode = Silent) const;


    // Utility internal methods

    void InitializeVisualDebugColor();
    void InitializeDisplayList(ScopedDisplayList& displayList, const char* groupName, const char* listName) const;
    void RenderSplineComputationInputs() const;
    void RenderBubbleData(const CircleArcSplineTurn& turn, const char* groupName) const;
    void RenderBubbleArray(const BubbleArray& bubbleArray) const;
    void RenderTurnList(const TurnList& turnList, const char* listBaseName, const StringPulledBubbleListDisplayConfig& displayConfig) const;
    void RenderResult(const char* displayListBaseName, CircleArcSplineComputationResult result, KyUInt32 warningflags) const;

    void DumpOrVisualDebugIfNeeded(SplineInputBlobDumper& inputBlobDumper, CircleArcSplineComputationResult overallResult,
        KyUInt32 overallWarningFlags, KyUInt32 radiusProfileCount);
    void VisualDebug_BubbleListConvertionError(const StringPulledBubbleList &stringPulledBubbleList, CircleArcSplineComputationResult result);
    void VisualDebugCircleArcSpline(CircleArcSpline& circleArcSpline);

#ifdef DBG_CircleArcSplineComputer_PrintStatsOnSmallArcs
    static void PrintSmallElementsStatistics();
#endif // DBG_CircleArcSplineComputer_PrintStatsOnSmallArcs


public: // Internal: We let parameters accessible for validation unit tests
    // Inputs
    const SplineComputationConfig* m_splineConfig;
    ChannelSectionPtr m_startSection;
    ChannelSectionPtr m_endSection;
    Vec3f m_startPosition;
    Vec3f m_endPosition;
    Vec2f m_startConstraint;
    Vec2f m_endConstraint;

public:
    // This is the minimal distance let between adjacent bubbles to ensure the
    // string can pass through the gap in-between. Default is 0.01m.
    KyFloat32 m_margin;

    // This is the cosine of the angle tolerated to consider start and end
    // constraints as respected.
    // Default is 1 degree.
    KyFloat32 m_maxAngleCosToleranceOnStartAndEndConstraints;

private:
    // Intermediary data structures
    TurnList::NodePool m_turnPool;
    TurnList m_turnList;

    // Intermediary parameter values
    KyFloat32 m_fullTurnToleranceSin;

    bool m_checkStartConstraintMaxAngle;
    Vec2f m_startConstraintCWLimit;
    Vec2f m_startConstraintCCWLimit;

    bool m_checkEndConstraintMaxAngle;
    Vec2f m_endConstraintCWLimit;
    Vec2f m_endConstraintCCWLimit;

    // Output
    KyArray<RadiusProfileCircleArcSplineComputerOutput> m_profileOutputs;

    // Debug / VisualDebug
    KyUInt32 m_debug_ChangeTurnBubble_VisualDebugListIdx;
    String m_debug_RadiusProfileLabel;

public: // internal
    bool m_isInComputingSpline;

public:
    // You can directly set these advanced parameters to toggle and configure VisualDebug.
    CircleArcSplineComputerVisualDebugConfig m_visualDebugConfig;
    bool m_debug_CheckSplineValidity;
};


KY_INLINE KyFloat32 CircleArcSplineComputer::GetReducedChannelWidth(KyFloat32 channelWidth) const
{
    const KyFloat32 reducedChannelWidth = (channelWidth > 2.0f * m_margin) ? (channelWidth - m_margin) : 0.5f * channelWidth;
    return reducedChannelWidth;
}

} // namespace Kaim

#endif // Navigation_CircleArcSplineComputer_H