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-djikstra.h>
21 #include <dali/public-api/common/vector-wrapper.h>
25 #include <dali-scene3d/internal/algorithm/path-finder-waypoint-data.h>
26 #include <dali-scene3d/public-api/algorithm/path-finder-waypoint.h>
28 using WayPointList = Dali::Scene3D::Algorithm::WayPointList;
30 namespace Dali::Scene3D::Internal::Algorithm
32 PathFinderAlgorithmDjikstra::PathFinderAlgorithmDjikstra(Dali::Scene3D::Algorithm::NavigationMesh& navMesh)
33 : mNavigationMesh(&GetImplementation(navMesh))
38 PathFinderAlgorithmDjikstra::~PathFinderAlgorithmDjikstra() = default;
40 Scene3D::Algorithm::WayPointList PathFinderAlgorithmDjikstra::FindPath(const Dali::Vector3& positionFrom, const Dali::Vector3& positionTo)
42 Dali::Vector3 outPosFrom;
43 uint32_t polyIndexFrom;
44 auto result = mNavigationMesh->FindFloor(positionFrom, outPosFrom, polyIndexFrom);
46 Scene3D::Algorithm::WayPointList waypoints;
50 Dali::Vector3 outPosTo;
52 result = mNavigationMesh->FindFloor(positionTo, outPosTo, polyIndexTo);
57 waypoints = FindPath(polyIndexFrom, polyIndexTo);
59 // replace first and last waypoint
60 auto& wpFrom = static_cast<WayPointData&>(waypoints[0]);
61 auto& wpTo = static_cast<WayPointData&>(waypoints.back());
63 Vector2 fromCenter(wpFrom.point3d.x, wpFrom.point3d.y);
64 wpFrom.point3d = outPosFrom;
65 wpFrom.point2d = fromCenter - Vector2(outPosFrom.x, outPosFrom.y);
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;
74 // Returns waypoints with non-zero size of empty vector in case of failure (no path to be found)
78 Scene3D::Algorithm::WayPointList PathFinderAlgorithmDjikstra::FindPath(uint32_t sourcePolyIndex, uint32_t targetPolyIndex)
80 auto nodeCount = uint32_t(mNodes.size());
81 std::vector<float> dist;
82 std::vector<uint32_t> prev;
84 dist.resize(mNodes.size());
85 prev.resize(mNodes.size());
87 std::vector<FaceNode*> nodeQueue;
88 nodeQueue.reserve(nodeCount);
90 [[maybe_unused]] auto sourcePos = Dali::Vector3(Face(sourcePolyIndex)->center);
92 for(auto i = 0u; i < nodeCount; ++i)
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]);
98 auto removeCount = 0u;
100 // Set distance of source
101 dist[sourcePolyIndex] = 0.0f;
103 // TO OPTIMIZE WITH PRIORITY QUEUE
104 auto FindMinDistance = [&nodeQueue](decltype(dist)& dist) {
105 float w = std::numeric_limits<float>::max();
107 for(auto i = 0u; i < dist.size(); ++i)
110 if(nodeQueue[i] && dist[i] != std::numeric_limits<float>::infinity() && dist[i] < w)
121 // find minimum distance
122 auto minDistIndex = FindMinDistance(dist);
124 // Failed to find minimum distance
125 if(minDistIndex == -1)
127 // Return empty WayPointList
131 // remove from queue by assigning infinity to distance
133 if(removeCount == nodeCount)
138 nodeQueue[minDistIndex] = nullptr;
140 [[maybe_unused]] auto sizeTmp = nodeQueue.size() - removeCount;
142 // check the neighbours
143 for(auto i = 0u; i < 3; ++i)
145 auto nIndex = mNodes[minDistIndex].faces[i];
146 if(nIndex != Scene3D::Algorithm::NavigationMesh::NULL_FACE && nodeQueue[nIndex] != nullptr)
148 auto alt = dist[minDistIndex] + mNodes[minDistIndex].weight[i];
149 if(alt < dist[nIndex])
152 prev[nIndex] = minDistIndex;
156 } while(removeCount != nodeCount);
158 // Compute distances for each node back to the source
159 auto u = targetPolyIndex;
160 std::list<uint32_t> q;
161 if(prev[u] != Scene3D::Algorithm::NavigationMesh::NULL_FACE || u == sourcePolyIndex)
163 while(u != Scene3D::Algorithm::NavigationMesh::NULL_FACE)
170 WayPointList waypoints;
171 waypoints.resize(q.size());
177 auto& wp = static_cast<WayPointData&>(waypoints[index]);
178 wp.face = mNavigationMesh->GetFace(n);
182 // set the common edge with previous node
185 const auto& node = mNodes[prevN];
186 for(auto i = 0u; i < 3; ++i)
188 if(node.faces[i] == wp.nodeIndex)
190 wp.edge = mNavigationMesh->GetEdge(node.edges[i]);
200 return OptimizeWaypoints(waypoints);
203 void PathFinderAlgorithmDjikstra::PrepareData()
205 // Build the list structure connecting the nodes
206 auto faceCount = mNavigationMesh->GetFaceCount();
208 mNodes.resize(faceCount);
210 // for each face build the list
211 for(auto i = 0u; i < faceCount; ++i)
213 auto& node = mNodes[i];
214 const auto* face = mNavigationMesh->GetFace(i);
215 auto c0 = Dali::Vector3(face->center);
217 // for each edge add neighbouring face and compute distance to set the weight of node
218 for(auto edgeIndex = 0u; edgeIndex < 3; ++edgeIndex)
220 const auto* edge = mNavigationMesh->GetEdge(face->edge[edgeIndex]);
221 auto p1 = edge->face[0];
222 auto p2 = edge->face[1];
224 // One of faces is current face so ignore it
225 auto p = ((p1 != i) ? p1 : p2);
226 node.faces[edgeIndex] = p;
227 if(p != ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE)
229 node.edges[edgeIndex] = face->edge[edgeIndex];
230 auto c1 = Dali::Vector3(mNavigationMesh->GetFace(p)->center);
231 node.weight[edgeIndex] = (c1 - c0).Length();
237 [[maybe_unused]] static float ccw(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C)
239 return (C.y - A.y) * (B.x - A.x) > (B.y - A.y) * (C.x - A.x);
242 [[maybe_unused]] static bool intersect(const Dali::Vector2& A, const Dali::Vector2& B, const Dali::Vector2& C, const Dali::Vector2& D)
244 return ccw(A, C, D) != ccw(B, C, D) && ccw(A, B, C) != ccw(A, B, D);
247 Scene3D::Algorithm::WayPointList PathFinderAlgorithmDjikstra::OptimizeWaypoints(WayPointList& waypoints) const
249 WayPointList optimizedWaypoints;
250 optimizedWaypoints.emplace_back(waypoints[0]);
251 optimizedWaypoints.reserve(waypoints.size());
253 auto startIndex = 1u;
255 bool finished = false;
256 for(auto j = 0; !finished; ++j)
258 auto& startWaypoint = optimizedWaypoints.back();
259 const auto& startWaypointData = static_cast<const WayPointData&>(startWaypoint);
261 // add new-last waypoint which will be overriden as long as intersection takes place
262 optimizedWaypoints.emplace_back();
263 for(auto wpIndex = startIndex; wpIndex < waypoints.size(); ++wpIndex)
265 if(wpIndex == waypoints.size() - 1)
267 optimizedWaypoints.back() = waypoints.back();
271 // Points between centres of faces
273 const auto& wpData = static_cast<const WayPointData&>(waypoints[wpIndex]);
275 auto Pa0 = Dali::Vector2(startWaypointData.face->center[0], startWaypointData.face->center[1]);
276 auto Pa1 = Dali::Vector2(wpData.face->center[0], wpData.face->center[1]);
278 bool doesIntersect = true;
279 for(auto i = startIndex; i < wpIndex; ++i)
281 const auto& wp = static_cast<WayPointData&>(waypoints[i]);
282 // Skip starting waypoint
283 if(wp.face == startWaypointData.face)
287 auto Pb0 = mNavigationMesh->GetVertex(wp.edge->vertex[0]);
288 auto Pb1 = mNavigationMesh->GetVertex(wp.edge->vertex[1]);
289 auto vPb0 = Dali::Vector2(Pb0->x, Pb0->y);
290 auto vPb1 = Dali::Vector2(Pb1->x, Pb1->y);
292 doesIntersect = intersect(Pa0, Pa1, vPb0, vPb1);
301 optimizedWaypoints.back() = waypoints[wpIndex - 1];
302 startIndex = wpIndex - 1;
308 for(auto& wp : optimizedWaypoints)
310 auto& wpData = static_cast<WayPointData&>(wp);
311 wpData.point3d = mNavigationMesh->PointLocalToScene(Dali::Vector3(wpData.face->center));
312 wpData.point2d = Vector2::ZERO;
315 return optimizedWaypoints;
317 } // namespace Dali::Scene3D::Internal::Algorithm