GoaLitiuM/FlaxEngine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix FindRandomPointAroundCircle to always find a valid point on a NavMesh in the radius

Browse Source 
#2398
This commit is contained in:
Wojtek Figat
2024-04-20 15:01:27 +02:00
parent 560cf65121
commit 41ffc16b66
5 changed files with 54 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
Source/Engine/Navigation/NavMeshBuilder.cpp
View File

@@ -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");

13
Source/Engine/Navigation/NavMeshData.cpp
View File

@@ -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<NavMeshDataHeader>(1);
const auto header = stream.Move<NavMeshDataHeader>();
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<NavMeshTileDataHeader>(1);
const auto tileHeader = stream.Move<NavMeshTileDataHeader>();
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<byte>(tileHeader->DataSize);
if (copyData)
{
tile.Data.Copy(tileData, tileHeader->DataSize);
}
else
{
tile.Data.Link(tileData, tileHeader->DataSize);
}
}
return false;

87
Source/Engine/Navigation/NavMeshRuntime.cpp
View File

@@ -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(&centerNavMesh.X, &extent.X, &filter, &centerPoly, nullptr);
if (!centerPoly)
{
if (!dtStatusSucceed(query->findNearestPoly(&centerNavMesh.X, &extent.X, &filter, &centerPoly, nullptr)))
return false;
}
dtPolyRef randomPoly = 0;
Float3 randomPt;
query->findRandomPointAroundCircle(centerPoly, &centerNavMesh.X, radius, &filter, Random::Rand, &randomPoly, &randomPt.X);
if (!randomPoly)
{
if (!dtStatusSucceed(query->findRandomPointAroundCircle(centerPoly, &centerNavMesh.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(&params)))
{
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);
}
}

2
Source/Engine/Navigation/NavMeshRuntime.h
View File

@@ -160,7 +160,7 @@ public:
/// <param name="point">The source point.</param>
/// <param name="result">The result position on the navmesh (valid only if method returns true).</param>
/// <returns>True if found valid location on the navmesh, otherwise false.</returns>
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);
}

27
Source/ThirdParty/recastnavigation/DetourNavMeshQuery.cpp vendored
View File

@@ -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);