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