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       if(!waypoints.empty())
 122       {
 123         // replace first and last waypoint
 124         auto& wpFrom = static_cast<WayPointData&>(waypoints[0]);
 125         auto& wpTo   = static_cast<WayPointData&>(waypoints.back());
 126
 127         Vector2 fromCenter(wpFrom.point3d.x, wpFrom.point3d.y);
 128         wpFrom.point3d = outPosFrom;
 129         wpFrom.point2d = fromCenter - Vector2(outPosFrom.x, outPosFrom.y);
 130
 131         Vector2 toCenter(wpTo.point3d.x, wpTo.point3d.y);
 132         wpTo.point3d = outPosTo;
 133         wpTo.point2d = toCenter - Vector2(outPosTo.x, outPosTo.y);
 134       }
 135     }
 136   }
 137
 138   // Returns waypoints with non-zero size of empty vector in case of failure (no path to be found)
 139   return waypoints;
 140 }
 141
 142 Scene3D::Algorithm::WayPointList PathFinderAlgorithmSPFADoubleWay::FindPath(FaceIndex sourcePolyIndex, FaceIndex targetPolyIndex)
 143 {
 144   // Fast return if source and target index is same.
 145   if(sourcePolyIndex == targetPolyIndex)
 146   {
 147     WayPointList waypoints;
 148     waypoints.resize(1);
 149
 150     auto& wp     = static_cast<WayPointData&>(waypoints[0]);
 151     wp.face      = mNavigationMesh->GetFace(sourcePolyIndex);
 152     wp.nodeIndex = sourcePolyIndex;
 153     wp.edge      = nullptr;
 154
 155     return OptimizeWaypoints(waypoints);
 156   }
 157
 158   // Fast return if source and target index is not in same components.
 159   // That mean, there is no path. Return empty list.
 160   if(GetComponentId(componentIds, sourcePolyIndex) != GetComponentId(componentIds, targetPolyIndex))
 161   {
 162     return WayPointList();
 163   }
 164
 165   // pair<navimesh FaceIndex, is backward direction>
 166   using queueItem = std::pair<FaceIndex, uint8_t>;
 167
 168   std::list<queueItem> nodeQueue;
 169
 170   std::unordered_set<FaceIndex> usedPolyIndexs[2];
 171
 172   // Set distance of source and target
 173   dist[sourcePolyIndex]     = 0.0f;
 174   dist[targetPolyIndex]     = 0.0f;
 175   priority[sourcePolyIndex] = 0.0f;
 176   priority[targetPolyIndex] = 0.0f;
 177   queued[sourcePolyIndex]   = true;
 178   queued[targetPolyIndex]   = true;
 179   nodeQueue.push_back(std::make_pair(sourcePolyIndex, 0));
 180   nodeQueue.push_back(std::make_pair(targetPolyIndex, 1));
 181   usedPolyIndexs[0].insert(sourcePolyIndex);
 182   usedPolyIndexs[1].insert(targetPolyIndex);
 183
 184   bool      foundPath          = false;
 185   FaceIndex forwardEndIndex    = Scene3D::Algorithm::NavigationMesh::NULL_FACE;
 186   FaceIndex backwardStartIndex = Scene3D::Algorithm::NavigationMesh::NULL_FACE;
 187
 188   const auto sourcePos = Dali::Vector3(Face(sourcePolyIndex)->center);
 189   const auto targetPos = Dali::Vector3(Face(targetPolyIndex)->center);
 190   Vector3    direction = targetPos - sourcePos;
 191   direction.Normalize();
 192
 193   // Note : we always success to found path since source and target is in same components.
 194   while(!foundPath)
 195   {
 196     // find minimum distance
 197     auto minDistIndex = nodeQueue.front().first;
 198     auto isBackward   = nodeQueue.front().second;
 199     nodeQueue.pop_front();
 200     queued[minDistIndex] = false;
 201
 202     // check the neighbours
 203     for(auto i = 0u; i < 3 && !foundPath; ++i)
 204     {
 205       auto nIndex = mNodes[minDistIndex].faces[i];
 206       if(nIndex != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 207       {
 208         if(usedPolyIndexs[!isBackward].count(nIndex))
 209         {
 210           // We found path!
 211           foundPath = true;
 212           if(isBackward)
 213           {
 214             forwardEndIndex    = nIndex;
 215             backwardStartIndex = minDistIndex;
 216           }
 217           else
 218           {
 219             forwardEndIndex    = minDistIndex;
 220             backwardStartIndex = nIndex;
 221           }
 222           break;
 223         }
 224
 225         usedPolyIndexs[isBackward].insert(nIndex);
 226
 227         auto alt = dist[minDistIndex] + mNodes[minDistIndex].weight[i];
 228         if(alt < dist[nIndex])
 229         {
 230           dist[nIndex] = alt;
 231
 232           if(isBackward)
 233           {
 234             prevBackward[nIndex] = minDistIndex;
 235             if(priority[nIndex] < 0.0f)
 236             {
 237               const auto currentPos = Dali::Vector3(Face(nIndex)->center);
 238               Vector3    diff       = currentPos - targetPos;
 239               priority[nIndex]      = std::max(0.0f, -direction.Dot(diff));
 240             }
 241           }
 242           else
 243           {
 244             prevForward[nIndex] = minDistIndex;
 245             if(priority[nIndex] < 0.0f)
 246             {
 247               const auto currentPos = Dali::Vector3(Face(nIndex)->center);
 248               Vector3    diff       = currentPos - sourcePos;
 249               priority[nIndex]      = std::max(0.0f, direction.Dot(diff));
 250             }
 251           }
 252
 253           if(!queued[nIndex])
 254           {
 255             queued[nIndex] = true;
 256             if(!nodeQueue.empty() && DistancePanaltyCalculate(nIndex) < DistancePanaltyCalculate(nodeQueue.front().first))
 257             {
 258               nodeQueue.push_front(std::make_pair(nIndex, isBackward));
 259             }
 260             else
 261             {
 262               nodeQueue.push_back(std::make_pair(nIndex, isBackward));
 263             }
 264           }
 265         }
 266       }
 267     }
 268   }
 269
 270   // Build path of face index
 271   std::list<FaceIndex> q;
 272   {
 273     FaceIndex u = forwardEndIndex;
 274     while(u != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 275     {
 276       q.push_front(u);
 277       u = prevForward[u];
 278     }
 279   }
 280   {
 281     FaceIndex u = backwardStartIndex;
 282     while(u != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 283     {
 284       q.push_back(u);
 285       u = prevBackward[u];
 286     }
 287   }
 288
 289   WayPointList waypoints;
 290   waypoints.resize(q.size());
 291
 292   auto index = 0u;
 293   auto prevN = 0u;
 294   for(auto n : q)
 295   {
 296     auto& wp     = static_cast<WayPointData&>(waypoints[index]);
 297     wp.face      = mNavigationMesh->GetFace(n);
 298     wp.nodeIndex = n;
 299
 300     wp.edge = nullptr;
 301     // set the common edge with previous node
 302     if(index > 0)
 303     {
 304       const auto& node = mNodes[prevN];
 305       for(auto i = 0u; i < 3; ++i)
 306       {
 307         if(node.faces[i] == wp.nodeIndex)
 308         {
 309           wp.edge = mNavigationMesh->GetEdge(node.edges[i]);
 310           break;
 311         }
 312       }
 313     }
 314
 315     prevN = n;
 316     index++;
 317   }
 318
 319   // Reset informations what we used.
 320   // Forward indexes
 321   for(const auto& i : usedPolyIndexs[0])
 322   {
 323     dist[i]         = std::numeric_limits<float>::infinity();
 324     priority[i]     = -1.0f;                                         // Initialize by negative value, that we didn't calculate yet.
 325     prevForward[i]  = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 326     prevBackward[i] = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 327     queued[i]       = false;
 328   }
 329   // Backward indexes
 330   for(const auto& i : usedPolyIndexs[1])
 331   {
 332     dist[i]         = std::numeric_limits<float>::infinity();
 333     priority[i]     = -1.0f;                                         // Initialize by negative value, that we didn't calculate yet.
 334     prevForward[i]  = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 335     prevBackward[i] = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 336     queued[i]       = false;
 337   }
 338
 339   return OptimizeWaypoints(waypoints);
 340 }
 341
 342 void PathFinderAlgorithmSPFADoubleWay::PrepareData()
 343 {
 344   // Build the list structure connecting the nodes
 345   auto faceCount = mNavigationMesh->GetFaceCount();
 346
 347   mNodes.resize(faceCount);
 348   dist.resize(faceCount);
 349   priority.resize(faceCount);
 350   prevForward.resize(faceCount);
 351   prevBackward.resize(faceCount);
 352   componentIds.resize(faceCount);
 353   queued.resize(faceCount);
 354
 355   // Temperal container for components level. It will be used for Union-Find algorithm.
 356   std::vector<FaceNodeIndex> componentLevels(faceCount);
 357
 358   // Initialize path informations.
 359   for(FaceNodeIndex i = 0u; i < faceCount; ++i)
 360   {
 361     dist[i]         = std::numeric_limits<float>::infinity();
 362     priority[i]     = -1.0f;                                         // Initialize by negative value, that we didn't calculate yet.
 363     prevForward[i]  = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 364     prevBackward[i] = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
 365     queued[i]       = false;
 366
 367     componentIds[i]    = i; // Components id should be initialized by itself.
 368     componentLevels[i] = 0u;
 369   }
 370
 371   // for each face build the list
 372   // TODO : Currently, we are assume that FaceNodeIndex is matched with FaceIndex 1:1. This might be changed in future.
 373   for(FaceNodeIndex i = 0u; i < faceCount; ++i)
 374   {
 375     auto&       node = mNodes[i];
 376     const auto* face = mNavigationMesh->GetFace(i);
 377     auto        c0   = Dali::Vector3(face->center);
 378
 379     // for each edge add neighbouring face and compute distance to set the weight of node
 380     for(auto edgeIndex = 0u; edgeIndex < 3; ++edgeIndex)
 381     {
 382       const auto* edge = mNavigationMesh->GetEdge(face->edge[edgeIndex]);
 383       auto        p1   = edge->face[0];
 384       auto        p2   = edge->face[1];
 385
 386       // One of faces is current face so ignore it
 387       auto p                = ((p1 != i) ? p1 : p2);
 388       node.faces[edgeIndex] = p;
 389       if(p != ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 390       {
 391         node.edges[edgeIndex]  = face->edge[edgeIndex];
 392         auto c1                = Dali::Vector3(mNavigationMesh->GetFace(p)->center);
 393         node.weight[edgeIndex] = (c1 - c0).Length();
 394
 395         // Connect two components
 396         ComponentsCombine(componentIds, componentLevels, i, p);
 397       }
 398     }
 399   }
 400 }
 401
 402 [[maybe_unused]] static float ccw(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C)
 403 {
 404   return (C.y - A.y) * (B.x - A.x) > (B.y - A.y) * (C.x - A.x);
 405 }
 406
 407 [[maybe_unused]] static bool intersect(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C, const Dali::Vector2& D)
 408 {
 409   return ccw(A, C, D) != ccw(B, C, D) && ccw(A, B, C) != ccw(A, B, D);
 410 }
 411
 412 Scene3D::Algorithm::WayPointList PathFinderAlgorithmSPFADoubleWay::OptimizeWaypoints(WayPointList& waypoints) const
 413 {
 414   WayPointList optimizedWaypoints;
 415   optimizedWaypoints.emplace_back(waypoints[0]);
 416   optimizedWaypoints.reserve(waypoints.size());
 417
 418   auto startIndex = 1u;
 419
 420   bool finished = false;
 421   for(auto j = 0; !finished; ++j)
 422   {
 423     auto&       startWaypoint     = optimizedWaypoints.back();
 424     const auto& startWaypointData = static_cast<const WayPointData&>(startWaypoint);
 425
 426     // add new-last waypoint which will be overriden as long as intersection takes place
 427     optimizedWaypoints.emplace_back();
 428     for(auto wpIndex = startIndex; wpIndex < waypoints.size(); ++wpIndex)
 429     {
 430       if(wpIndex == waypoints.size() - 1)
 431       {
 432         optimizedWaypoints.back() = waypoints.back();
 433         finished                  = true;
 434         continue;
 435       }
 436       // Points between centres of faces
 437
 438       const auto& wpData = static_cast<const WayPointData&>(waypoints[wpIndex]);
 439
 440       auto Pa0 = Dali::Vector2(startWaypointData.face->center[0], startWaypointData.face->center[1]);
 441       auto Pa1 = Dali::Vector2(wpData.face->center[0], wpData.face->center[1]);
 442
 443       bool doesIntersect = true;
 444       for(auto i = startIndex; i < wpIndex; ++i)
 445       {
 446         const auto& wp = static_cast<WayPointData&>(waypoints[i]);
 447         // Skip starting waypoint
 448         if(wp.face == startWaypointData.face)
 449         {
 450           continue;
 451         }
 452         auto Pb0  = mNavigationMesh->GetVertex(wp.edge->vertex[0]);
 453         auto Pb1  = mNavigationMesh->GetVertex(wp.edge->vertex[1]);
 454         auto vPb0 = Dali::Vector2(Pb0->x, Pb0->y);
 455         auto vPb1 = Dali::Vector2(Pb1->x, Pb1->y);
 456
 457         doesIntersect = intersect(Pa0, Pa1, vPb0, vPb1);
 458         if(!doesIntersect)
 459         {
 460           break;
 461         }
 462       }
 463
 464       if(!doesIntersect)
 465       {
 466         optimizedWaypoints.back() = waypoints[wpIndex - 1];
 467         startIndex                = wpIndex - 1;
 468         break;
 469       }
 470     }
 471   }
 472
 473   for(auto& wp : optimizedWaypoints)
 474   {
 475     auto& wpData   = static_cast<WayPointData&>(wp);
 476     wpData.point3d = mNavigationMesh->PointLocalToScene(Dali::Vector3(wpData.face->center));
 477     wpData.point2d = Vector2::ZERO;
 478   }
 479
 480   return optimizedWaypoints;
 481 }
 482 } // namespace Dali::Scene3D::Internal::Algorithm