Polyline.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
#pragma once
 
#include "VectorTypes.h"
#include "SegmentTypes.h"
#include "LineTypes.h"
#include "BoxTypes.h"
 
namespace UE
{
namespace Geometry
{
 
using namespace UE::Math;
 
 
/*
 *  TPolylinePolicy represents a collection of needed support classes for TPolyline depending on the required dimension
 */
 
template<typename T, int DIM>
class TPolylinePolicy
{};
 
template<typename T>
class TPolylinePolicy<T, 3>
{
public:
    typedef TVector<T> VectorType;
    typedef TSegment3<T> SegmentType;
    typedef TAxisAlignedBox3<T> BoxType;
};
 
template<typename T>
class TPolylinePolicy<T, 2>
{
public:
    typedef TVector2<T> VectorType;
    typedef TSegment2<T> SegmentType;
    typedef TAxisAlignedBox2<T> BoxType;
};
 
template<typename T, int D>
class TPolyline
{
protected:
    typedef typename TPolylinePolicy<T, D>::VectorType VectorType;
    typedef typename TPolylinePolicy<T, D>::SegmentType SegmentType;
    typedef typename TPolylinePolicy<T, D>::BoxType BoxType;
 
    TArray<VectorType> Vertices;
    
public:
 
 
    TPolyline()
    {
    }
 
    TPolyline(const TArray<VectorType>& VertexList) : Vertices(VertexList)
    {
    }
 
    TPolyline(const VectorType& Point0, const VectorType& Point1)
    {
        Vertices.Add(Point0);
        Vertices.Add(Point1);
    }
 
    const VectorType& operator[](int Index) const
    {
        return Vertices[Index];
    }
 
    VectorType& operator[](int Index)
    {
        return Vertices[Index];
    }
 
 
    const VectorType& Start() const
    {
        return Vertices[0];
    }
 
    const VectorType& End() const
    {
        return Vertices[Vertices.Num()-1];
    }
 
 
    const TArray<VectorType>& GetVertices() const
    {
        return Vertices;
    }
 
    int VertexCount() const
    {
        return Vertices.Num();
    }
 
    int SegmentCount() const
    {
        return Vertices.Num()-1;
    }
 
 
    void Clear()
    {
        Vertices.Reset();
    }
 
    void AppendVertex(const VectorType& Position)
    {
        Vertices.Add(Position);
    }
 
    void AppendVertices(const TArray<VectorType>& NewVertices)
    {
        Vertices.Append(NewVertices);
    }
 
    template<typename OtherVectorType>
    void AppendVertices(const TArray<OtherVectorType>& NewVertices)
    {
        int32 NumV = NewVertices.Num();
        Vertices.Reserve(Vertices.Num() + NumV);
        for (int32 k = 0; k < NumV; ++k)
        {
            Vertices.Append( (VectorType)NewVertices[k] );
        }
    }
 
    void Set(int VertexIndex, const VectorType& Position)
    {
        Vertices[VertexIndex] = Position;
    }
 
    void RemoveVertex(int VertexIndex)
    {
        Vertices.RemoveAt(VertexIndex);
    }
 
    void SetVertices(const TArray<VectorType>& NewVertices)
    {
        int NumVerts = NewVertices.Num();
        Vertices.SetNum(NumVerts, EAllowShrinking::No);
        for (int k = 0; k < NumVerts; ++k)
        {
            Vertices[k] = NewVertices[k];
        }
    }
 
 
    void Reverse()
    {
        int32 j = Vertices.Num() - 1;
        for (int32 VertexIndex = 0; VertexIndex < j; VertexIndex++, j--)
        {
            Swap(Vertices[VertexIndex], Vertices[j]);
        }
    }
 
 
    VectorType GetTangent(int VertexIndex) const
    {
        if (VertexIndex == 0)
        {
            return (Vertices[1] - Vertices[0]).Normalized();
        } 
        int NumVerts = Vertices.Num();
        if (VertexIndex == NumVerts - 1)
        {
            return (Vertices[NumVerts-1] - Vertices[NumVerts-2]).Normalized();
        }
        return (Vertices[VertexIndex+1] - Vertices[VertexIndex-1]).Normalized();
    }
 
 
 
    SegmentType GetSegment(int SegmentIndex) const
    {
        return SegmentType(Vertices[SegmentIndex], Vertices[SegmentIndex+1]);
    }
 
 
    VectorType GetSegmentPoint(int SegmentIndex, T SegmentParam) const
    {
        SegmentType seg(Vertices[SegmentIndex], Vertices[SegmentIndex + 1]);
        return seg.PointAt(SegmentParam);
    }
 
 
    VectorType GetSegmentPointUnitParam(int SegmentIndex, T SegmentParam) const
    {
        SegmentType seg(Vertices[SegmentIndex], Vertices[SegmentIndex + 1]);
        return seg.PointBetween(SegmentParam);
    }
 
 
 
    BoxType GetBounds() const
    {
        BoxType box = BoxType::Empty();
        int NumVertices = Vertices.Num();
        for (int k = 0; k < NumVertices; ++k)
        {
            box.Contain(Vertices[k]);
        }
        return box;
    }
 
 
 
    T Length() const
    {
        T length = 0;
        int N = SegmentCount();
        for (int i = 0; i < N; ++i)
        {
            length += Distance(Vertices[i], Vertices[i+1]);
        }
        return length;
    }
 
 
 
    class SegmentIterator
    {
    public:
        inline bool operator!()
        {
            return i < Polyline->SegmentCount();
        }
        inline SegmentType operator*() const
        {
            check(Polyline != nullptr && i < Polyline->SegmentCount());
            return SegmentType(Polyline->Vertices[i], Polyline->Vertices[i+1]);
        }
        inline SegmentIterator & operator++()       // prefix
        {
            i++;
            return *this;
        }
        inline SegmentIterator operator++(int)      // postfix
        {
            SegmentIterator copy(*this);
            i++;
            return copy;
        }
        inline bool operator==(const SegmentIterator & i3) { return i3.Polyline == Polyline && i3.i == i; }
        inline bool operator!=(const SegmentIterator & i3) { return i3.Polyline != Polyline || i3.i != i; }
    protected:
        const TPolyline<T, D>* Polyline;
        int i;
        inline SegmentIterator(const  TPolyline<T, D>* p, int iCur) : Polyline(p), i(iCur) {}
        friend class TPolyline;
    };
    friend class SegmentIterator;
 
    SegmentIterator SegmentItr() const
    {
        return SegmentIterator(this, 0);
    }
 
    class SegmentEnumerable
    {
    public:
        const TPolyline<T,D>* Polyline;
        SegmentEnumerable() : Polyline(nullptr) {}
        SegmentEnumerable(const TPolyline<T,D> * p) : Polyline(p) {}
        SegmentIterator begin() { return Polyline->SegmentItr(); }
        SegmentIterator end() { return SegmentIterator(Polyline, Polyline->SegmentCount()); }
    };
 
    SegmentEnumerable Segments() const
    {
        return SegmentEnumerable(this);
    }
 
 
 
 
 
 
    T DistanceSquared(const VectorType& QueryPoint, int& NearestSegIndexOut, T& NearestSegParamOut) const
    {
        NearestSegIndexOut = -1;
        NearestSegParamOut = TNumericLimits<T>::Max();
        T dist = TNumericLimits<T>::Max();
        int N = SegmentCount();
        for (int vi = 0; vi < N; ++vi)
        {
            // @todo can't we just use segment function here now?
            SegmentType seg = SegmentType(Vertices[vi], Vertices[vi+1]);
            T t = (QueryPoint - seg.Center).Dot(seg.Direction);
            T d = TNumericLimits<T>::Max();
            if (t >= seg.Extent)
            {
                d = UE::Geometry::DistanceSquared(seg.EndPoint(), QueryPoint);
            }
            else if (t <= -seg.Extent)
            {
                d = UE::Geometry::DistanceSquared(seg.StartPoint(), QueryPoint);
            }
            else
            {
                d = UE::Geometry::DistanceSquared(seg.PointAt(t), QueryPoint);
            }
            if (d < dist)
            {
                dist = d;
                NearestSegIndexOut = vi;
                NearestSegParamOut = TMathUtil<T>::Clamp(t, -seg.Extent, seg.Extent);
            }
        }
        return dist;
    }
 
 
 
    T DistanceSquared(const VectorType& QueryPoint) const
    {
        int seg; T segt;
        return DistanceSquared(QueryPoint, seg, segt);
    }
 
 
 
 
    T AverageEdgeLength() const
    {
        T avg = 0; int N = Vertices.Num();
        for (int i = 1; i < N; ++i) {
            avg += Distance(Vertices[i], Vertices[i - 1]);
        }
        return avg / (T)(N-1);
    }
 
    VectorType GetPointFromFirstVertex(const T InDistance) const
    {
        if (InDistance <= 0)
        {
            return Vertices[0];
        }
 
        T RemainingDistance = InDistance;
        const int N = Vertices.Num();
        for (int i = 1; i < N; ++i)
        {
            const T SegmentLength = Distance(Vertices[i], Vertices[i - 1]);
            if (SegmentLength > 0 && SegmentLength > RemainingDistance)
            {
                return Lerp(Vertices[i - 1], Vertices[i], RemainingDistance / SegmentLength);
            }
            RemainingDistance -= SegmentLength;
        }
 
        return Vertices.Last();
    }
 
    VectorType GetPointFromLastVertex(const T InDistance) const
    {
        if (InDistance <= 0)
        {
            return Vertices.Last();
        }
 
        T RemainingDistance = InDistance;
        const int N = Vertices.Num();
        for (int i = N-1; i > 0; --i)
        {
            const T SegmentLength = Distance(Vertices[i], Vertices[i - 1]);
            if (SegmentLength > 0 && SegmentLength > RemainingDistance)
            {
                return Lerp(Vertices[i], Vertices[i-1], RemainingDistance / SegmentLength);
            }
            RemainingDistance -= SegmentLength;
        }
 
        return Vertices[0];
    }
 
    void SmoothSubdivide(TPolyline<T,D>& NewPolyline) const
    {
        const T Alpha = (T)1 / (T)3;
        const T OneMinusAlpha = (T)2 / (T)3;
 
        int N = Vertices.Num() - 1;
        NewPolyline.Vertices.SetNum(2*N);
        NewPolyline.Vertices[0] = Vertices[0];
        int k = 1;
        for (int i = 1; i < N; ++i)
        {
            const VectorType& Prev = Vertices[i-1];
            const VectorType& Cur = Vertices[i];
            const VectorType& Next = Vertices[i+1];
            NewPolyline.Vertices[k++] = Alpha * Prev + OneMinusAlpha * Cur;
            NewPolyline.Vertices[k++] = OneMinusAlpha * Cur + Alpha * Next;
        }
        NewPolyline.Vertices[k] = Vertices[N];
    }
 
    void Simplify(T ClusterTolerance, T LineDeviationTolerance)
    {
        const int32 VertexCount = Vertices.Num();
        if (VertexCount < 3)
        {
            // we need at least 3 vertices to be able to simplify a line
            return;
        }
 
        TArray<VectorType> NewVertices;
        NewVertices.Reserve(VertexCount);
 
        // STAGE 1.  Vertex Reduction within tolerance of prior vertex cluster
        if (ClusterTolerance > 0)
        {
            T ClusterToleranceSquared = ClusterTolerance * ClusterTolerance;
            NewVertices.Add(Vertices[0]);               // keep the first vertex
            for (int32 Index = 1; Index < VertexCount-1; Index++) 
            {
                if (Geometry::DistanceSquared(Vertices[Index], NewVertices.Last()) < ClusterToleranceSquared)
                {
                    continue;
                }
                NewVertices.Add(Vertices[Index]);
            }
            NewVertices.Add(Vertices.Last());       // keep the last vertex
        }
        else
        {
            NewVertices = Vertices;
        }
 
        // STAGE 2.  Douglas-Peucker polyline simplification
        TArray<bool> Marked;
        if (LineDeviationTolerance > 0 && NewVertices.Num() >= 3)
        {
            Marked.SetNumZeroed(NewVertices.Num());
 
            // mark the first and last vertices to make sure we keep them
            Marked[0] = true;
            Marked.Last() = true;
            SimplifyDouglasPeucker(LineDeviationTolerance, NewVertices, 0, NewVertices.Num()-1, Marked);
        }
 
        // STAGE 3. Copy back values in Vertices
        if (Marked.IsEmpty())
        {
            // we have not run Douglas-Pecker so we can just copy the list
            Vertices = MoveTemp(NewVertices);
        }
        else
        {
            // only keep the marked ones
            Vertices.Reset();
            const int32 MarkedCount = Marked.Num();
            for (int32 Index=0; Index<MarkedCount; ++Index)
            {
                if (Marked[Index])
                {
                    Vertices.Add(NewVertices[Index]);
                }
            }
        }
    }
 
private:
    // Polygon simplification
    // code adapted from: http://softsurfer.com/Archive/algorithm_0205/algorithm_0205.htm
    // simplifyDP():
    //  This is the Douglas-Peucker recursive simplification routine
    //  It just marks Vertices that are part of the simplified polyline
    //  for approximating the polyline subchain v[j] to v[k].
    //    Input:  tol = approximation tolerance
    //            v[] = polyline array of vertex points
    //            j,k = indices for the subchain v[j] to v[k]
    //    Output: mk[] = array of markers matching vertex array v[]
    static void SimplifyDouglasPeucker(T Tolerance, const TArray<VectorType>& Vertices, int32 j, int32 k, TArray<bool>& Marked)
    {
        Marked.SetNum(Vertices.Num());
        if (k <= j + 1) // there is nothing to simplify
            return;
 
        // check for adequate approximation by segment S from v[j] to v[k]
        int maxi = j;          // index of vertex farthest from S
        T maxd2 = 0;         // distance squared of farthest vertex
        T tol2 = Tolerance * Tolerance;  // tolerance squared
        SegmentType S = SegmentType(Vertices[j], Vertices[k]);    // segment from v[j] to v[k]
 
        // test each vertex v[i] for max distance from S
        // Note: this works in any dimension (2D, 3D, ...)
        for (int i = j + 1; i < k; i++)
        {
            T dv2 = S.DistanceSquared(Vertices[i]);
            if (dv2 <= maxd2)
                continue;
            // v[i] is a max vertex
            maxi = i;
            maxd2 = dv2;
        }
        if (maxd2 > tol2)       // error is worse than the tolerance
        {
            // split the polyline at the farthest vertex from S
            Marked[maxi] = true;      // mark v[maxi] for the simplified polyline
            // recursively simplify the two subpolylines at v[maxi]
            SimplifyDouglasPeucker(Tolerance, Vertices, j, maxi, Marked);  // polyline v[j] to v[maxi]
            SimplifyDouglasPeucker(Tolerance, Vertices, maxi, k, Marked);  // polyline v[maxi] to v[k]
        }
        // else the approximation is OK, so ignore intermediate Vertices
        return;
    }
};
 
} // end namespace UE::Geometry
} // end namespace UE