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