From 41ffc16b66d756e82b160b1803a97512e0c3f0f2 Mon Sep 17 00:00:00 2001 From: Wojtek Figat Date: Sat, 20 Apr 2024 15:01:27 +0200 Subject: [PATCH] Fix `FindRandomPointAroundCircle` to always find a valid point on a NavMesh in the radius #2398 --- Source/Engine/Navigation/NavMeshBuilder.cpp | 1 + Source/Engine/Navigation/NavMeshData.cpp | 13 +-- Source/Engine/Navigation/NavMeshRuntime.cpp | 87 +++++-------------- Source/Engine/Navigation/NavMeshRuntime.h | 2 +- .../recastnavigation/DetourNavMeshQuery.cpp | 27 +++++- 5 files changed, 54 insertions(+), 76 deletions(-) diff --git a/Source/Engine/Navigation/NavMeshBuilder.cpp b/Source/Engine/Navigation/NavMeshBuilder.cpp index eedd71158..5bc5141fa 100644 --- a/Source/Engine/Navigation/NavMeshBuilder.cpp +++ b/Source/Engine/Navigation/NavMeshBuilder.cpp @@ -628,6 +628,7 @@ bool GenerateTile(NavMesh* navMesh, NavMeshRuntime* runtime, int32 x, int32 y, B LOG(Warning, "Could not build Detour navmesh."); return true; } + ASSERT_LOW_LAYER(navDataSize > 4 && *(uint32*)navData == DT_NAVMESH_MAGIC); // Sanity check for Detour header { PROFILE_CPU_NAMED("Navigation.CreateTile"); diff --git a/Source/Engine/Navigation/NavMeshData.cpp b/Source/Engine/Navigation/NavMeshData.cpp index 16227ce98..c6faccc1e 100644 --- a/Source/Engine/Navigation/NavMeshData.cpp +++ b/Source/Engine/Navigation/NavMeshData.cpp @@ -17,7 +17,7 @@ void NavMeshData::Save(WriteStream& stream) // Write tiles for (int32 tileIndex = 0; tileIndex < Tiles.Count(); tileIndex++) { - auto& tile = Tiles[tileIndex]; + auto& tile = Tiles.Get()[tileIndex]; // Write tile header NavMeshTileDataHeader tileHeader; @@ -41,7 +41,6 @@ void NavMeshData::Save(WriteStream& stream) bool NavMeshData::Load(BytesContainer& data, bool copyData) { - // No data if (data.Length() < sizeof(NavMeshDataHeader)) { LOG(Warning, "No valid navmesh data."); @@ -50,7 +49,7 @@ bool NavMeshData::Load(BytesContainer& data, bool copyData) MemoryReadStream stream(data.Get(), data.Length()); // Read header - const auto header = stream.Move(1); + const auto header = stream.Move(); if (header->Version != 1) { LOG(Warning, "Invalid valid navmesh data version {0}.", header->Version); @@ -67,10 +66,10 @@ bool NavMeshData::Load(BytesContainer& data, bool copyData) // Read tiles for (int32 tileIndex = 0; tileIndex < Tiles.Count(); tileIndex++) { - auto& tile = Tiles[tileIndex]; + auto& tile = Tiles.Get()[tileIndex]; // Read tile header - const auto tileHeader = stream.Move(1); + const auto tileHeader = stream.Move(); if (tileHeader->DataSize <= 0) { LOG(Warning, "Invalid navmesh tile data."); @@ -83,13 +82,9 @@ bool NavMeshData::Load(BytesContainer& data, bool copyData) // Read tile data const auto tileData = stream.Move(tileHeader->DataSize); if (copyData) - { tile.Data.Copy(tileData, tileHeader->DataSize); - } else - { tile.Data.Link(tileData, tileHeader->DataSize); - } } return false; diff --git a/Source/Engine/Navigation/NavMeshRuntime.cpp b/Source/Engine/Navigation/NavMeshRuntime.cpp index 0e6ae0a0e..c4576aa53 100644 --- a/Source/Engine/Navigation/NavMeshRuntime.cpp +++ b/Source/Engine/Navigation/NavMeshRuntime.cpp @@ -60,17 +60,12 @@ bool NavMeshRuntime::FindDistanceToWall(const Vector3& startPosition, NavMeshHit Float3::Transform(startPosition, Properties.Rotation, startPositionNavMesh); dtPolyRef startPoly = 0; - query->findNearestPoly(&startPositionNavMesh.X, &extent.X, &filter, &startPoly, nullptr); - if (!startPoly) - { + if (!dtStatusSucceed(query->findNearestPoly(&startPositionNavMesh.X, &extent.X, &filter, &startPoly, nullptr))) return false; - } Float3 hitPosition, hitNormal; if (!dtStatusSucceed(query->findDistanceToWall(startPoly, &startPositionNavMesh.X, maxDistance, &filter, &hitInfo.Distance, &hitPosition.X, &hitNormal.X))) - { return false; - } Quaternion invRotation; Quaternion::Invert(Properties.Rotation, invRotation); @@ -98,17 +93,11 @@ bool NavMeshRuntime::FindPath(const Vector3& startPosition, const Vector3& endPo Float3::Transform(endPosition, Properties.Rotation, endPositionNavMesh); dtPolyRef startPoly = 0; - query->findNearestPoly(&startPositionNavMesh.X, &extent.X, &filter, &startPoly, nullptr); - if (!startPoly) - { + if (!dtStatusSucceed(query->findNearestPoly(&startPositionNavMesh.X, &extent.X, &filter, &startPoly, nullptr))) return false; - } dtPolyRef endPoly = 0; - query->findNearestPoly(&endPositionNavMesh.X, &extent.X, &filter, &endPoly, nullptr); - if (!endPoly) - { + if (!dtStatusSucceed(query->findNearestPoly(&endPositionNavMesh.X, &extent.X, &filter, &endPoly, nullptr))) return false; - } dtPolyRef path[NAV_MESH_PATH_MAX_SIZE]; int32 pathSize; @@ -166,30 +155,19 @@ bool NavMeshRuntime::TestPath(const Vector3& startPosition, const Vector3& endPo Float3::Transform(endPosition, Properties.Rotation, endPositionNavMesh); dtPolyRef startPoly = 0; - query->findNearestPoly(&startPositionNavMesh.X, &extent.X, &filter, &startPoly, nullptr); - if (!startPoly) - { + if (!dtStatusSucceed(query->findNearestPoly(&startPositionNavMesh.X, &extent.X, &filter, &startPoly, nullptr))) return false; - } dtPolyRef endPoly = 0; - query->findNearestPoly(&endPositionNavMesh.X, &extent.X, &filter, &endPoly, nullptr); - if (!endPoly) - { + if (!dtStatusSucceed(query->findNearestPoly(&endPositionNavMesh.X, &extent.X, &filter, &endPoly, nullptr))) return false; - } dtPolyRef path[NAV_MESH_PATH_MAX_SIZE]; int32 pathSize; const auto findPathStatus = query->findPath(startPoly, endPoly, &startPositionNavMesh.X, &endPositionNavMesh.X, &filter, path, &pathSize, NAV_MESH_PATH_MAX_SIZE); if (dtStatusFailed(findPathStatus)) - { return false; - } - if (dtStatusDetail(findPathStatus, DT_PARTIAL_RESULT)) - { return false; - } return true; } @@ -210,11 +188,8 @@ bool NavMeshRuntime::FindClosestPoint(const Vector3& point, Vector3& result) con dtPolyRef startPoly = 0; Float3 nearestPt; - query->findNearestPoly(&pointNavMesh.X, &extent.X, &filter, &startPoly, &nearestPt.X); - if (!startPoly) - { + if (!dtStatusSucceed(query->findNearestPoly(&pointNavMesh.X, &extent.X, &filter, &startPoly, &nearestPt.X))) return false; - } Quaternion invRotation; Quaternion::Invert(Properties.Rotation, invRotation); @@ -235,11 +210,8 @@ bool NavMeshRuntime::FindRandomPoint(Vector3& result) const dtPolyRef randomPoly = 0; Float3 randomPt; - query->findRandomPoint(&filter, Random::Rand, &randomPoly, &randomPt.X); - if (!randomPoly) - { + if (!dtStatusSucceed(query->findRandomPoint(&filter, Random::Rand, &randomPoly, &randomPt.X))) return false; - } Quaternion invRotation; Quaternion::Invert(Properties.Rotation, invRotation); @@ -257,25 +229,19 @@ bool NavMeshRuntime::FindRandomPointAroundCircle(const Vector3& center, float ra dtQueryFilter filter; InitFilter(filter); - Float3 extent = Properties.DefaultQueryExtent; + Float3 extent(radius); Float3 centerNavMesh; Float3::Transform(center, Properties.Rotation, centerNavMesh); dtPolyRef centerPoly = 0; - query->findNearestPoly(¢erNavMesh.X, &extent.X, &filter, ¢erPoly, nullptr); - if (!centerPoly) - { + if (!dtStatusSucceed(query->findNearestPoly(¢erNavMesh.X, &extent.X, &filter, ¢erPoly, nullptr))) return false; - } dtPolyRef randomPoly = 0; Float3 randomPt; - query->findRandomPointAroundCircle(centerPoly, ¢erNavMesh.X, radius, &filter, Random::Rand, &randomPoly, &randomPt.X); - if (!randomPoly) - { + if (!dtStatusSucceed(query->findRandomPointAroundCircle(centerPoly, ¢erNavMesh.X, radius, &filter, Random::Rand, &randomPoly, &randomPt.X))) return false; - } Quaternion invRotation; Quaternion::Invert(Properties.Rotation, invRotation); @@ -300,11 +266,8 @@ bool NavMeshRuntime::RayCast(const Vector3& startPosition, const Vector3& endPos Float3::Transform(endPosition, Properties.Rotation, endPositionNavMesh); dtPolyRef startPoly = 0; - query->findNearestPoly(&startPositionNavMesh.X, &extent.X, &filter, &startPoly, nullptr); - if (!startPoly) - { + if (!dtStatusSucceed(query->findNearestPoly(&startPositionNavMesh.X, &extent.X, &filter, &startPoly, nullptr))) return false; - } dtRaycastHit hit; hit.path = nullptr; @@ -361,14 +324,6 @@ void NavMeshRuntime::EnsureCapacity(int32 tilesToAddCount) // Ensure to have size assigned ASSERT(_tileSize != 0); - // Allocate navmesh and initialize the default query - if (!_navMesh) - _navMesh = dtAllocNavMesh(); - if (dtStatusFailed(_navMeshQuery->init(_navMesh, MAX_NODES))) - { - LOG(Error, "Failed to initialize navmesh {0}.", Properties.Name); - } - // Prepare parameters dtNavMeshParams params; params.orig[0] = 0.0f; @@ -381,11 +336,17 @@ void NavMeshRuntime::EnsureCapacity(int32 tilesToAddCount) params.maxPolys = 1 << (22 - tilesBits); // Initialize nav mesh + if (!_navMesh) + _navMesh = dtAllocNavMesh(); if (dtStatusFailed(_navMesh->init(¶ms))) { - LOG(Error, "Navmesh {0} init failed.", Properties.Name); + LOG(Error, "Navmesh {0} init failed", Properties.Name); return; } + if (dtStatusFailed(_navMeshQuery->init(_navMesh, MAX_NODES))) + { + LOG(Error, "Navmesh query {0} init failed", Properties.Name); + } // Prepare tiles container _tiles.EnsureCapacity(newCapacity); @@ -405,7 +366,7 @@ void NavMeshRuntime::EnsureCapacity(int32 tilesToAddCount) const auto result = _navMesh->addTile(data, dataSize, flags, 0, nullptr); if (dtStatusFailed(result)) { - LOG(Warning, "Could not add tile to navmesh {0} (error: {1}).", Properties.Name, result & ~DT_FAILURE); + LOG(Warning, "Could not add tile ({2}x{3}, layer {4}) to navmesh {0} (error: {1})", Properties.Name, result & ~DT_FAILURE, tile.X, tile.Y, tile.Layer); } } } @@ -490,13 +451,11 @@ void NavMeshRuntime::RemoveTile(int32 x, int32 y, int32 layer) const auto tileRef = _navMesh->getTileRefAt(x, y, layer); if (tileRef == 0) - { return; - } if (dtStatusFailed(_navMesh->removeTile(tileRef, nullptr, nullptr))) { - LOG(Warning, "Failed to remove tile from navmesh {0}.", Properties.Name); + LOG(Warning, "Failed to remove tile ({1}x{2}, layer {3}) from navmesh {0}", Properties.Name, x, y, layer); } for (int32 i = 0; i < _tiles.Count(); i++) @@ -532,7 +491,7 @@ void NavMeshRuntime::RemoveTiles(bool (*prediction)(const NavMeshRuntime* navMes { if (dtStatusFailed(_navMesh->removeTile(tileRef, nullptr, nullptr))) { - LOG(Warning, "Failed to remove tile from navmesh {0}.", Properties.Name); + LOG(Warning, "Failed to remove tile ({1}x{2}, layer {3}) from navmesh {0}", Properties.Name, tile.X, tile.Y, tile.Layer); } } @@ -668,7 +627,7 @@ void NavMeshRuntime::AddTileInternal(NavMesh* navMesh, NavMeshTileData& tileData // Remove any existing tile at that location if (dtStatusFailed(_navMesh->removeTile(tileRef, nullptr, nullptr))) { - LOG(Warning, "Failed to remove tile from navmesh {0}.", Properties.Name); + LOG(Warning, "Failed to remove tile from navmesh {0}", Properties.Name); } // Reuse tile data container @@ -712,6 +671,6 @@ void NavMeshRuntime::AddTileInternal(NavMesh* navMesh, NavMeshTileData& tileData const auto result = _navMesh->addTile(data, dataSize, flags, 0, nullptr); if (dtStatusFailed(result)) { - LOG(Warning, "Could not add tile to navmesh {0} (error: {1}).", Properties.Name, result & ~DT_FAILURE); + LOG(Warning, "Could not add tile ({2}x{3}, layer {4}) to navmesh {0} (error: {1})", Properties.Name, result & ~DT_FAILURE, tileData.PosX, tileData.PosY, tileData.Layer); } } diff --git a/Source/Engine/Navigation/NavMeshRuntime.h b/Source/Engine/Navigation/NavMeshRuntime.h index 9b090b97c..40ea0959e 100644 --- a/Source/Engine/Navigation/NavMeshRuntime.h +++ b/Source/Engine/Navigation/NavMeshRuntime.h @@ -160,7 +160,7 @@ public: /// The source point. /// The result position on the navmesh (valid only if method returns true). /// True if found valid location on the navmesh, otherwise false. - API_FUNCTION() bool ProjectPoint(const Vector3& point, API_PARAM(Out) Vector3& result) const + API_FUNCTION() DEPRECATED bool ProjectPoint(const Vector3& point, API_PARAM(Out) Vector3& result) const { return FindClosestPoint(point, result); } diff --git a/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.cpp b/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.cpp index 7faefde81..5c7b6d797 100644 --- a/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.cpp +++ b/Source/ThirdParty/recastnavigation/DetourNavMeshQuery.cpp @@ -487,14 +487,37 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f int checksLimit = 100; do { + // Loop until finds a point on a random poly that is in the radius const float s = frand(); const float t = frand(); dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt); } while (dtDistancePtPtSqr2D(centerPos, pt) > radiusSqr && checksLimit-- > 0); if (checksLimit <= 0) - return DT_FAILURE; - + { + // Check nearby polygons + float halfExtents[3] = { maxRadius, maxRadius, maxRadius }; + dtPolyRef polys[32]; + int polyCount; + queryPolygons(centerPos, halfExtents, filter, polys, &polyCount, 32); + for (int i = 0; i < polyCount && checksLimit <= 0; i++) + { + checksLimit = 100; + randomPolyRef = polys[i]; + m_nav->getTileAndPolyByRefUnsafe(randomPolyRef, &randomTile, &randomPoly); + do + { + // Loop until finds a point on a random poly that is in the radius + const float s = frand(); + const float t = frand(); + dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt); + } + while (dtDistancePtPtSqr2D(centerPos, pt) > radiusSqr && checksLimit-- > 0); + } + if (checksLimit <= 0) + return DT_FAILURE; + } + closestPointOnPoly(randomPolyRef, pt, pt, NULL); dtVcopy(randomPt, pt);