2 * Copyright (c) 2023 Samsung Electronics Co., Ltd.
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
8 * http://www.apache.org/licenses/LICENSE-2.0
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.
18 #include <dali-scene3d/internal/algorithm/path-finder-dijkstra.h>
21 #include <dali/public-api/common/vector-wrapper.h>
23 #include <algorithm> ///< for std::priority_queue
24 #include <queue> ///< for std::priority_queue
27 #include <dali-scene3d/internal/algorithm/path-finder-waypoint-data.h>
28 #include <dali-scene3d/public-api/algorithm/path-finder-waypoint.h>
30 using WayPointList = Dali::Scene3D::Algorithm::WayPointList;
34 struct DijkstraComparer
36 float distance{-1.0f};
37 Dali::Scene3D::Algorithm::FaceIndex index{Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE};
40 * @brief Custom operator for Dijkstra comparer.
42 * @param[in] rhs Target to be compared.
43 * @return True if this has less priority than rhs.
45 bool operator<(const DijkstraComparer& rhs) const
47 // Shorter distance have higher prioirty.
48 return distance > rhs.distance;
53 namespace Dali::Scene3D::Internal::Algorithm
55 PathFinderAlgorithmDijkstra::PathFinderAlgorithmDijkstra(Dali::Scene3D::Algorithm::NavigationMesh& navMesh)
56 : mNavigationMesh(&GetImplementation(navMesh))
61 PathFinderAlgorithmDijkstra::~PathFinderAlgorithmDijkstra() = default;
63 Scene3D::Algorithm::WayPointList PathFinderAlgorithmDijkstra::FindPath(const Dali::Vector3& positionFrom, const Dali::Vector3& positionTo)
65 Dali::Vector3 outPosFrom;
66 FaceIndex polyIndexFrom;
67 auto result = mNavigationMesh->FindFloor(positionFrom, outPosFrom, polyIndexFrom);
69 Scene3D::Algorithm::WayPointList waypoints;
73 Dali::Vector3 outPosTo;
74 FaceIndex polyIndexTo;
75 result = mNavigationMesh->FindFloor(positionTo, outPosTo, polyIndexTo);
80 waypoints = FindPath(polyIndexFrom, polyIndexTo);
82 if(!waypoints.empty())
84 // replace first and last waypoint
85 auto& wpFrom = static_cast<WayPointData&>(waypoints[0]);
86 auto& wpTo = static_cast<WayPointData&>(waypoints.back());
88 Vector2 fromCenter(wpFrom.point3d.x, wpFrom.point3d.y);
89 wpFrom.point3d = outPosFrom;
90 wpFrom.point2d = fromCenter - Vector2(outPosFrom.x, outPosFrom.y);
92 Vector2 toCenter(wpTo.point3d.x, wpTo.point3d.y);
93 wpTo.point3d = outPosTo;
94 wpTo.point2d = toCenter - Vector2(outPosTo.x, outPosTo.y);
99 // Returns waypoints with non-zero size of empty vector in case of failure (no path to be found)
103 Scene3D::Algorithm::WayPointList PathFinderAlgorithmDijkstra::FindPath(FaceIndex sourcePolyIndex, FaceIndex targetPolyIndex)
105 auto nodeCount = uint32_t(mNodes.size());
106 std::vector<float> dist;
107 std::vector<FaceIndex> prev;
108 std::vector<bool> faceVisited;
110 std::priority_queue<DijkstraComparer> priorityDistanceHeap;
112 dist.resize(mNodes.size());
113 prev.resize(mNodes.size());
115 faceVisited.resize(nodeCount);
117 for(auto i = 0u; i < nodeCount; ++i)
119 dist[i] = std::numeric_limits<float>::infinity();
120 prev[i] = Scene3D::Algorithm::NavigationMesh::NULL_FACE; // set prev to null polygon
121 faceVisited[i] = false;
124 // Set distance of source
125 dist[sourcePolyIndex] = 0.0f;
127 priorityDistanceHeap.push({dist[sourcePolyIndex], sourcePolyIndex});
129 while(!priorityDistanceHeap.empty())
131 // find minimum distance index
132 auto comparer = priorityDistanceHeap.top();
133 priorityDistanceHeap.pop();
135 auto minDistanceIndex = comparer.index;
137 // Old item. just ignore.
138 if(faceVisited[minDistanceIndex])
143 faceVisited[minDistanceIndex] = true;
145 // Fast break if we found solution.
146 if(minDistanceIndex == targetPolyIndex)
151 // check the neighbours
152 for(auto i = 0u; i < 3; ++i)
154 auto nIndex = mNodes[minDistanceIndex].faces[i];
155 if(nIndex != Scene3D::Algorithm::NavigationMesh::NULL_FACE && !faceVisited[nIndex])
157 auto alt = dist[minDistanceIndex] + mNodes[minDistanceIndex].weight[i];
158 if(alt < dist[nIndex])
161 prev[nIndex] = minDistanceIndex;
163 priorityDistanceHeap.push({dist[nIndex], nIndex});
169 // Failed to find minimum distance
170 if(!faceVisited[targetPolyIndex])
172 // Return empty WayPointList
176 // Compute distances for each node back to the source
177 auto u = targetPolyIndex;
178 std::list<FaceIndex> q;
179 if(prev[u] != Scene3D::Algorithm::NavigationMesh::NULL_FACE || u == sourcePolyIndex)
181 while(u != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
188 WayPointList waypoints;
189 waypoints.resize(q.size());
195 auto& wp = static_cast<WayPointData&>(waypoints[index]);
196 wp.face = mNavigationMesh->GetFace(n);
200 // set the common edge with previous node
203 const auto& node = mNodes[prevN];
204 for(auto i = 0u; i < 3; ++i)
206 if(node.faces[i] == wp.nodeIndex)
208 wp.edge = mNavigationMesh->GetEdge(node.edges[i]);
218 return OptimizeWaypoints(waypoints);
221 void PathFinderAlgorithmDijkstra::PrepareData()
223 // Build the list structure connecting the nodes
224 auto faceCount = mNavigationMesh->GetFaceCount();
226 mNodes.resize(faceCount);
228 // for each face build the list
229 // TODO : Currently, we are assume that FaceNodeIndex is matched with FaceIndex 1:1. This might be changed in future.
230 for(auto i = 0u; i < faceCount; ++i)
232 auto& node = mNodes[i];
233 const auto* face = mNavigationMesh->GetFace(i);
234 auto c0 = Dali::Vector3(face->center);
236 // for each edge add neighbouring face and compute distance to set the weight of node
237 for(auto edgeIndex = 0u; edgeIndex < 3; ++edgeIndex)
239 const auto* edge = mNavigationMesh->GetEdge(face->edge[edgeIndex]);
240 auto p1 = edge->face[0];
241 auto p2 = edge->face[1];
243 // One of faces is current face so ignore it
244 auto p = ((p1 != i) ? p1 : p2);
245 node.faces[edgeIndex] = p;
246 if(p != ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE)
248 node.edges[edgeIndex] = face->edge[edgeIndex];
249 auto c1 = Dali::Vector3(mNavigationMesh->GetFace(p)->center);
250 node.weight[edgeIndex] = (c1 - c0).Length();
256 [[maybe_unused]] static float ccw(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C)
258 return (C.y - A.y) * (B.x - A.x) > (B.y - A.y) * (C.x - A.x);
261 [[maybe_unused]] static bool intersect(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C, const Dali::Vector2& D)
263 return ccw(A, C, D) != ccw(B, C, D) && ccw(A, B, C) != ccw(A, B, D);
266 Scene3D::Algorithm::WayPointList PathFinderAlgorithmDijkstra::OptimizeWaypoints(WayPointList& waypoints) const
268 WayPointList optimizedWaypoints;
269 optimizedWaypoints.emplace_back(waypoints[0]);
270 optimizedWaypoints.reserve(waypoints.size());
272 auto startIndex = 1u;
274 bool finished = false;
275 for(auto j = 0; !finished; ++j)
277 auto& startWaypoint = optimizedWaypoints.back();
278 const auto& startWaypointData = static_cast<const WayPointData&>(startWaypoint);
280 // add new-last waypoint which will be overriden as long as intersection takes place
281 optimizedWaypoints.emplace_back();
282 for(auto wpIndex = startIndex; wpIndex < waypoints.size(); ++wpIndex)
284 if(wpIndex == waypoints.size() - 1)
286 optimizedWaypoints.back() = waypoints.back();
290 // Points between centres of faces
292 const auto& wpData = static_cast<const WayPointData&>(waypoints[wpIndex]);
294 auto Pa0 = Dali::Vector2(startWaypointData.face->center[0], startWaypointData.face->center[1]);
295 auto Pa1 = Dali::Vector2(wpData.face->center[0], wpData.face->center[1]);
297 bool doesIntersect = true;
298 for(auto i = startIndex; i < wpIndex; ++i)
300 const auto& wp = static_cast<WayPointData&>(waypoints[i]);
301 // Skip starting waypoint
302 if(wp.face == startWaypointData.face)
306 auto Pb0 = mNavigationMesh->GetVertex(wp.edge->vertex[0]);
307 auto Pb1 = mNavigationMesh->GetVertex(wp.edge->vertex[1]);
308 auto vPb0 = Dali::Vector2(Pb0->x, Pb0->y);
309 auto vPb1 = Dali::Vector2(Pb1->x, Pb1->y);
311 doesIntersect = intersect(Pa0, Pa1, vPb0, vPb1);
320 optimizedWaypoints.back() = waypoints[wpIndex - 1];
321 startIndex = wpIndex - 1;
327 for(auto& wp : optimizedWaypoints)
329 auto& wpData = static_cast<WayPointData&>(wp);
330 wpData.point3d = mNavigationMesh->PointLocalToScene(Dali::Vector3(wpData.face->center));
331 wpData.point2d = Vector2::ZERO;
334 return optimizedWaypoints;
336 } // namespace Dali::Scene3D::Internal::Algorithm