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