MeshDijkstra.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
#pragma once
 
#include "VectorTypes.h"
#include "MatrixTypes.h"
#include "BoxTypes.h"
#include "FrameTypes.h"
#include "Util/IndexPriorityQueue.h"
#include "Util/DynamicVector.h"
 
namespace UE
{
namespace Geometry
{
 
template<class PointSetType>
class TMeshDijkstra
{
public:
    const PointSetType* PointSet;
 
    TUniqueFunction<FVector3d(int32)> GetPositionFunc;
 
    bool bEnableDistanceWeighting = false;
 
    TUniqueFunction<double(int32 FromVID, int32 ToVID, int32 SeedVID, double EuclideanDistance)> GetWeightedDistanceFunc;
 
 
    TMeshDijkstra(const PointSetType* PointSetIn)
    {
        PointSet = PointSetIn;
 
        int32 MaxID = PointSet->MaxVertexID();
        Queue.Initialize(MaxID);
 
        MaxGraphDistance = 0.0;
        MaxGraphDistancePointID = -1;
 
        GetPositionFunc = [this](int32 PointID) { return  PointSet->GetVertex(PointID); };
        bEnableDistanceWeighting = false;
        GetWeightedDistanceFunc = [](int32, int32, int32, double Distance) { return Distance; };
    }
 
    static double InvalidDistance() { return TNumericLimits<double>::Max(); };
 
    void Reset()
    {
        IDToNodeIndexMap.Reset();
        AllocatedNodes.Clear();
        Queue.Clear(false);
        MaxGraphDistance = 0.0;
        MaxGraphDistancePointID = -1;
    }
 
 
    struct FSeedPoint
    {
        int32 ExternalID = -1;
        int32 PointID = 0;
        double StartDistance = 0;
    };
 
 
    void ComputeToMaxDistance(const TArray<FSeedPoint>& SeedPointsIn, double ComputeToMaxDistanceIn)
    {
        MaxGraphDistance = 0.0f;
        MaxGraphDistancePointID = -1;
 
        SeedPoints.Reset();
        for (const FSeedPoint& SeedPoint : SeedPointsIn)
        {
            int32 NewIndex = SeedPoints.Num();
            SeedPoints.Add(SeedPoint);
 
            int32 PointID = SeedPoint.PointID;
            if (ensure(Queue.Contains(PointID) == false))
            {
                FGraphNode* Node = GetNodeForPointSetID(PointID, true);
                Node->GraphDistance = SeedPoint.StartDistance;
                Node->bFrozen = true;
                Node->SeedPointID = NewIndex;
                Queue.Insert(PointID, float(Node->GraphDistance));
            }
        }
 
        while (Queue.GetCount() > 0)
        {
            int32 NextID = Queue.Dequeue();
            FGraphNode* Node = GetNodeForPointSetID(NextID, false);
            check(Node != nullptr);
 
            MaxGraphDistance = TMathUtil<double>::Max(Node->GraphDistance, MaxGraphDistance);
            if (MaxGraphDistance > ComputeToMaxDistanceIn)
            {
                return;
            }
 
            Node->bFrozen = true;
            MaxGraphDistancePointID = Node->PointID;
            UpdateNeighboursSparse(Node);
        }
    }
 
 
 
    void ComputeToMaxDistance(const TArray<FVector2d>& SeedPointsIn, double ComputeToMaxDistanceIn)
    {
        TArray<FSeedPoint> ConvertedSeedPoints;
        int32 N = SeedPointsIn.Num();
        ConvertedSeedPoints.SetNum(SeedPointsIn.Num());
        for (int32 k = 0; k < N; ++k)
        {
            ConvertedSeedPoints[k] = FSeedPoint{ -1, (int)SeedPointsIn[k].X, SeedPointsIn[k].Y };
        }
        ComputeToMaxDistance(ConvertedSeedPoints, ComputeToMaxDistanceIn);
    }
 
 
 
    bool ComputeToTargetPoint(const TArray<FSeedPoint>& SeedPointsIn, int32 TargetPointID, double ComputeToMaxDistanceIn = TNumericLimits<double>::Max())
    {
        MaxGraphDistance = 0.0f;
        MaxGraphDistancePointID = -1;
 
        SeedPoints.Reset();
        for (const FSeedPoint& SeedPoint : SeedPointsIn)
        {
            int32 NewIndex = SeedPoints.Num();
            SeedPoints.Add(SeedPoint);
 
            int32 PointID = SeedPoint.PointID;
            if (ensure(Queue.Contains(PointID) == false))
            {
                FGraphNode* Node = GetNodeForPointSetID(PointID, true);
                Node->GraphDistance = SeedPoint.StartDistance;
                Node->bFrozen = true;
                Node->SeedPointID = NewIndex;
                Queue.Insert(PointID, (float)Node->GraphDistance);
            }
        }
 
        while (Queue.GetCount() > 0)
        {
            int32 NextID = Queue.Dequeue();
            FGraphNode* Node = GetNodeForPointSetID(NextID, false);
            check(Node != nullptr);
 
            MaxGraphDistance = TMathUtil<double>::Max(Node->GraphDistance, MaxGraphDistance);
            if (MaxGraphDistance > ComputeToMaxDistanceIn)
            {
                return false;
            }
 
            Node->bFrozen = true;
            MaxGraphDistancePointID = Node->PointID;
 
            if (Node->PointID == TargetPointID)
            {
                return true;
            }
 
            UpdateNeighboursSparse(Node);
        }
 
        return false;
    }
 
 
 
 
    double GetMaxGraphDistance() const 
    {
        return MaxGraphDistance;
    }
 
 
    int32 GetMaxGraphDistancePointID() const
    {
        return MaxGraphDistancePointID;
    }
 
 
    int32 GetSeedExternalIDForPointSetID(int32 PointID)
    {
        const FGraphNode* Node = GetNodeForPointSetID(PointID);
        if (Node == nullptr || Node->bFrozen == false)
        {
            return -1;
        }
        int32 SeedIndex = Node->SeedPointID;
        return SeedPoints[SeedIndex].ExternalID;
    }
 
 
    bool HasDistance(int32 PointID) const
    {
        const FGraphNode* Node = GetNodeForPointSetID(PointID);
        return (Node != nullptr && Node->bFrozen);
    }
 
 
    double GetDistance(int32 PointID) const
    {
        const FGraphNode* Node = GetNodeForPointSetID(PointID);
        return (Node != nullptr && Node->bFrozen) ? Node->GraphDistance : InvalidDistance();
    }
 
    bool FindPathToNearestSeed(int32 PointID, TArray<int32>& PathToSeedOut, int32 MaxLength = 100000)
    {
        const FGraphNode* CurNode = GetNodeForPointSetID(PointID);
        if (CurNode == nullptr || CurNode->bFrozen == false)
        {
            return false;
        }
 
        PathToSeedOut.Reset();
        PathToSeedOut.Add(PointID);
 
        int32 IterCount = 0;
        while (IterCount++ < MaxLength)
        {
            if (CurNode->ParentPointID == -1)
            {
                return true;
            }
 
            PathToSeedOut.Add(CurNode->ParentPointID);
 
            CurNode = GetNodeForPointSetID(CurNode->ParentPointID);
            if (CurNode == nullptr || CurNode->bFrozen == false)
            {
                PathToSeedOut.Reset();
                return false;
            }
        }
        return true;
    }
 
private:
 
    // information about each active/computed point
    struct FGraphNode
    {
        int32 PointID;
        int32 ParentPointID;
        int32 SeedPointID;
        double GraphDistance;
        bool bFrozen;
    };
 
    // To avoid constructing FGraphNode for all input points (because we are computing a "local" param),
    // we only allocate on demand, and then store a sparse mapping in IDToNodeIndexMap
    TMap<int32, int32> IDToNodeIndexMap;
    TDynamicVector<FGraphNode> AllocatedNodes;
 
    // queue of nodes to process (for dijkstra front propagation)
    FIndexPriorityQueue Queue;
 
    TArray<FSeedPoint> SeedPoints;
 
    // max distances encountered during last compute
    double MaxGraphDistance;
 
    int32 MaxGraphDistancePointID;
 
    FGraphNode* GetNodeForPointSetID(int32 PointSetID, bool bCreateIfMissing)
    {
        const int32* AllocatedIndex = IDToNodeIndexMap.Find(PointSetID);
        if (AllocatedIndex == nullptr)
        {
            if (bCreateIfMissing)
            {
                FGraphNode NewNode{ PointSetID, -1, 0, false };
                int32 NewIndex = AllocatedNodes.Num();
                AllocatedNodes.Add(NewNode);
                IDToNodeIndexMap.Add(PointSetID, NewIndex);
                return &AllocatedNodes[NewIndex];
            }
            else
            {
                return nullptr;
            }
        }
        else
        {
            return &AllocatedNodes[*AllocatedIndex];
        }
    }
 
 
    const FGraphNode* GetNodeForPointSetID(int32 PointSetID) const
    {
        const int32* AllocatedIndex = IDToNodeIndexMap.Find(PointSetID);
        return (AllocatedIndex != nullptr) ? &AllocatedNodes[*AllocatedIndex] : nullptr;
    }
 
 
    // given new Distance/UV at Parent, check if any of its neighbours are in the queue,
    // and if they are, and the new graph distance is shorter, update their queue position
    // (this is basically the update step of Disjktras algorithm)
    void UpdateNeighboursSparse(FGraphNode* Parent)
    {
        FVector3d ParentPos(GetPositionFunc(Parent->PointID));
        double ParentDist = Parent->GraphDistance;
 
        for (int32 NbrPointID : PointSet->VtxVerticesItr(Parent->PointID))
        {
            FGraphNode* NbrNode = GetNodeForPointSetID(NbrPointID, true);
            if (NbrNode->bFrozen)
            {
                continue;
            }
 
            double LocalDist = Distance(ParentPos, GetPositionFunc(NbrPointID));
            if (bEnableDistanceWeighting)
            {
                int SeedPointID = SeedPoints[Parent->SeedPointID].PointID;
                LocalDist = GetWeightedDistanceFunc(Parent->PointID, NbrPointID, SeedPointID, LocalDist);
            }
            double NbrDist = ParentDist + LocalDist;
 
            if (Queue.Contains(NbrPointID))
            {
                if (NbrDist < NbrNode->GraphDistance)
                {
                    NbrNode->ParentPointID = Parent->PointID;
                    NbrNode->GraphDistance = NbrDist;
                    NbrNode->SeedPointID = Parent->SeedPointID;
                    Queue.Update(NbrPointID, float(NbrNode->GraphDistance));
                }
            }
            else 
            {
                NbrNode->ParentPointID = Parent->PointID;
                NbrNode->GraphDistance = NbrDist;
                NbrNode->SeedPointID = Parent->SeedPointID;
                Queue.Insert(NbrPointID, float(NbrNode->GraphDistance));
            }
        }
    }
 
};
 
 
} // end namespace UE::Geometry
} // end namespace UE