From e80ad84d6b7ed42f195c8f477de3c61baf5705cb Mon Sep 17 00:00:00 2001
From: Eunki Hong <eunkiki.hong@samsung.com>
Date: Thu, 31 Aug 2023 02:49:41 +0900
Subject: [PATCH] Optimize dijkstra path finder algorithm

Let we use dijkstra with O(E log E) method, which is optimized.

Change-Id: Ie3afe3ff1a526e1ec6edb6b3d5817834caaf1120
Signed-off-by: Eunki Hong <eunkiki.hong@samsung.com>
---
 .../internal/algorithm/path-finder-djikstra.cpp    | 96 +++++++++++++---------
 1 file changed, 56 insertions(+), 40 deletions(-)

diff --git a/dali-scene3d/internal/algorithm/path-finder-djikstra.cpp b/dali-scene3d/internal/algorithm/path-finder-djikstra.cpp
index 726c1fb..926b673 100644
--- a/dali-scene3d/internal/algorithm/path-finder-djikstra.cpp
+++ b/dali-scene3d/internal/algorithm/path-finder-djikstra.cpp
@@ -20,6 +20,8 @@
 // EXTERNAL INCLUDES
 #include <dali/public-api/common/vector-wrapper.h>
 #include <limits>
+#include <algorithm> ///< for std::priority_queue
+#include <queue>     ///< for std::priority_queue
 
 // INTERNAL INCLUDES
 #include <dali-scene3d/internal/algorithm/path-finder-waypoint-data.h>
@@ -27,6 +29,27 @@
 
 using WayPointList = Dali::Scene3D::Algorithm::WayPointList;
 
+namespace
+{
+struct DijkstraComparer
+{
+  float                               distance{-1.0f};
+  Dali::Scene3D::Algorithm::FaceIndex index{Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE};
+
+  /**
+   * @brief Custom operator for Dijkstra comparer.
+   *
+   * @param[in] rhs Target to be compared.
+   * @return True if this has less priority than rhs.
+   */
+  bool operator<(const DijkstraComparer& rhs) const
+  {
+    // Shorter distance have higher prioirty.
+    return distance > rhs.distance;
+  }
+};
+} // namespace
+
 namespace Dali::Scene3D::Internal::Algorithm
 {
 PathFinderAlgorithmDjikstra::PathFinderAlgorithmDjikstra(Dali::Scene3D::Algorithm::NavigationMesh& navMesh)
@@ -80,80 +103,73 @@ Scene3D::Algorithm::WayPointList PathFinderAlgorithmDjikstra::FindPath(FaceIndex
   auto                   nodeCount = uint32_t(mNodes.size());
   std::vector<float>     dist;
   std::vector<FaceIndex> prev;
+  std::vector<bool>      faceVisited;
+
+  std::priority_queue<DijkstraComparer> priorityDistanceHeap;
 
   dist.resize(mNodes.size());
   prev.resize(mNodes.size());
 
-  std::vector<FaceNode*> nodeQueue;
-  nodeQueue.reserve(nodeCount);
-
-  [[maybe_unused]] auto sourcePos = Dali::Vector3(Face(sourcePolyIndex)->center);
+  faceVisited.resize(nodeCount);
 
   for(auto i = 0u; i < nodeCount; ++i)
   {
     dist[i] = std::numeric_limits<float>::infinity();
     prev[i] = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
-    nodeQueue.emplace_back(&mNodes[i]);
+    faceVisited[i] = false;
   }
-  auto removeCount = 0u;
 
   // Set distance of source
   dist[sourcePolyIndex] = 0.0f;
 
-  // TO OPTIMIZE WITH PRIORITY QUEUE
-  auto FindMinDistance = [&nodeQueue](decltype(dist)& dist) {
-    float w     = std::numeric_limits<float>::max();
-    int   index = -1;
-    for(auto i = 0u; i < dist.size(); ++i)
-    {
-      // skip infinity
-      if(nodeQueue[i] && dist[i] != std::numeric_limits<float>::infinity() && dist[i] < w)
-      {
-        w     = dist[i];
-        index = int(i);
-      }
-    }
-    return index;
-  };
+  priorityDistanceHeap.push({dist[sourcePolyIndex], sourcePolyIndex});
 
-  do
+  while(!priorityDistanceHeap.empty())
   {
-    // find minimum distance
-    auto minDistIndex = FindMinDistance(dist);
+    // find minimum distance index
+    auto comparer = priorityDistanceHeap.top();
+    priorityDistanceHeap.pop();
 
-    // Failed to find minimum distance
-    if(minDistIndex == -1)
-    {
-      // Return empty WayPointList
-      return {};
-    }
+    auto  minDistanceIndex = comparer.index;
 
-    // remove from queue by assigning infinity to distance
-    removeCount++;
-    if(removeCount == nodeCount)
+    // Old item. just ignore.
+    if(faceVisited[minDistanceIndex])
     {
       continue;
     }
 
-    nodeQueue[minDistIndex] = nullptr;
+    faceVisited[minDistanceIndex] = true;
 
-    [[maybe_unused]] auto sizeTmp = nodeQueue.size() - removeCount;
+    // Fast break if we found solution.
+    if(minDistanceIndex == targetPolyIndex)
+    {
+      break;
+    }
 
     // check the neighbours
     for(auto i = 0u; i < 3; ++i)
     {
-      auto nIndex = mNodes[minDistIndex].faces[i];
-      if(nIndex != Scene3D::Algorithm::NavigationMesh::NULL_FACE && nodeQueue[nIndex] != nullptr)
+      auto nIndex = mNodes[minDistanceIndex].faces[i];
+      if(nIndex != Scene3D::Algorithm::NavigationMesh::NULL_FACE && !faceVisited[nIndex])
       {
-        auto alt = dist[minDistIndex] + mNodes[minDistIndex].weight[i];
+        auto alt = dist[minDistanceIndex] + mNodes[minDistanceIndex].weight[i];
         if(alt < dist[nIndex])
         {
           dist[nIndex] = alt;
-          prev[nIndex] = minDistIndex;
+          prev[nIndex] = minDistanceIndex;
+
+          priorityDistanceHeap.push({dist[nIndex], nIndex});
         }
       }
     }
-  } while(removeCount != nodeCount);
+  }
+
+  // Failed to find minimum distance
+  if(!faceVisited[targetPolyIndex])
+  {
+    // Return empty WayPointList
+    return {};
+  }
 
   // Compute distances for each node back to the source
   auto                 u = targetPolyIndex;
-- 
2.7.4