dali-scene3d/internal/algorithm/navigation-mesh-impl.cpp

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