dali-scene3d/internal/algorithm/path-finder-dijkstra.cpp

   1 /*
   2  * Copyright (c) 2023 Samsung Electronics Co., Ltd.
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  * http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17 // CLASS HEADER
  18 #include <dali-scene3d/internal/algorithm/path-finder-dijkstra.h>
  19
  20 // EXTERNAL INCLUDES
  21 #include <dali/public-api/common/vector-wrapper.h>
  22 #include <limits>
  23 #include <algorithm> ///< for std::priority_queue
  24 #include <queue>     ///< for std::priority_queue
  25
  26 // INTERNAL INCLUDES
  27 #include <dali-scene3d/internal/algorithm/path-finder-waypoint-data.h>
  28 #include <dali-scene3d/public-api/algorithm/path-finder-waypoint.h>
  29
  30 using WayPointList = Dali::Scene3D::Algorithm::WayPointList;
  31
  32 namespace
  33 {
  34 struct DijkstraComparer
  35 {
  36   float                               distance{-1.0f};
  37   Dali::Scene3D::Algorithm::FaceIndex index{Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE};
  38
  39   /**
  40    * @brief Custom operator for Dijkstra comparer.
  41    *
  42    * @param[in] rhs Target to be compared.
  43    * @return True if this has less priority than rhs.
  44    */
  45   bool operator<(const DijkstraComparer& rhs) const
  46   {
  47     // Shorter distance have higher prioirty.
  48     return distance > rhs.distance;
  49   }
  50 };
  51 } // namespace
  52
  53 namespace Dali::Scene3D::Internal::Algorithm
  54 {
  55 PathFinderAlgorithmDijkstra::PathFinderAlgorithmDijkstra(Dali::Scene3D::Algorithm::NavigationMesh& navMesh)
  56 : mNavigationMesh(&GetImplementation(navMesh))
  57 {
  58   PrepareData();
  59 }
  60
  61 PathFinderAlgorithmDijkstra::~PathFinderAlgorithmDijkstra() = default;
  62
  63 Scene3D::Algorithm::WayPointList PathFinderAlgorithmDijkstra::FindPath(const Dali::Vector3& positionFrom, const Dali::Vector3& positionTo)
  64 {
  65   Dali::Vector3 outPosFrom;
  66   FaceIndex     polyIndexFrom;
  67   auto          result = mNavigationMesh->FindFloor(positionFrom, outPosFrom, polyIndexFrom);
  68
  69   Scene3D::Algorithm::WayPointList waypoints;
  70
  71   if(result)
  72   {
  73     Dali::Vector3 outPosTo;
  74     FaceIndex     polyIndexTo;
  75     result = mNavigationMesh->FindFloor(positionTo, outPosTo, polyIndexTo);
  76
  77     if(result)
  78     {
  79       // Get waypoints
  80       waypoints = FindPath(polyIndexFrom, polyIndexTo);
  81
  82       if(!waypoints.empty())
  83       {
  84         // replace first and last waypoint
  85         auto& wpFrom = static_cast<WayPointData&>(waypoints[0]);
  86         auto& wpTo   = static_cast<WayPointData&>(waypoints.back());
  87
  88         Vector2 fromCenter(wpFrom.point3d.x, wpFrom.point3d.y);
  89         wpFrom.point3d = outPosFrom;
  90         wpFrom.point2d = fromCenter - Vector2(outPosFrom.x, outPosFrom.y);
  91
  92         Vector2 toCenter(wpTo.point3d.x, wpTo.point3d.y);
  93         wpTo.point3d = outPosTo;
  94         wpTo.point2d = toCenter - Vector2(outPosTo.x, outPosTo.y);
  95       }
  96     }
  97   }
  98
  99   // Returns waypoints with non-zero size of empty vector in case of failure (no path to be found)
 100   return waypoints;
 101 }
 102
 103 Scene3D::Algorithm::WayPointList PathFinderAlgorithmDijkstra::FindPath(FaceIndex sourcePolyIndex, FaceIndex targetPolyIndex)
 104 {
 105   auto                   nodeCount = uint32_t(mNodes.size());
 106   std::vector<float>     dist;
 107   std::vector<FaceIndex> prev;
 108   std::vector<bool>      faceVisited;
 109
 110   std::priority_queue<DijkstraComparer> priorityDistanceHeap;
 111
 112   dist.resize(mNodes.size());
 113   prev.resize(mNodes.size());
 114
 115   faceVisited.resize(nodeCount);
 116
 117   for(auto i = 0u; i < nodeCount; ++i)
 118   {
 119     dist[i] = std::numeric_limits<float>::infinity();
 120     prev[i] = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 121     faceVisited[i] = false;
 122   }
 123
 124   // Set distance of source
 125   dist[sourcePolyIndex] = 0.0f;
 126
 127   priorityDistanceHeap.push({dist[sourcePolyIndex], sourcePolyIndex});
 128
 129   while(!priorityDistanceHeap.empty())
 130   {
 131     // find minimum distance index
 132     auto comparer = priorityDistanceHeap.top();
 133     priorityDistanceHeap.pop();
 134
 135     auto  minDistanceIndex = comparer.index;
 136
 137     // Old item. just ignore.
 138     if(faceVisited[minDistanceIndex])
 139     {
 140       continue;
 141     }
 142
 143     faceVisited[minDistanceIndex] = true;
 144
 145     // Fast break if we found solution.
 146     if(minDistanceIndex == targetPolyIndex)
 147     {
 148       break;
 149     }
 150
 151     // check the neighbours
 152     for(auto i = 0u; i < 3; ++i)
 153     {
 154       auto nIndex = mNodes[minDistanceIndex].faces[i];
 155       if(nIndex != Scene3D::Algorithm::NavigationMesh::NULL_FACE && !faceVisited[nIndex])
 156       {
 157         auto alt = dist[minDistanceIndex] + mNodes[minDistanceIndex].weight[i];
 158         if(alt < dist[nIndex])
 159         {
 160           dist[nIndex] = alt;
 161           prev[nIndex] = minDistanceIndex;
 162
 163           priorityDistanceHeap.push({dist[nIndex], nIndex});
 164         }
 165       }
 166     }
 167   }
 168
 169   // Failed to find minimum distance
 170   if(!faceVisited[targetPolyIndex])
 171   {
 172     // Return empty WayPointList
 173     return {};
 174   }
 175
 176   // Compute distances for each node back to the source
 177   auto                 u = targetPolyIndex;
 178   std::list<FaceIndex> q;
 179   if(prev[u] != Scene3D::Algorithm::NavigationMesh::NULL_FACE || u == sourcePolyIndex)
 180   {
 181     while(u != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 182     {
 183       q.push_front(u);
 184       u = prev[u];
 185     }
 186   }
 187
 188   WayPointList waypoints;
 189   waypoints.resize(q.size());
 190
 191   auto index = 0u;
 192   auto prevN = 0u;
 193   for(auto n : q)
 194   {
 195     auto& wp     = static_cast<WayPointData&>(waypoints[index]);
 196     wp.face      = mNavigationMesh->GetFace(n);
 197     wp.nodeIndex = n;
 198
 199     wp.edge = nullptr;
 200     // set the common edge with previous node
 201     if(index > 0)
 202     {
 203       const auto& node = mNodes[prevN];
 204       for(auto i = 0u; i < 3; ++i)
 205       {
 206         if(node.faces[i] == wp.nodeIndex)
 207         {
 208           wp.edge = mNavigationMesh->GetEdge(node.edges[i]);
 209           break;
 210         }
 211       }
 212     }
 213
 214     prevN = n;
 215     index++;
 216   }
 217
 218   return OptimizeWaypoints(waypoints);
 219 }
 220
 221 void PathFinderAlgorithmDijkstra::PrepareData()
 222 {
 223   // Build the list structure connecting the nodes
 224   auto faceCount = mNavigationMesh->GetFaceCount();
 225
 226   mNodes.resize(faceCount);
 227
 228   // for each face build the list
 229   // TODO : Currently, we are assume that FaceNodeIndex is matched with FaceIndex 1:1. This might be changed in future.
 230   for(auto i = 0u; i < faceCount; ++i)
 231   {
 232     auto&       node = mNodes[i];
 233     const auto* face = mNavigationMesh->GetFace(i);
 234     auto        c0   = Dali::Vector3(face->center);
 235
 236     // for each edge add neighbouring face and compute distance to set the weight of node
 237     for(auto edgeIndex = 0u; edgeIndex < 3; ++edgeIndex)
 238     {
 239       const auto* edge = mNavigationMesh->GetEdge(face->edge[edgeIndex]);
 240       auto        p1   = edge->face[0];
 241       auto        p2   = edge->face[1];
 242
 243       // One of faces is current face so ignore it
 244       auto p                = ((p1 != i) ? p1 : p2);
 245       node.faces[edgeIndex] = p;
 246       if(p != ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 247       {
 248         node.edges[edgeIndex]  = face->edge[edgeIndex];
 249         auto c1                = Dali::Vector3(mNavigationMesh->GetFace(p)->center);
 250         node.weight[edgeIndex] = (c1 - c0).Length();
 251       }
 252     }
 253   }
 254 }
 255
 256 [[maybe_unused]] static float ccw(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C)
 257 {
 258   return (C.y - A.y) * (B.x - A.x) > (B.y - A.y) * (C.x - A.x);
 259 }
 260
 261 [[maybe_unused]] static bool intersect(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C, const Dali::Vector2& D)
 262 {
 263   return ccw(A, C, D) != ccw(B, C, D) && ccw(A, B, C) != ccw(A, B, D);
 264 }
 265
 266 Scene3D::Algorithm::WayPointList PathFinderAlgorithmDijkstra::OptimizeWaypoints(WayPointList& waypoints) const
 267 {
 268   WayPointList optimizedWaypoints;
 269   optimizedWaypoints.emplace_back(waypoints[0]);
 270   optimizedWaypoints.reserve(waypoints.size());
 271
 272   auto startIndex = 1u;
 273
 274   bool finished = false;
 275   for(auto j = 0; !finished; ++j)
 276   {
 277     auto&       startWaypoint     = optimizedWaypoints.back();
 278     const auto& startWaypointData = static_cast<const WayPointData&>(startWaypoint);
 279
 280     // add new-last waypoint which will be overriden as long as intersection takes place
 281     optimizedWaypoints.emplace_back();
 282     for(auto wpIndex = startIndex; wpIndex < waypoints.size(); ++wpIndex)
 283     {
 284       if(wpIndex == waypoints.size() - 1)
 285       {
 286         optimizedWaypoints.back() = waypoints.back();
 287         finished                  = true;
 288         continue;
 289       }
 290       // Points between centres of faces
 291
 292       const auto& wpData = static_cast<const WayPointData&>(waypoints[wpIndex]);
 293
 294       auto Pa0 = Dali::Vector2(startWaypointData.face->center[0], startWaypointData.face->center[1]);
 295       auto Pa1 = Dali::Vector2(wpData.face->center[0], wpData.face->center[1]);
 296
 297       bool doesIntersect = true;
 298       for(auto i = startIndex; i < wpIndex; ++i)
 299       {
 300         const auto& wp = static_cast<WayPointData&>(waypoints[i]);
 301         // Skip starting waypoint
 302         if(wp.face == startWaypointData.face)
 303         {
 304           continue;
 305         }
 306         auto Pb0  = mNavigationMesh->GetVertex(wp.edge->vertex[0]);
 307         auto Pb1  = mNavigationMesh->GetVertex(wp.edge->vertex[1]);
 308         auto vPb0 = Dali::Vector2(Pb0->x, Pb0->y);
 309         auto vPb1 = Dali::Vector2(Pb1->x, Pb1->y);
 310
 311         doesIntersect = intersect(Pa0, Pa1, vPb0, vPb1);
 312         if(!doesIntersect)
 313         {
 314           break;
 315         }
 316       }
 317
 318       if(!doesIntersect)
 319       {
 320         optimizedWaypoints.back() = waypoints[wpIndex - 1];
 321         startIndex                = wpIndex - 1;
 322         break;
 323       }
 324     }
 325   }
 326
 327   for(auto& wp : optimizedWaypoints)
 328   {
 329     auto& wpData   = static_cast<WayPointData&>(wp);
 330     wpData.point3d = mNavigationMesh->PointLocalToScene(Dali::Vector3(wpData.face->center));
 331     wpData.point2d = Vector2::ZERO;
 332   }
 333
 334   return optimizedWaypoints;
 335 }
 336 } // namespace Dali::Scene3D::Internal::Algorithm