dali-scene3d/internal/algorithm/path-finder-spfa-double-way.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-spfa-double-way.h>
  19
  20 // EXTERNAL INCLUDES
  21 #include <dali/public-api/common/vector-wrapper.h>
  22 #include <limits>
  23 #include <unordered_set>
  24
  25 // INTERNAL INCLUDES
  26 #include <dali-scene3d/internal/algorithm/path-finder-waypoint-data.h>
  27 #include <dali-scene3d/public-api/algorithm/path-finder-waypoint.h>
  28
  29 using WayPointList  = Dali::Scene3D::Algorithm::WayPointList;
  30 using FaceNodeIndex = Dali::Scene3D::Internal::Algorithm::PathFinderAlgorithmSPFADoubleWay::FaceNodeIndex;
  31
  32 namespace
  33 {
  34 constexpr float PRIORITY_SCALE_FACTOR = 0.7f; ///< The value of heuristic factor that how much will you consider
  35                                               ///  direction of source --> target. If 0.0f, we will use only dist.
  36
  37 /**
  38  * @brief Get the Component Id object
  39  *
  40  * @param[in,out] components Container of components id stored.
  41  * @param[in] index index what we want to get components's id.
  42  * @return FaceIndex top-value of this components.
  43  */
  44 FaceNodeIndex GetComponentId(std::vector<FaceNodeIndex>& components, FaceNodeIndex index)
  45 {
  46   if(components[index] == index)
  47   {
  48     return index;
  49   }
  50   // Get my parent's components id, and update myself.
  51   FaceNodeIndex ret        = GetComponentId(components, components[index]);
  52   return components[index] = ret;
  53 }
  54
  55 /**
  56  * @brief Combine two elements by Union-Find algorithm.
  57  *
  58  * @param[in,out] components Container of components id stored.
  59  * @param[in,out] componentsLevel Container of components level stored.
  60  * @param[in] index0 index of components what we want to be combined.
  61  * @param[in] index1 index of components what we want to be combined.
  62  */
  63 void ComponentsCombine(std::vector<FaceNodeIndex>& components, std::vector<FaceNodeIndex>& componentsLevel, FaceNodeIndex index0, FaceNodeIndex index1)
  64 {
  65   FaceNodeIndex ancestor0 = GetComponentId(components, index0);
  66   FaceNodeIndex ancestor1 = GetComponentId(components, index1);
  67   if(ancestor0 == ancestor1)
  68   {
  69     return;
  70   }
  71
  72   if(componentsLevel[ancestor0] < componentsLevel[ancestor1])
  73   {
  74     components[ancestor0] = ancestor1;
  75   }
  76   else
  77   {
  78     components[ancestor1] = ancestor0;
  79     if(componentsLevel[ancestor0] == componentsLevel[ancestor1])
  80     {
  81       ++componentsLevel[ancestor0];
  82     }
  83   }
  84 }
  85 } // namespace
  86
  87 namespace Dali::Scene3D::Internal::Algorithm
  88 {
  89 PathFinderAlgorithmSPFADoubleWay::PathFinderAlgorithmSPFADoubleWay(Dali::Scene3D::Algorithm::NavigationMesh& navMesh)
  90 : mNavigationMesh(&GetImplementation(navMesh))
  91 {
  92   PrepareData();
  93 }
  94
  95 PathFinderAlgorithmSPFADoubleWay::~PathFinderAlgorithmSPFADoubleWay() = default;
  96
  97 float PathFinderAlgorithmSPFADoubleWay::DistancePanaltyCalculate(FaceIndex index) const noexcept
  98 {
  99   return dist[index] - priority[index] * PRIORITY_SCALE_FACTOR;
 100 }
 101
 102 Scene3D::Algorithm::WayPointList PathFinderAlgorithmSPFADoubleWay::FindPath(const Dali::Vector3& positionFrom, const Dali::Vector3& positionTo)
 103 {
 104   Dali::Vector3 outPosFrom;
 105   FaceIndex     polyIndexFrom;
 106   auto          result = mNavigationMesh->FindFloor(positionFrom, outPosFrom, polyIndexFrom);
 107
 108   Scene3D::Algorithm::WayPointList waypoints;
 109
 110   if(result)
 111   {
 112     Dali::Vector3 outPosTo;
 113     FaceIndex     polyIndexTo;
 114     result = mNavigationMesh->FindFloor(positionTo, outPosTo, polyIndexTo);
 115
 116     if(result)
 117     {
 118       // Get waypoints
 119       waypoints = FindPath(polyIndexFrom, polyIndexTo);
 120
 121       // replace first and last waypoint
 122       auto& wpFrom = static_cast<WayPointData&>(waypoints[0]);
 123       auto& wpTo   = static_cast<WayPointData&>(waypoints.back());
 124
 125       Vector2 fromCenter(wpFrom.point3d.x, wpFrom.point3d.y);
 126       wpFrom.point3d = outPosFrom;
 127       wpFrom.point2d = fromCenter - Vector2(outPosFrom.x, outPosFrom.y);
 128
 129       Vector2 toCenter(wpTo.point3d.x, wpTo.point3d.y);
 130       wpTo.point3d = outPosTo;
 131       wpTo.point2d = toCenter - Vector2(outPosTo.x, outPosTo.y);
 132       wpTo.point3d = outPosTo;
 133     }
 134   }
 135
 136   // Returns waypoints with non-zero size of empty vector in case of failure (no path to be found)
 137   return waypoints;
 138 }
 139
 140 Scene3D::Algorithm::WayPointList PathFinderAlgorithmSPFADoubleWay::FindPath(FaceIndex sourcePolyIndex, FaceIndex targetPolyIndex)
 141 {
 142   // Fast return if source and target index is same.
 143   if(sourcePolyIndex == targetPolyIndex)
 144   {
 145     WayPointList waypoints;
 146     waypoints.resize(1);
 147
 148     auto& wp     = static_cast<WayPointData&>(waypoints[0]);
 149     wp.face      = mNavigationMesh->GetFace(sourcePolyIndex);
 150     wp.nodeIndex = sourcePolyIndex;
 151     wp.edge      = nullptr;
 152
 153     return OptimizeWaypoints(waypoints);
 154   }
 155
 156   // Fast return if source and target index is not in same components.
 157   // That mean, there is no path. Return empty list.
 158   if(GetComponentId(componentIds, sourcePolyIndex) != GetComponentId(componentIds, targetPolyIndex))
 159   {
 160     return WayPointList();
 161   }
 162
 163   // pair<navimesh FaceIndex, is backward direction>
 164   using queueItem = std::pair<FaceIndex, uint8_t>;
 165
 166   std::list<queueItem> nodeQueue;
 167
 168   std::unordered_set<FaceIndex> usedPolyIndexs[2];
 169
 170   // Set distance of source and target
 171   dist[sourcePolyIndex]     = 0.0f;
 172   dist[targetPolyIndex]     = 0.0f;
 173   priority[sourcePolyIndex] = 0.0f;
 174   priority[targetPolyIndex] = 0.0f;
 175   queued[sourcePolyIndex]   = true;
 176   queued[targetPolyIndex]   = true;
 177   nodeQueue.push_back(std::make_pair(sourcePolyIndex, 0));
 178   nodeQueue.push_back(std::make_pair(targetPolyIndex, 1));
 179   usedPolyIndexs[0].insert(sourcePolyIndex);
 180   usedPolyIndexs[1].insert(targetPolyIndex);
 181
 182   bool      foundPath          = false;
 183   FaceIndex forwardEndIndex    = Scene3D::Algorithm::NavigationMesh::NULL_FACE;
 184   FaceIndex backwardStartIndex = Scene3D::Algorithm::NavigationMesh::NULL_FACE;
 185
 186   const auto sourcePos = Dali::Vector3(Face(sourcePolyIndex)->center);
 187   const auto targetPos = Dali::Vector3(Face(targetPolyIndex)->center);
 188   Vector3    direction = targetPos - sourcePos;
 189   direction.Normalize();
 190
 191   // Note : we always success to found path since source and target is in same components.
 192   while(!foundPath)
 193   {
 194     // find minimum distance
 195     auto minDistIndex = nodeQueue.front().first;
 196     auto isBackward   = nodeQueue.front().second;
 197     nodeQueue.pop_front();
 198     queued[minDistIndex] = false;
 199
 200     // check the neighbours
 201     for(auto i = 0u; i < 3 && !foundPath; ++i)
 202     {
 203       auto nIndex = mNodes[minDistIndex].faces[i];
 204       if(nIndex != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 205       {
 206         if(usedPolyIndexs[!isBackward].count(nIndex))
 207         {
 208           // We found path!
 209           foundPath = true;
 210           if(isBackward)
 211           {
 212             forwardEndIndex    = nIndex;
 213             backwardStartIndex = minDistIndex;
 214           }
 215           else
 216           {
 217             forwardEndIndex    = minDistIndex;
 218             backwardStartIndex = nIndex;
 219           }
 220           break;
 221         }
 222
 223         usedPolyIndexs[isBackward].insert(nIndex);
 224
 225         auto alt = dist[minDistIndex] + mNodes[minDistIndex].weight[i];
 226         if(alt < dist[nIndex])
 227         {
 228           dist[nIndex] = alt;
 229
 230           if(isBackward)
 231           {
 232             prevBackward[nIndex] = minDistIndex;
 233             if(priority[nIndex] < 0.0f)
 234             {
 235               const auto currentPos = Dali::Vector3(Face(nIndex)->center);
 236               Vector3    diff       = currentPos - targetPos;
 237               priority[nIndex]      = std::max(0.0f, -direction.Dot(diff));
 238             }
 239           }
 240           else
 241           {
 242             prevForward[nIndex] = minDistIndex;
 243             if(priority[nIndex] < 0.0f)
 244             {
 245               const auto currentPos = Dali::Vector3(Face(nIndex)->center);
 246               Vector3    diff       = currentPos - sourcePos;
 247               priority[nIndex]      = std::max(0.0f, direction.Dot(diff));
 248             }
 249           }
 250
 251           if(!queued[nIndex])
 252           {
 253             queued[nIndex] = true;
 254             if(!nodeQueue.empty() && DistancePanaltyCalculate(nIndex) < DistancePanaltyCalculate(nodeQueue.front().first))
 255             {
 256               nodeQueue.push_front(std::make_pair(nIndex, isBackward));
 257             }
 258             else
 259             {
 260               nodeQueue.push_back(std::make_pair(nIndex, isBackward));
 261             }
 262           }
 263         }
 264       }
 265     }
 266   }
 267
 268   // Build path of face index
 269   std::list<FaceIndex> q;
 270   {
 271     FaceIndex u = forwardEndIndex;
 272     while(u != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 273     {
 274       q.push_front(u);
 275       u = prevForward[u];
 276     }
 277   }
 278   {
 279     FaceIndex u = backwardStartIndex;
 280     while(u != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 281     {
 282       q.push_back(u);
 283       u = prevBackward[u];
 284     }
 285   }
 286
 287   WayPointList waypoints;
 288   waypoints.resize(q.size());
 289
 290   auto index = 0u;
 291   auto prevN = 0u;
 292   for(auto n : q)
 293   {
 294     auto& wp     = static_cast<WayPointData&>(waypoints[index]);
 295     wp.face      = mNavigationMesh->GetFace(n);
 296     wp.nodeIndex = n;
 297
 298     wp.edge = nullptr;
 299     // set the common edge with previous node
 300     if(index > 0)
 301     {
 302       const auto& node = mNodes[prevN];
 303       for(auto i = 0u; i < 3; ++i)
 304       {
 305         if(node.faces[i] == wp.nodeIndex)
 306         {
 307           wp.edge = mNavigationMesh->GetEdge(node.edges[i]);
 308           break;
 309         }
 310       }
 311     }
 312
 313     prevN = n;
 314     index++;
 315   }
 316
 317   // Reset informations what we used.
 318   // Forward indexes
 319   for(const auto& i : usedPolyIndexs[0])
 320   {
 321     dist[i]         = std::numeric_limits<float>::infinity();
 322     priority[i]     = -1.0f;                                         // Initialize by negative value, that we didn't calculate yet.
 323     prevForward[i]  = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 324     prevBackward[i] = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 325     queued[i]       = false;
 326   }
 327   // Backward indexes
 328   for(const auto& i : usedPolyIndexs[1])
 329   {
 330     dist[i]         = std::numeric_limits<float>::infinity();
 331     priority[i]     = -1.0f;                                         // Initialize by negative value, that we didn't calculate yet.
 332     prevForward[i]  = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 333     prevBackward[i] = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 334     queued[i]       = false;
 335   }
 336
 337   return OptimizeWaypoints(waypoints);
 338 }
 339
 340 void PathFinderAlgorithmSPFADoubleWay::PrepareData()
 341 {
 342   // Build the list structure connecting the nodes
 343   auto faceCount = mNavigationMesh->GetFaceCount();
 344
 345   mNodes.resize(faceCount);
 346   dist.resize(faceCount);
 347   priority.resize(faceCount);
 348   prevForward.resize(faceCount);
 349   prevBackward.resize(faceCount);
 350   componentIds.resize(faceCount);
 351   queued.resize(faceCount);
 352
 353   // Temperal container for components level. It will be used for Union-Find algorithm.
 354   std::vector<FaceNodeIndex> componentLevels(faceCount);
 355
 356   // Initialize path informations.
 357   for(FaceNodeIndex i = 0u; i < faceCount; ++i)
 358   {
 359     dist[i]         = std::numeric_limits<float>::infinity();
 360     priority[i]     = -1.0f;                                         // Initialize by negative value, that we didn't calculate yet.
 361     prevForward[i]  = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 362     prevBackward[i] = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 363     queued[i]       = false;
 364
 365     componentIds[i]    = i; // Components id should be initialized by itself.
 366     componentLevels[i] = 0u;
 367   }
 368
 369   // for each face build the list
 370   // TODO : Currently, we are assume that FaceNodeIndex is matched with FaceIndex 1:1. This might be changed in future.
 371   for(FaceNodeIndex i = 0u; i < faceCount; ++i)
 372   {
 373     auto&       node = mNodes[i];
 374     const auto* face = mNavigationMesh->GetFace(i);
 375     auto        c0   = Dali::Vector3(face->center);
 376
 377     // for each edge add neighbouring face and compute distance to set the weight of node
 378     for(auto edgeIndex = 0u; edgeIndex < 3; ++edgeIndex)
 379     {
 380       const auto* edge = mNavigationMesh->GetEdge(face->edge[edgeIndex]);
 381       auto        p1   = edge->face[0];
 382       auto        p2   = edge->face[1];
 383
 384       // One of faces is current face so ignore it
 385       auto p                = ((p1 != i) ? p1 : p2);
 386       node.faces[edgeIndex] = p;
 387       if(p != ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 388       {
 389         node.edges[edgeIndex]  = face->edge[edgeIndex];
 390         auto c1                = Dali::Vector3(mNavigationMesh->GetFace(p)->center);
 391         node.weight[edgeIndex] = (c1 - c0).Length();
 392
 393         // Connect two components
 394         ComponentsCombine(componentIds, componentLevels, i, p);
 395       }
 396     }
 397   }
 398 }
 399
 400 [[maybe_unused]] static float ccw(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C)
 401 {
 402   return (C.y - A.y) * (B.x - A.x) > (B.y - A.y) * (C.x - A.x);
 403 }
 404
 405 [[maybe_unused]] static bool intersect(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C, const Dali::Vector2& D)
 406 {
 407   return ccw(A, C, D) != ccw(B, C, D) && ccw(A, B, C) != ccw(A, B, D);
 408 }
 409
 410 Scene3D::Algorithm::WayPointList PathFinderAlgorithmSPFADoubleWay::OptimizeWaypoints(WayPointList& waypoints) const
 411 {
 412   WayPointList optimizedWaypoints;
 413   optimizedWaypoints.emplace_back(waypoints[0]);
 414   optimizedWaypoints.reserve(waypoints.size());
 415
 416   auto startIndex = 1u;
 417
 418   bool finished = false;
 419   for(auto j = 0; !finished; ++j)
 420   {
 421     auto&       startWaypoint     = optimizedWaypoints.back();
 422     const auto& startWaypointData = static_cast<const WayPointData&>(startWaypoint);
 423
 424     // add new-last waypoint which will be overriden as long as intersection takes place
 425     optimizedWaypoints.emplace_back();
 426     for(auto wpIndex = startIndex; wpIndex < waypoints.size(); ++wpIndex)
 427     {
 428       if(wpIndex == waypoints.size() - 1)
 429       {
 430         optimizedWaypoints.back() = waypoints.back();
 431         finished                  = true;
 432         continue;
 433       }
 434       // Points between centres of faces
 435
 436       const auto& wpData = static_cast<const WayPointData&>(waypoints[wpIndex]);
 437
 438       auto Pa0 = Dali::Vector2(startWaypointData.face->center[0], startWaypointData.face->center[1]);
 439       auto Pa1 = Dali::Vector2(wpData.face->center[0], wpData.face->center[1]);
 440
 441       bool doesIntersect = true;
 442       for(auto i = startIndex; i < wpIndex; ++i)
 443       {
 444         const auto& wp = static_cast<WayPointData&>(waypoints[i]);
 445         // Skip starting waypoint
 446         if(wp.face == startWaypointData.face)
 447         {
 448           continue;
 449         }
 450         auto Pb0  = mNavigationMesh->GetVertex(wp.edge->vertex[0]);
 451         auto Pb1  = mNavigationMesh->GetVertex(wp.edge->vertex[1]);
 452         auto vPb0 = Dali::Vector2(Pb0->x, Pb0->y);
 453         auto vPb1 = Dali::Vector2(Pb1->x, Pb1->y);
 454
 455         doesIntersect = intersect(Pa0, Pa1, vPb0, vPb1);
 456         if(!doesIntersect)
 457         {
 458           break;
 459         }
 460       }
 461
 462       if(!doesIntersect)
 463       {
 464         optimizedWaypoints.back() = waypoints[wpIndex - 1];
 465         startIndex                = wpIndex - 1;
 466         break;
 467       }
 468     }
 469   }
 470
 471   for(auto& wp : optimizedWaypoints)
 472   {
 473     auto& wpData   = static_cast<WayPointData&>(wp);
 474     wpData.point3d = mNavigationMesh->PointLocalToScene(Dali::Vector3(wpData.face->center));
 475     wpData.point2d = Vector2::ZERO;
 476   }
 477
 478   return optimizedWaypoints;
 479 }
 480 } // namespace Dali::Scene3D::Internal::Algorithm