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