splinetrajectory.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: BRGR
#ifndef Navigation_SplineTrajectory_H
#define Navigation_SplineTrajectory_H

#include "gwnavruntime/pathfollower/followedcirclearcspline.h"
#include "gwnavruntime/pathfollower/radiusprofile.h"
#include "gwnavruntime/pathfollower/resettrajectorystatus.h"
#include "gwnavruntime/pathfollower/splinetrajectoryconfig.h"
#include "gwnavruntime/channel/channelsectionptr.h"
#include "gwnavruntime/path/positiononpath.h"
#include "gwnavruntime/world/worldelement.h"
#include "gwnavruntime/queries/circlearcsplinecomputationquery.h"


namespace Kaim
{

class VisualDebugServer;
class SplineTrajectoryConfig;
class PositionOnLivePath;
class CircleArcSplineComputer;
class Trajectory;
class ComputeVelocityVisualDebug;
class BaseShortcutTrajectory;
class Bot;
class IAvoidanceComputer;


class SplineTrajectory: public RefCountBase<SplineTrajectory, MemStat_PathFollowing>
{
    KY_DEFINE_NEW_DELETE_OPERATORS(MemStat_PathFollowing)
    KY_CLASS_WITHOUT_COPY(SplineTrajectory)

public:
    SplineTrajectory(Trajectory* trajectory);
    ~SplineTrajectory() {}

    void Compute(KyFloat32 simulationTimeInSeconds, Ptr<BaseShortcutTrajectory> shortcutTrajectory);

    const FollowedCircleArcSpline* GetFollowedCircleArcSpline() const;

public: // internal

    // High-level internal methods

    Bot* GetBot() const;

    void Clear(); // fully clear the trajectory and default its config
    void ClearTrajectory(); // only clear the current computed trajectory

    void SetSplineTrajectoryConfig(const SplineTrajectoryConfig& other);

    // VisualDebug

    virtual void SendVisualDebugBlob(VisualDebugServer& visualDebugServer, VisualDebugSendChangeEvent changeEvent);
    void RemoveAllVisualDebugBeforeTrajectoryChange(VisualDebugServer& visualDebugServer);

protected:
    void ForceClearTrajectory(); // clear trajectory without checking Bot::IsPathRecomputationNeeded();
    void ClearSplineComputer();
    void ClearSplineCut();

    void UpdateCircleArcSpline(KyFloat32 simulationTimeInSeconds, FollowedCircleArcSpline& previousShortcutSpline);
    void UpdateShortcutSpline(KyFloat32 simulationTimeInSeconds, const FollowedCircleArcSpline& previousShortcutSpline, Ptr<BaseShortcutTrajectory> shortcutTrajectory);

    bool CanUseChannel();
    bool IsBotBackInChannel();
    bool IsBotOutOfChannel();
    bool IsEndOfChannelReached();
    bool IsFallbackToShortcutRequired();

    void FollowFrozenSpline(KyFloat32 simulationTimeInSeconds);
    bool CanFrozenSplineBeFollowed();

    // Spline computation internal methods

    void RequestSplineComputation();
    void IntegrateSplineFromQuery();

    bool ShouldRecomputeSpline(const ChannelSectionPtr& upperBoundSection);
    void ComputeSpline(KyFloat32 simulationTimeInSeconds, const ChannelSectionPtr& upperBoundSection);

    void InitSplineStartPosition();
    void ComputeSplineStartPosition();
    void UpdateStartCutWithUpperBound(const ChannelSectionPtr& upperBoundSection);
    KyResult ComputeNewSplineEndPosition();
    void CutFrozenSpline(KyFloat32 simulationTimeInSeconds);
    void AddStartConstraintToSplineQuery();
    KyUInt32 SelectSpline(const CircleArcSplineComputer& circleArcSplineComputer);

    void ComputeSplineFromShortcutTrajectory(KyFloat32 simulationTimeInSeconds, Ptr<BaseShortcutTrajectory> shortcutTrajectory);

    // Spline follow internal methods

    void UpdateSplinePosition(KyFloat32 simulationTimeInSeconds);
    void UpdateSplineTargetPosition(KyFloat32 simulationTimeInSeconds);
    void ComputeMovingDirection2D(KyFloat32 simulationTimeInSeconds, Vec2f& out_movingDirection);
    void ComputeVelocity(KyFloat32 simulationTimeInSeconds);

    void ComputeAvoidanceTrajectory(KyFloat32 simulationTimeInSeconds, KyFloat32 desiredSpeed, const Kaim::Vec2f& movingDirection);
    void StopBot(const Vec2f& frontDirection);
    
    void DrawComputeVelocityVisualDebug(const ComputeVelocityVisualDebug& collectedDebugData);

    bool CapDirectionChangeAngle(const Vec2f& normalizedFrom, const Vec2f& normalizedTo, Vec2f& result, KyFloat32 maxAngleInRad) const;

public: // internal
    class SplineCut
    {
    public:
        SplineCut() { Clear(); }
        ~SplineCut() { Clear(); }

        bool IsValid() { return m_channelSectionIdx != KyUInt32MAXVAL && m_positionOnSpline.IsValid() && m_splineSection.IsInitialized(); }

        void Clear()
        {
            m_positionOnSpline.Invalidate();
            m_splineSection.Reset();
            m_channelSectionIdx = KyUInt32MAXVAL;
            m_mode = CutSplineAtArcStart;
        }

    public:
        PositionOnCircleArcSpline m_positionOnSpline;
        CircleArcSplineSection m_splineSection;
        KyUInt32 m_channelSectionIdx;
        SplineCutMode m_mode;
    };

    Trajectory* m_trajectory;
    ResetTrajectoryStatus m_trajectoryStatus; // maintain internal status of the SplineTrajectory
    Ptr<IAvoidanceComputer> m_avoidanceComputer;
    FollowedCircleArcSpline m_followedSpline; 
    KyUInt32 m_positionOnSplineChannelSectionIdx; // The channel section of the current bot position on spline, computed only at the time a spline is computed

    // information used for computation of spline (full spline or new end part of current spline)
    ChannelSectionPtr m_splineComputerStartSection;
    Vec3f m_splineComputerStartPosition;

    // Some spline information computed when identifying spline cut position
    // and used once the new spline is computed to append it to the kept part
    // of the former spline.
    SplineCutMode m_botPosSplineCutMode;
    SplineCut m_splineCutThisFrame;
    SplineCut m_splineCutWhenSplineRequested;

    Ptr<CircleArcSplineComputationQuery> m_splineComputationQuery;

    SplineTrajectoryConfig m_splineTrajectoryConfig;
    RadiusProfile m_currentSplineRadiusProfile;

    Vec2f m_previousMovingDirection;
    float m_stopWaitTime;

    bool m_advancedVisualDebuggingEnabled;
};

} // namespace Kaim

#endif // Navigation_SplineTrajectory_H