channelbordersimplifier.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_SweepLinePolylineSimplifier_H
#define Navigation_SweepLinePolylineSimplifier_H


#include "gwnavruntime/channel/channel.h"
#include "gwnavruntime/containers/kyarray.h"
#include "gwnavruntime/containers/sharedpoollist.h"
#include "gwnavruntime/math/vec3f.h"
#include "gwnavruntime/queries/utils/polylinecasthelper.h"


namespace Kaim
{

class ChannelComputerConfig;
class ScopedDisplayList;

class PolylineExtremityContext
{
public:
    PolylineExtremityContext() : m_useStartAsInput(false), m_useEndAsInput(false), m_startCanBeMoved(false), m_endCanBeMoved(false) {}
    Vec2f m_startPos;
    Vec2f m_endPos;
    bool m_useStartAsInput;
    bool m_useEndAsInput;
    bool m_startCanBeMoved;
    bool m_endCanBeMoved;

    KyFloat32 m_startX;
    KyFloat32 m_startY;
    KyFloat32 m_endX;
    KyFloat32 m_endY;
};

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

    void Clear();
    void Initialize(Database* database, const Vec3f& edgeStartPos, const Vec2f& pathEdgeDir2d, KyFloat32 pathEdgeLength);
    void SetSmoothingAngle(KyFloat32 angleInDegrees);

    KyResult SimplifyOnLeft(const SharedPoolList<PolylineCastIntersector::BorderEdge>& leftBorders, PolylineExtremityContext& extremityContext);
    KyResult SimplifyOnRight(const SharedPoolList<PolylineCastIntersector::BorderEdge>& rightBorders, PolylineExtremityContext& extremityContext);
    KyResult Simplify(const SharedPoolList<PolylineCastIntersector::BorderEdge>& borders, bool onLeft, PolylineExtremityContext& extremityContext);
    void ProcessExtremity(SharedPoolList<Vec2f>& polyline, PolylineExtremityContext& extremityContext);

    Vec3f GetGlobalPosFromLocalPos(const Vec2f& localPos)
    {
        return m_edgeStartPos + Vec3f(localPos.x * m_edgeDir2d + localPos.y * m_edgeDir2d.PerpCCW());
    }

    Vec3f GetGlobalPosFromLocalPos(KyFloat32 x, KyFloat32 y)
    {
        return GetGlobalPosFromLocalPos(Vec2f(x, y));
    }

public: // internal
    void RemoveNoiseInPolyline(SharedPoolList<Vec2f>& polyline);
    
    void CreatePolylineFromBorder(const SharedPoolList<PolylineCastIntersector::BorderEdge>& borderEdgeList,
        SharedPoolList<Vec2f>& borderPolyline);

    KyResult SwapXAndYCoordinatesToEdgeLocal(SharedPoolList<Vec2f>& polyline);

    void FixXCoordinatesOrder(SharedPoolList<Vec2f>& polyline);
    void MoveYPositionOfAllPoints(SharedPoolList<Vec2f>& polyline);

    void FixPolylineAroundStartPoint(SharedPoolList<Vec2f>& polyline, KyFloat32& xCoordinate, KyFloat32& yCoordinate, bool canMovePoint);
    void FixPolylineAroundEndPoint(SharedPoolList<Vec2f>& polyline, KyFloat32& xCoordinate, KyFloat32& yCoordinate, bool canMovePoint);
    // ----------- Visual Debug ---------------

    // Polyline drawing
    class PolylineDisplayConfig
    {
    public:
        PolylineDisplayConfig() { SetDefaults(); }
        void SetDefaults()
        {
            m_arrowWidth = 0.0f;
            m_arrowColor.SetOnlyLineColor(VisualColor::Yellow);
        }

        KyFloat32 m_arrowWidth;
        VisualShapeColor m_arrowColor;
    };

    void RenderBorderEdges(const char* listName, VisualColor visualColor, const SharedPoolList<PolylineCastIntersector::BorderEdge>& leftBorders);
    void RenderPolyline(const char* listName, VisualColor visualColor, SharedPoolList<Vec2f>& polyline, bool hideByDefault = true);

private:
    void EraseCurrentPoint(SharedPoolList<Vec2f>& polyline,
        SharedPoolList<Vec2f>::Iterator& previous,
        SharedPoolList<Vec2f>::Iterator& current,
        SharedPoolList<Vec2f>::Iterator& next);

public: // internal
    Database* m_database;

    SharedPoolList<Vec2f> m_leftPolyline;
    SharedPoolList<Vec2f> m_rightPolyline;
    SharedPoolList<Vec2f>::NodePool m_positionPool;
    KyFloat32 m_nonNullMinY;

    Vec3f m_edgeStartPos;
    Vec2f m_edgeDir2d;
    KyFloat32 m_edgeLength2d;
    KyFloat32 m_maxTangent;

    KyFloat32 m_channelRadius;
    KyFloat32 m_min2dDistanceBetweenPoints; // The distance beneath which close border points are collapsed into one point
    KyFloat32 m_max2dDistanceBetweenPoints;

    bool m_advancedVisualDebuggingEnabled;
};


class SweepLinePolylineSimplifier
{
public:
    SweepLinePolylineSimplifier(KyFloat32 maxY, KyFloat32 maxSlope, KyFloat32 minProjDistBetweenPoint);
    ~SweepLinePolylineSimplifier() { Clear(); }

    void Clear();

    KyResult Simplify(SharedPoolList<Vec2f>& polyline);

    static bool CompVec2fY(const Vec2f& p0, const Vec2f& p1) { return (p0.y < p1.y) || (p0.y == p1.y && p0.x < p1.x); }
    static bool IsSlope1StrictlyGreaterThanSlope2(const Vec2f& slope1, const Vec2f& slope2)
    {
        return (fabsf(slope1.y * slope2.x) > fabsf(slope1.x * slope2.y));
    }
private:
    KyResult ConsumeInputPolyline(const SharedPoolList<Vec2f>& inPolyline);
    KyResult SimplifyPolyline();
    KyResult BuildOutput(SharedPoolList<Vec2f>& polyline);

    SharedPoolList<Vec2f>::Iterator GetAndPopPointToProcess();

    KyResult SimplifyAfterPolylinePoint(SharedPoolList<Vec2f>::Iterator pointIt);
    KyResult SimplifyBeforePolylinePoint(SharedPoolList<Vec2f>::Iterator pointIt);

    Vec2f GetMaxSlopeForPoint(const Vec2f &point);

    SharedPoolList<Vec2f>::Iterator ComputeFirstPointIteratorAfterLine(SharedPoolList<Vec2f>::Iterator firstPointItToCheck,
        const Vec2f& lineStartPoint, const Vec2f& lineSlope);
    SharedPoolList<Vec2f>::Iterator ComputeFirstPointIteratorBeforeLine(SharedPoolList<Vec2f>::Iterator firstPointItToCheck,
        const Vec2f& lineStartPoint, const Vec2f& lineSlope);

    SharedPoolList<Vec2f>::Iterator GetMoreConstraintSlopeForward(const Vec2f& point, SharedPoolList<Vec2f>::Iterator currentIt);
    SharedPoolList<Vec2f>::Iterator GetMoreConstraintSlopeBackward(const Vec2f& point, SharedPoolList<Vec2f>::Iterator currentIt);
    
    void RemoveFromWorkingPolyline(SharedPoolList<Vec2f>::Iterator firstIteratorToRemove, SharedPoolList<Vec2f>::Iterator stopAtThis);

public:
    SharedPoolList<Vec2f> m_workingPolyline;
    SharedPoolList<Vec2f>::NodePool m_pointPool;

    KyArrayCPP<SharedPoolList<Vec2f>::Iterator> m_openNodes;

    Vec2f m_maxSlope;
    KyFloat32 m_minX;
    KyFloat32 m_maxX;
    KyFloat32 m_maxY;
    KyFloat32 m_minY;
    KyFloat32 m_minProjDistBetweenPoints;

    Vec3f m_origin;
    Vec2f m_yAxis;
};


KY_INLINE KyResult ChannelBorderSimplifier::SimplifyOnLeft(const SharedPoolList<PolylineCastIntersector::BorderEdge>& leftBorders, PolylineExtremityContext& extremityContext)
{
    return Simplify(leftBorders, true, extremityContext);
}
KY_INLINE KyResult ChannelBorderSimplifier::SimplifyOnRight(const SharedPoolList<PolylineCastIntersector::BorderEdge>& rightBorders, PolylineExtremityContext& extremityContext)
{
    return Simplify(rightBorders, false, extremityContext);
}

} // namespace Kaim

#endif // Navigation_SweepLinePolylineSimplifier_H