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/navigation-mesh-impl.h>
21 #include <dali/integration-api/debug.h>
31 namespace Dali::Scene3D::Internal::Algorithm
34 using Poly = Dali::Scene3D::Algorithm::NavigationMesh::Face;
35 using Edge = Dali::Scene3D::Algorithm::NavigationMesh::Edge;
36 using Vertex = Dali::Scene3D::Algorithm::NavigationMesh::Vertex;
38 // Internal Navigation ray structure
41 Dali::Vector3 origin; // Origin of ray
42 Dali::Vector3 direction; // Direction of ray
46 * Helper function calculating intersection point between triangle and ray
48 static bool RayTriangleIntersect(
49 const Vector3& origin, const Vector3& direction, const Vector3& vertex0, const Vector3& vertex1, const Vector3& vertex2, const Vector3& normal, float& outDistance, Vector3& position)
55 // check if ray and plane are parallel ?
56 float NdotRayDirection = N.Dot(direction);
57 if(Dali::Equals(NdotRayDirection, 0.0f))
59 return false; // they are parallel so they don'outDistance intersect !
62 // compute d parameter using equation 2
63 float d = -N.Dot(vertex0);
65 // compute outDistance (equation 3)
66 outDistance = -(N.Dot(origin) + d) / NdotRayDirection;
68 // check if the triangle is in behind the ray
69 if(outDistance < 0) return false; // the triangle is behind
71 // compute the intersection point using equation 1
72 auto P = origin + (direction * outDistance);
76 auto edge0 = vertex1 - vertex0;
77 auto edge1 = vertex2 - vertex1;
78 auto edge2 = vertex0 - vertex2;
79 auto C0 = P - vertex0;
80 auto C1 = P - vertex1;
81 auto C2 = P - vertex2;
83 auto r0 = N.Dot(edge0.Cross(C0));
84 auto r1 = N.Dot(edge1.Cross(C1));
85 auto r2 = N.Dot(edge2.Cross(C2));
88 r2 > 0) return true; // P is inside the triangle
90 return false; // this ray hits the triangle
93 NavigationMesh::NavigationMesh(const std::vector<uint8_t>& buffer)
95 mBuffer.resize(buffer.size());
96 std::copy(buffer.begin(), buffer.end(), mBuffer.begin());
98 // Setup header from the buffer
99 mHeader = *reinterpret_cast<NavigationMeshHeader_V10*>(mBuffer.data());
100 mCurrentFace = Scene3D::Algorithm::NavigationMesh::NULL_FACE;
103 [[nodiscard]] uint32_t NavigationMesh::GetFaceCount() const
105 return mHeader.polyCount;
108 [[nodiscard]] uint32_t NavigationMesh::GetEdgeCount() const
110 return mHeader.edgeCount;
113 [[nodiscard]] uint32_t NavigationMesh::GetVertexCount() const
115 return mHeader.vertexCount;
118 bool NavigationMesh::FindFloorForFace(const Dali::Vector3& position, uint32_t faceIndex, bool dontCheckNeighbours, Dali::Vector3& outPosition)
120 if(faceIndex == ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE)
122 faceIndex = mCurrentFace;
125 // No current face, do full check
126 if(mCurrentFace == ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE)
128 return FindFloor(position, outPosition);
132 ray.origin = PointSceneToLocal(Dali::Vector3(position)); // origin is equal position
134 // Ray direction matches gravity direction
135 ray.direction = Vector3(mHeader.gravityVector);
137 IntersectResult result = NavigationRayFaceIntersection(ray, *GetFace(uint16_t(faceIndex)));
140 outPosition = PointLocalToScene(result.point);
145 if(dontCheckNeighbours)
150 // Test neighbouring by edges
151 const auto& poly = GetFace(uint16_t(faceIndex));
153 // collect faces sharing edges
154 std::vector<uint32_t> faces;
156 for(unsigned short i : poly->edge)
158 const auto& edge = *GetEdge(i);
161 for(unsigned short j : edge.face)
163 if(j != ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE && j != faceIndex)
165 faces.emplace_back(j);
175 for(const auto& p : faces)
177 if(FindFloorForFace(position, p, true, outPosition))
189 * Drops observer onto the floor
191 * When dropping observer, the nearest floor face is found
193 bool NavigationMesh::FindFloor(const Dali::Vector3& position, Dali::Vector3& outPosition)
195 uint32_t polyIndex = 0;
196 return FindFloor(position, outPosition, polyIndex);
199 bool NavigationMesh::FindFloor(const Dali::Vector3& position, Dali::Vector3& outPosition, uint32_t& faceIndex)
201 [[maybe_unused]] auto newPos = PointSceneToLocal(Dali::Vector3(position));
205 ray.origin = PointSceneToLocal(Dali::Vector3(position)); // origin is equal position
207 // Ray direction matches gravity direction
208 ray.direction = Vector3(mHeader.gravityVector);
210 const auto POLY_COUNT = GetFaceCount();
211 std::vector<IntersectResult> results;
212 for(auto i = 0u; i < POLY_COUNT; ++i)
214 auto result = NavigationRayFaceIntersection(ray, *GetFace(i));
217 result.faceIndex = i;
218 results.emplace_back(result);
222 // find minimal distance to the floor and return that position and distance
228 std::sort(results.begin(), results.end(), [](const IntersectResult& lhs, const IntersectResult& rhs)
229 { return lhs.distance < rhs.distance; });
231 outPosition = PointLocalToScene(results.front().point);
232 faceIndex = results.front().faceIndex;
233 mCurrentFace = results.front().faceIndex;
238 const Poly* NavigationMesh::GetFace(int index) const
240 auto* basePtr = reinterpret_cast<const Poly*>(mBuffer.data() + mHeader.dataOffset + mHeader.polyDataOffset);
241 return &basePtr[index];
244 const Edge* NavigationMesh::GetEdge(int index) const
246 auto* basePtr = reinterpret_cast<const Edge*>(mBuffer.data() + mHeader.dataOffset + mHeader.edgeDataOffset);
247 return &basePtr[index];
250 const Vertex* NavigationMesh::GetVertex(int index) const
252 auto* basePtr = reinterpret_cast<const Vertex*>(mBuffer.data() + mHeader.dataOffset + mHeader.vertexDataOffset);
253 return &basePtr[index];
256 NavigationMesh::IntersectResult NavigationMesh::NavigationRayFaceIntersection(NavigationRay& ray, const Face& face)
258 auto vi0 = *GetVertex(face.vertex[0]);
259 auto vi1 = *GetVertex(face.vertex[1]);
260 auto vi2 = *GetVertex(face.vertex[2]);
262 IntersectResult result{Vector3::ZERO, 0.0f, 0u, false};
264 result.result = RayTriangleIntersect(ray.origin, ray.direction, Vector3(vi0.x, vi0.y, vi0.z), Vector3(vi1.x, vi1.y, vi1.z), Vector3(vi2.x, vi2.y, vi2.z), Vector3(face.normal), result.distance, result.point);
268 void NavigationMesh::SetTransform(const Dali::Matrix& transform)
270 mTransform = transform;
271 transform.InvertTransform(mTransformInverse);
274 Dali::Vector3 NavigationMesh::PointSceneToLocal(const Dali::Vector3& point)
276 // Transform point into navmesh space
277 Dali::Vector4 invNewPos = mTransformInverse * Dali::Vector4(point.x, -point.y, point.z, 1.0f);
278 invNewPos.y *= -1.0f;
280 return Dali::Vector3(invNewPos.AsFloat());
283 Dali::Vector3 NavigationMesh::PointLocalToScene(const Dali::Vector3& point)
285 // Transform point into scene transform space
286 Dali::Vector4 newPos = mTransform * Dali::Vector4(point.x, -point.y, point.z, 1.0f);
289 return Dali::Vector3(newPos.AsFloat());
292 Dali::Vector3 NavigationMesh::GetGravityVector() const
294 return Dali::Vector3( mHeader.gravityVector );