dali-scene3d/internal/algorithm/navigation-mesh-impl.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/navigation-mesh-impl.h>
  19
  20 // EXTERNAL INCLUDES
  21 #include <dali/integration-api/debug.h>
  22
  23 #include <algorithm>
  24 #include <filesystem>
  25
  26 using Dali::Vector3;
  27
  28 namespace Dali::Scene3D::Internal::Algorithm
  29 {
  30 using Poly   = Dali::Scene3D::Algorithm::NavigationMesh::Face;
  31 using Edge   = Dali::Scene3D::Algorithm::NavigationMesh::Edge;
  32 using Vertex = Dali::Scene3D::Algorithm::NavigationMesh::Vertex;
  33
  34 // Internal Navigation ray structure
  35 struct NavigationRay
  36 {
  37   Dali::Vector3 origin;    // Origin of ray
  38   Dali::Vector3 direction; // Direction of ray
  39 };
  40
  41 /**
  42  * Helper function calculating intersection point between triangle and ray
  43  */
  44 static bool RayTriangleIntersect(
  45   const Vector3& origin, const Vector3& direction, const Vector3& vertex0, const Vector3& vertex1, const Vector3& vertex2, const Vector3& normal, float& outDistance, Vector3& position)
  46 {
  47   auto N = normal;
  48   N.Normalize();
  49   // Step 1: finding P
  50
  51   // check if ray and plane are parallel ?
  52   float NdotRayDirection = N.Dot(direction);
  53   if(Dali::Equals(NdotRayDirection, 0.0f))
  54   {
  55     return false; // they are parallel so they don'outDistance intersect !
  56   }
  57
  58   // compute d parameter using equation 2
  59   float d = -N.Dot(vertex0);
  60
  61   // compute outDistance (equation 3)
  62   outDistance = -(N.Dot(origin) + d) / NdotRayDirection;
  63
  64   // check if the triangle is in behind the ray
  65   if(outDistance < 0) return false; // the triangle is behind
  66
  67   // compute the intersection point using equation 1
  68   auto P = origin + (direction * outDistance);
  69
  70   position = P;
  71
  72   auto edge0 = vertex1 - vertex0;
  73   auto edge1 = vertex2 - vertex1;
  74   auto edge2 = vertex0 - vertex2;
  75   auto C0    = P - vertex0;
  76   auto C1    = P - vertex1;
  77   auto C2    = P - vertex2;
  78
  79   auto r0 = N.Dot(edge0.Cross(C0));
  80   auto r1 = N.Dot(edge1.Cross(C1));
  81   auto r2 = N.Dot(edge2.Cross(C2));
  82   if(r0 > 0 &&
  83      r1 > 0 &&
  84      r2 > 0) return true; // P is inside the triangle
  85
  86   return false; // this ray hits the triangle
  87 }
  88
  89 NavigationMesh::NavigationMesh(const std::vector<uint8_t>& buffer)
  90 {
  91   mBuffer.resize(buffer.size());
  92   std::copy(buffer.begin(), buffer.end(), mBuffer.begin());
  93
  94   // Setup header from the buffer
  95   mHeader      = *reinterpret_cast<NavigationMeshHeader_V10*>(mBuffer.data());
  96   mCurrentFace = Scene3D::Algorithm::NavigationMesh::NULL_FACE;
  97 }
  98
  99 [[nodiscard]] uint32_t NavigationMesh::GetFaceCount() const
 100 {
 101   return mHeader.polyCount;
 102 }
 103
 104 [[nodiscard]] uint32_t NavigationMesh::GetEdgeCount() const
 105 {
 106   return mHeader.edgeCount;
 107 }
 108
 109 [[nodiscard]] uint32_t NavigationMesh::GetVertexCount() const
 110 {
 111   return mHeader.vertexCount;
 112 }
 113
 114 bool NavigationMesh::FindFloorForFace(const Dali::Vector3& position, FaceIndex faceIndex, bool dontCheckNeighbours, Dali::Vector3& outPosition)
 115 {
 116   if(faceIndex == ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 117   {
 118     faceIndex = mCurrentFace;
 119   }
 120
 121   // No current face, do full check
 122   if(mCurrentFace == ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE)
 123   {
 124     return FindFloor(position, outPosition);
 125   }
 126
 127   NavigationRay ray;
 128   ray.origin = PointSceneToLocal(Dali::Vector3(position)); // origin is equal position
 129
 130   // Ray direction matches gravity direction
 131   ray.direction = Vector3(mHeader.gravityVector);
 132
 133   IntersectResult result = NavigationRayFaceIntersection(ray, *GetFace(faceIndex));
 134   if(result.result)
 135   {
 136     outPosition = PointLocalToScene(result.point);
 137     return true;
 138   }
 139   else
 140   {
 141     if(dontCheckNeighbours)
 142     {
 143       return false;
 144     }
 145
 146     // Test neighbouring by edges
 147     const auto& poly = GetFace(faceIndex);
 148
 149     // collect faces sharing edges
 150     std::vector<FaceIndex> neighbourFaces;
 151
 152     for(auto edgeIndex : poly->edge)
 153     {
 154       const auto& edge = *GetEdge(edgeIndex);
 155
 156       // store faces
 157       for(auto edgeFaceIndex : edge.face)
 158       {
 159         if(edgeFaceIndex != ::Dali::Scene3D::Algorithm::NavigationMesh::NULL_FACE && edgeFaceIndex != faceIndex)
 160         {
 161           neighbourFaces.emplace_back(edgeFaceIndex);
 162         }
 163       }
 164     }
 165
 166     if(neighbourFaces.empty())
 167     {
 168       return false;
 169     }
 170
 171     for(const auto& neighbourFaceIndex : neighbourFaces)
 172     {
 173       if(FindFloorForFace(position, neighbourFaceIndex, true, outPosition))
 174       {
 175         mCurrentFace = neighbourFaceIndex;
 176         return true;
 177       }
 178     }
 179   }
 180
 181   return false;
 182 }
 183
 184 /**
 185  * Drops observer onto the floor
 186  *
 187  * When dropping observer, the nearest floor face is found
 188  */
 189 bool NavigationMesh::FindFloor(const Dali::Vector3& position, Dali::Vector3& outPosition)
 190 {
 191   FaceIndex faceIndex = 0u;
 192   return FindFloor(position, outPosition, faceIndex);
 193 }
 194
 195 bool NavigationMesh::FindFloor(const Dali::Vector3& position, Dali::Vector3& outPosition, FaceIndex& faceIndex)
 196 {
 197   [[maybe_unused]] auto newPos = PointSceneToLocal(Dali::Vector3(position));
 198
 199   NavigationRay ray;
 200
 201   ray.origin = PointSceneToLocal(Dali::Vector3(position)); // origin is equal position
 202
 203   // Ray direction matches gravity direction
 204   ray.direction = Vector3(mHeader.gravityVector);
 205
 206   const auto                   POLY_COUNT = GetFaceCount();
 207   std::vector<IntersectResult> results;
 208   for(auto faceIndex = 0u; faceIndex < POLY_COUNT; ++faceIndex)
 209   {
 210     auto result = NavigationRayFaceIntersection(ray, *GetFace(faceIndex));
 211     if(result.result)
 212     {
 213       result.faceIndex = faceIndex;
 214       results.emplace_back(result);
 215     }
 216   }
 217
 218   // find minimal distance to the floor and return that position and distance
 219   if(results.empty())
 220   {
 221     return false;
 222   }
 223
 224   std::sort(results.begin(), results.end(), [](const IntersectResult& lhs, const IntersectResult& rhs) { return lhs.distance < rhs.distance; });
 225
 226   outPosition  = PointLocalToScene(results.front().point);
 227   faceIndex    = results.front().faceIndex;
 228   mCurrentFace = results.front().faceIndex;
 229
 230   return true;
 231 }
 232
 233 const Poly* NavigationMesh::GetFace(FaceIndex index) const
 234 {
 235   auto* basePtr = reinterpret_cast<const Poly*>(mBuffer.data() + mHeader.dataOffset + mHeader.polyDataOffset);
 236   return &basePtr[index];
 237 }
 238
 239 const Edge* NavigationMesh::GetEdge(EdgeIndex index) const
 240 {
 241   auto* basePtr = reinterpret_cast<const Edge*>(mBuffer.data() + mHeader.dataOffset + mHeader.edgeDataOffset);
 242   return &basePtr[index];
 243 }
 244
 245 const Vertex* NavigationMesh::GetVertex(VertexIndex index) const
 246 {
 247   auto* basePtr = reinterpret_cast<const Vertex*>(mBuffer.data() + mHeader.dataOffset + mHeader.vertexDataOffset);
 248   return &basePtr[index];
 249 }
 250
 251 NavigationMesh::IntersectResult NavigationMesh::NavigationRayFaceIntersection(NavigationRay& ray, const Face& face) const
 252 {
 253   auto vi0 = *GetVertex(face.vertex[0]);
 254   auto vi1 = *GetVertex(face.vertex[1]);
 255   auto vi2 = *GetVertex(face.vertex[2]);
 256
 257   IntersectResult result{Vector3::ZERO, 0.0f, 0u, false};
 258
 259   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);
 260   return result;
 261 }
 262
 263 void NavigationMesh::SetTransform(const Dali::Matrix& transform)
 264 {
 265   mTransform = transform;
 266   transform.InvertTransform(mTransformInverse);
 267 }
 268
 269 Dali::Vector3 NavigationMesh::PointSceneToLocal(const Dali::Vector3& point) const
 270 {
 271   // Transform point into navmesh space
 272   Dali::Vector4 invNewPos = mTransformInverse * Dali::Vector4(point.x, -point.y, point.z, 1.0f);
 273   invNewPos.y *= -1.0f;
 274
 275   return Dali::Vector3(invNewPos.AsFloat());
 276 }
 277
 278 Dali::Vector3 NavigationMesh::PointLocalToScene(const Dali::Vector3& point) const
 279 {
 280   // Transform point into scene transform space
 281   Dali::Vector4 newPos = mTransform * Dali::Vector4(point.x, -point.y, point.z, 1.0f);
 282   newPos.y *= -1.0f;
 283
 284   return Dali::Vector3(newPos.AsFloat());
 285 }
 286
 287 Dali::Vector3 NavigationMesh::GetGravityVector() const
 288 {
 289   return Dali::Vector3(mHeader.gravityVector);
 290 }
 291
 292 } // namespace Dali::Scene3D::Internal::Algorithm