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