dali-scene3d/internal/algorithm/path-finder-spfa.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.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 <vector>
  25
  26 using WayPointList = Dali::Scene3D::Algorithm::WayPointList;
  27
  28 namespace Dali::Scene3D::Internal::Algorithm
  29 {
  30 PathFinderAlgorithmSPFA::PathFinderAlgorithmSPFA(Dali::Scene3D::Algorithm::NavigationMesh& navMesh)
  31 : mNavigationMesh(&GetImplementation(navMesh))
  32 {
  33   PrepareData();
  34 }
  35
  36 PathFinderAlgorithmSPFA::~PathFinderAlgorithmSPFA() = default;
  37
  38 Scene3D::Algorithm::WayPointList PathFinderAlgorithmSPFA::FindPath(const Dali::Vector3& positionFrom, const Dali::Vector3& positionTo)
  39 {
  40   Dali::Vector3 outPosFrom;
  41   uint32_t      polyIndexFrom;
  42   auto          result = mNavigationMesh->FindFloor(positionFrom, outPosFrom, polyIndexFrom);
  43
  44   Scene3D::Algorithm::WayPointList waypoints;
  45
  46   if(result)
  47   {
  48     Dali::Vector3 outPosTo;
  49     uint32_t      polyIndexTo;
  50     result = mNavigationMesh->FindFloor(positionTo, outPosTo, polyIndexTo);
  51
  52     if(result)
  53     {
  54       // Get waypoints
  55       waypoints = FindPath(polyIndexFrom, polyIndexTo);
  56
  57       // replace first and last waypoint
  58       auto& wpFrom = static_cast<WayPointData&>(waypoints[0]);
  59       auto& wpTo   = static_cast<WayPointData&>(waypoints.back());
  60
  61       Vector2 fromCenter(wpFrom.point3d.x, wpFrom.point3d.y);
  62       wpFrom.point3d = outPosFrom;
  63       wpFrom.point2d = fromCenter - Vector2(outPosFrom.x, outPosFrom.y);
  64
  65       Vector2 toCenter(wpTo.point3d.x, wpTo.point3d.y);
  66       wpTo.point3d = outPosTo;
  67       wpTo.point2d = toCenter - Vector2(outPosTo.x, outPosTo.y);
  68       wpTo.point3d = outPosTo;
  69     }
  70   }
  71
  72   // Returns waypoints with non-zero size of empty vector in case of failure (no path to be found)
  73   return waypoints;
  74 }
  75
  76 Scene3D::Algorithm::WayPointList PathFinderAlgorithmSPFA::FindPath(uint32_t sourcePolyIndex, uint32_t targetPolyIndex)
  77 {
  78   auto                  nodeCount = uint32_t(mNodes.size());
  79   std::vector<float>    dist;
  80   std::vector<uint32_t> prev;
  81   std::vector<bool>     queued;
  82
  83   dist.resize(mNodes.size());
  84   prev.resize(mNodes.size());
  85   queued.resize(mNodes.size());
  86
  87   std::list<uint32_t> nodeQueue;
  88
  89   [[maybe_unused]] auto sourcePos = Dali::Vector3(Face(sourcePolyIndex)->center);
  90
  91   for(auto i = 0u; i < nodeCount; ++i)
  92   {
  93     dist[i]   = std::numeric_limits<float>::infinity();
  94     prev[i]   = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
  95     queued[i] = false;
  96   }
  97
  98   // Set distance of source
  99   dist[sourcePolyIndex]   = 0.0f;
 100   queued[sourcePolyIndex] = true;
 101   nodeQueue.push_back(sourcePolyIndex);
 102
 103   while(!nodeQueue.empty())
 104   {
 105     // find minimum distance
 106     auto minDistIndex = nodeQueue.front();
 107     nodeQueue.pop_front();
 108     queued[minDistIndex] = false;
 109
 110     // Skip operator when minDistIndex is target
 111     if(minDistIndex == targetPolyIndex)
 112     {
 113       continue;
 114     }
 115
 116     // check the neighbours
 117     for(auto i = 0u; i < 3; ++i)
 118     {
 119       auto nIndex = mNodes[minDistIndex].faces[i];
 120       if(nIndex != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 121       {
 122         auto alt = dist[minDistIndex] + mNodes[minDistIndex].weight[i];
 123         if(alt < dist[nIndex])
 124         {
 125           dist[nIndex] = alt;
 126           prev[nIndex] = minDistIndex;
 127
 128           if(!queued[nIndex])
 129           {
 130             queued[nIndex] = true;
 131             if(!nodeQueue.empty() && dist[nIndex] < dist[nodeQueue.front()])
 132             {
 133               nodeQueue.push_front(nIndex);
 134             }
 135             else
 136             {
 137               nodeQueue.push_back(nIndex);
 138             }
 139           }
 140         }
 141       }
 142     }
 143   }
 144
 145   // Compute distances for each node back to the source
 146   auto                u = targetPolyIndex;
 147   std::list<uint32_t> q;
 148   if(prev[u] != Scene3D::Algorithm::NavigationMesh::NULL_FACE || u == sourcePolyIndex)
 149   {
 150     while(u != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 151     {
 152       q.push_front(u);
 153       u = prev[u];
 154     }
 155   }
 156
 157   WayPointList waypoints;
 158   waypoints.resize(q.size());
 159
 160   if(q.size() == 0)
 161   {
 162     // Fail to found path. Return empty list.
 163     return waypoints;
 164   }
 165
 166   auto index = 0u;
 167   auto prevN = 0u;
 168   for(auto n : q)
 169   {
 170     auto& wp     = static_cast<WayPointData&>(waypoints[index]);
 171     wp.face      = mNavigationMesh->GetFace(n);
 172     wp.nodeIndex = n;
 173
 174     wp.edge = nullptr;
 175     // set the common edge with previous node
 176     if(index > 0)
 177     {
 178       const auto& node = mNodes[prevN];
 179       for(auto i = 0u; i < 3; ++i)
 180       {
 181         if(node.faces[i] == wp.nodeIndex)
 182         {
 183           wp.edge = mNavigationMesh->GetEdge(node.edges[i]);
 184           break;
 185         }
 186       }
 187     }
 188
 189     prevN = n;
 190     index++;
 191   }
 192
 193   return OptimizeWaypoints(waypoints);
 194 }
 195
 196 void PathFinderAlgorithmSPFA::PrepareData()
 197 {
 198   // Build the list structure connecting the nodes
 199   auto faceCount = mNavigationMesh->GetFaceCount();
 200
 201   mNodes.resize(faceCount);
 202
 203   // for each face build the list
 204   for(auto i = 0u; i < faceCount; ++i)
 205   {
 206     auto&       node = mNodes[i];
 207     const auto* face = mNavigationMesh->GetFace(i);
 208     auto        c0   = Dali::Vector3(face->center);
 209
 210     // for each edge add neighbouring face and compute distance to set the weight of node
 211     for(auto edgeIndex = 0u; edgeIndex < 3; ++edgeIndex)
 212     {
 213       const auto* edge = mNavigationMesh->GetEdge(face->edge[edgeIndex]);
 214       auto        p1   = edge->face[0];
 215       auto        p2   = edge->face[1];
 216
 217       // One of faces is current face so ignore it
 218       auto p                = ((p1 != i) ? p1 : p2);
 219       node.faces[edgeIndex] = p;
 220       if(p != ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 221       {
 222         node.edges[edgeIndex]  = face->edge[edgeIndex];
 223         auto c1                = Dali::Vector3(mNavigationMesh->GetFace(p)->center);
 224         node.weight[edgeIndex] = (c1 - c0).Length();
 225       }
 226     }
 227   }
 228 }
 229
 230 [[maybe_unused]] static float ccw(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C)
 231 {
 232   return (C.y - A.y) * (B.x - A.x) > (B.y - A.y) * (C.x - A.x);
 233 }
 234
 235 [[maybe_unused]] static bool intersect(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C, const Dali::Vector2& D)
 236 {
 237   return ccw(A, C, D) != ccw(B, C, D) && ccw(A, B, C) != ccw(A, B, D);
 238 }
 239
 240 Scene3D::Algorithm::WayPointList PathFinderAlgorithmSPFA::OptimizeWaypoints(WayPointList& waypoints) const
 241 {
 242   WayPointList optimizedWaypoints;
 243   optimizedWaypoints.emplace_back(waypoints[0]);
 244   optimizedWaypoints.reserve(waypoints.size());
 245
 246   auto startIndex = 1u;
 247
 248   bool finished = false;
 249   for(auto j = 0; !finished; ++j)
 250   {
 251     auto&       startWaypoint     = optimizedWaypoints.back();
 252     const auto& startWaypointData = static_cast<const WayPointData&>(startWaypoint);
 253
 254     // add new-last waypoint which will be overriden as long as intersection takes place
 255     optimizedWaypoints.emplace_back();
 256     for(auto wpIndex = startIndex; wpIndex < waypoints.size(); ++wpIndex)
 257     {
 258       if(wpIndex == waypoints.size() - 1)
 259       {
 260         optimizedWaypoints.back() = waypoints.back();
 261         finished                  = true;
 262         continue;
 263       }
 264       // Points between centres of faces
 265
 266       const auto& wpData = static_cast<const WayPointData&>(waypoints[wpIndex]);
 267
 268       auto Pa0 = Dali::Vector2(startWaypointData.face->center[0], startWaypointData.face->center[1]);
 269       auto Pa1 = Dali::Vector2(wpData.face->center[0], wpData.face->center[1]);
 270
 271       bool doesIntersect = true;
 272       for(auto i = startIndex; i < wpIndex; ++i)
 273       {
 274         const auto& wp = static_cast<WayPointData&>(waypoints[i]);
 275         // Skip starting waypoint
 276         if(wp.face == startWaypointData.face)
 277         {
 278           continue;
 279         }
 280         auto Pb0  = mNavigationMesh->GetVertex(wp.edge->vertex[0]);
 281         auto Pb1  = mNavigationMesh->GetVertex(wp.edge->vertex[1]);
 282         auto vPb0 = Dali::Vector2(Pb0->x, Pb0->y);
 283         auto vPb1 = Dali::Vector2(Pb1->x, Pb1->y);
 284
 285         doesIntersect = intersect(Pa0, Pa1, vPb0, vPb1);
 286         if(!doesIntersect)
 287         {
 288           break;
 289         }
 290       }
 291
 292       if(!doesIntersect)
 293       {
 294         optimizedWaypoints.back() = waypoints[wpIndex - 1];
 295         startIndex                = wpIndex - 1;
 296         break;
 297       }
 298     }
 299   }
 300
 301   for(auto& wp : optimizedWaypoints)
 302   {
 303     auto& wpData   = static_cast<WayPointData&>(wp);
 304     wpData.point3d = mNavigationMesh->PointLocalToScene(Dali::Vector3(wpData.face->center));
 305     wpData.point2d = Vector2::ZERO;
 306   }
 307
 308   return optimizedWaypoints;
 309 }
 310 } // namespace Dali::Scene3D::Internal::Algorithm