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