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

Add support for multiple navmeshes on a scene

Browse Source
This commit is contained in:
Wojtek Figat
2021-01-13 14:28:46 +01:00
parent dba43c4e9f
commit 27ed23c1b9
15 changed files with 1012 additions and 626 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,18 +1,23 @@
// Copyright (c) 2012-2021 Wojciech Figat. All rights reserved.
#include "NavMeshRuntime.h"
#include "NavigationScene.h"
#include "NavMesh.h"
#include "Engine/Core/Log.h"
#include "Engine/Profiler/ProfilerCPU.h"
#include "Engine/Threading/Threading.h"
#include <ThirdParty/recastnavigation/DetourNavMesh.h>
#include <ThirdParty/recastnavigation/DetourNavMeshQuery.h>
#include <ThirdParty/recastnavigation/RecastAlloc.h>
#define MAX_NODES 2048
#define USE_DATA_LINK 0
#define USE_NAV_MESH_ALLOC 1
NavMeshRuntime::NavMeshRuntime()
#define DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL 50.0f
#define DEFAULT_NAV_QUERY_EXTENT_VERTICAL 250.0f
NavMeshRuntime::NavMeshRuntime(const NavMeshProperties& properties)
: Properties(properties)
{
_navMesh = nullptr;
_navMeshQuery = dtAllocNavMeshQuery();
@@ -30,6 +35,150 @@ int32 NavMeshRuntime::GetTilesCapacity() const
return _navMesh ? _navMesh->getMaxTiles() : 0;
}
bool NavMeshRuntime::FindDistanceToWall(const Vector3& startPosition, NavMeshHit& hitInfo, float maxDistance) const
{
ScopeLock lock(Locker);
const auto query = GetNavMeshQuery();
if (!query)
{
return false;
}
dtQueryFilter filter;
Vector3 extent(DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL, DEFAULT_NAV_QUERY_EXTENT_VERTICAL, DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL);
dtPolyRef startPoly = 0;
query->findNearestPoly(&startPosition.X, &extent.X, &filter, &startPoly, nullptr);
if (!startPoly)
{
return false;
}
return dtStatusSucceed(query->findDistanceToWall(startPoly, &startPosition.X, maxDistance, &filter, &hitInfo.Distance, &hitInfo.Position.X, &hitInfo.Normal.X));
}
bool NavMeshRuntime::FindPath(const Vector3& startPosition, const Vector3& endPosition, Array<Vector3, HeapAllocation>& resultPath) const
{
resultPath.Clear();
ScopeLock lock(Locker);
const auto query = GetNavMeshQuery();
if (!query)
{
return false;
}
dtQueryFilter filter;
Vector3 extent(DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL, DEFAULT_NAV_QUERY_EXTENT_VERTICAL, DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL);
dtPolyRef startPoly = 0;
query->findNearestPoly(&startPosition.X, &extent.X, &filter, &startPoly, nullptr);
if (!startPoly)
{
return false;
}
dtPolyRef endPoly = 0;
query->findNearestPoly(&endPosition.X, &extent.X, &filter, &endPoly, nullptr);
if (!endPoly)
{
return false;
}
dtPolyRef path[NAV_MESH_PATH_MAX_SIZE];
int32 pathSize;
const auto findPathStatus = query->findPath(startPoly, endPoly, &startPosition.X, &endPosition.X, &filter, path, &pathSize, NAV_MESH_PATH_MAX_SIZE);
if (dtStatusFailed(findPathStatus))
{
return false;
}
// Check for special case, where path has not been found, and starting polygon was the one closest to the target
if (pathSize == 1 && dtStatusDetail(findPathStatus, DT_PARTIAL_RESULT))
{
// In this case we find a point on starting polygon, that's closest to destination and store it as path end
resultPath.Resize(2);
resultPath[0] = startPosition;
resultPath[1] = startPosition;
query->closestPointOnPolyBoundary(startPoly, &endPosition.X, &resultPath[1].X);
}
else
{
int straightPathCount = 0;
resultPath.EnsureCapacity(NAV_MESH_PATH_MAX_SIZE);
const auto findStraightPathStatus = query->findStraightPath(&startPosition.X, &endPosition.X, path, pathSize, (float*)resultPath.Get(), nullptr, nullptr, &straightPathCount, resultPath.Capacity(), DT_STRAIGHTPATH_AREA_CROSSINGS);
if (dtStatusFailed(findStraightPathStatus))
{
return false;
}
resultPath.Resize(straightPathCount);
}
return true;
}
bool NavMeshRuntime::ProjectPoint(const Vector3& point, Vector3& result) const
{
ScopeLock lock(Locker);
const auto query = GetNavMeshQuery();
if (!query)
{
return false;
}
dtQueryFilter filter;
Vector3 extent(DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL, DEFAULT_NAV_QUERY_EXTENT_VERTICAL, DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL);
dtPolyRef startPoly = 0;
query->findNearestPoly(&point.X, &extent.X, &filter, &startPoly, &result.X);
if (!startPoly)
{
return false;
}
return true;
}
bool NavMeshRuntime::RayCast(const Vector3& startPosition, const Vector3& endPosition, NavMeshHit& hitInfo) const
{
ScopeLock lock(Locker);
const auto query = GetNavMeshQuery();
if (!query)
{
return false;
}
dtQueryFilter filter;
Vector3 extent(DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL, DEFAULT_NAV_QUERY_EXTENT_VERTICAL, DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL);
dtPolyRef startPoly = 0;
query->findNearestPoly(&startPosition.X, &extent.X, &filter, &startPoly, nullptr);
if (!startPoly)
{
return false;
}
dtRaycastHit hit;
hit.path = nullptr;
hit.maxPath = 0;
const bool result = dtStatusSucceed(query->raycast(startPoly, &startPosition.X, &endPosition.X, &filter, 0, &hit));
if (hit.t >= MAX_float)
{
hitInfo.Position = endPosition;
hitInfo.Distance = 0;
}
else
{
hitInfo.Position = startPosition + (endPosition - startPosition) * hit.t;
hitInfo.Distance = hit.t;
}
hitInfo.Normal = *(Vector3*)&hit.hitNormal;
return result;
}
void NavMeshRuntime::SetTileSize(float tileSize)
{
ScopeLock lock(Locker);
@@ -132,13 +281,13 @@ void NavMeshRuntime::EnsureCapacity(int32 tilesToAddCount)
}
}
void NavMeshRuntime::AddTiles(NavigationScene* scene)
void NavMeshRuntime::AddTiles(NavMesh* navMesh)
{
// Skip if no data
ASSERT(scene);
if (scene->Data.Tiles.IsEmpty())
ASSERT(navMesh);
if (navMesh->Data.Tiles.IsEmpty())
return;
auto& data = scene->Data;
auto& data = navMesh->Data;
PROFILE_CPU_NAMED("NavMeshRuntime.AddTiles");
@@ -164,14 +313,14 @@ void NavMeshRuntime::AddTiles(NavigationScene* scene)
// Add new tiles
for (auto& tileData : data.Tiles)
{
AddTileInternal(scene, tileData);
AddTileInternal(navMesh, tileData);
}
}
void NavMeshRuntime::AddTile(NavigationScene* scene, NavMeshTileData& tileData)
void NavMeshRuntime::AddTile(NavMesh* navMesh, NavMeshTileData& tileData)
{
ASSERT(scene);
auto& data = scene->Data;
ASSERT(navMesh);
auto& data = navMesh->Data;
PROFILE_CPU_NAMED("NavMeshRuntime.AddTile");
@@ -195,17 +344,17 @@ void NavMeshRuntime::AddTile(NavigationScene* scene, NavMeshTileData& tileData)
EnsureCapacity(1);
// Add new tile
AddTileInternal(scene, tileData);
AddTileInternal(navMesh, tileData);
}
bool IsTileFromScene(const NavMeshRuntime* navMesh, const NavMeshTile& tile, void* customData)
{
return tile.Scene == (NavigationScene*)customData;
return tile.NavMesh == (NavMesh*)customData;
}
void NavMeshRuntime::RemoveTiles(NavigationScene* scene)
void NavMeshRuntime::RemoveTiles(NavMesh* navMesh)
{
RemoveTiles(IsTileFromScene, scene);
RemoveTiles(IsTileFromScene, navMesh);
}
void NavMeshRuntime::RemoveTile(int32 x, int32 y, int32 layer)
@@ -274,6 +423,101 @@ void NavMeshRuntime::RemoveTiles(bool (* prediction)(const NavMeshRuntime* navMe
}
}
#if COMPILE_WITH_DEBUG_DRAW
#include "Engine/Debug/DebugDraw.h"
void DrawPoly(NavMeshRuntime* navMesh, const dtMeshTile& tile, const dtPoly& poly)
{
const unsigned int ip = (unsigned int)(&poly - tile.polys);
const dtPolyDetail& pd = tile.detailMeshes[ip];
const Color color = navMesh->Properties.Color;
const float drawOffsetY = 10.0f + ((float)GetHash(color) / (float)MAX_uint32) * 10.0f; // Apply some offset to prevent Z-fighting for different navmeshes
const Color fillColor = color * 0.5f;
const Color edgesColor = Color::FromHSV(color.ToHSV() + Vector3(20.0f, 0, -0.1f), color.A);
for (int i = 0; i < pd.triCount; i++)
{
Vector3 v[3];
const unsigned char* t = &tile.detailTris[(pd.triBase + i) * 4];
for (int k = 0; k < 3; k++)
{
if (t[k] < poly.vertCount)
{
v[k] = *(Vector3*)&tile.verts[poly.verts[t[k]] * 3];
}
else
{
v[k] = *(Vector3*)&tile.detailVerts[(pd.vertBase + t[k] - poly.vertCount) * 3];
}
}
v[0].Y += drawOffsetY;
v[1].Y += drawOffsetY;
v[2].Y += drawOffsetY;
DEBUG_DRAW_TRIANGLE(v[0], v[1], v[2], fillColor, 0, true);
}
for (int k = 0; k < pd.triCount; k++)
{
const unsigned char* t = &tile.detailTris[(pd.triBase + k) * 4];
Vector3 v[3];
for (int m = 0; m < 3; m++)
{
if (t[m] < poly.vertCount)
v[m] = *(Vector3*)&tile.verts[poly.verts[t[m]] * 3];
else
v[m] = *(Vector3*)&tile.detailVerts[(pd.vertBase + (t[m] - poly.vertCount)) * 3];
}
v[0].Y += drawOffsetY;
v[1].Y += drawOffsetY;
v[2].Y += drawOffsetY;
for (int m = 0, n = 2; m < 3; n = m++)
{
// Skip inner detail edges
if (((t[3] >> (n * 2)) & 0x3) == 0)
continue;
DEBUG_DRAW_LINE(v[n], v[m], edgesColor, 0, true);
}
}
}
void NavMeshRuntime::DebugDraw()
{
ScopeLock lock(Locker);
const dtNavMesh* dtNavMesh = GetNavMesh();
const int tilesCount = dtNavMesh ? dtNavMesh->getMaxTiles() : 0;
if (tilesCount == 0)
return;
for (int tileIndex = 0; tileIndex < tilesCount; tileIndex++)
{
const dtMeshTile* tile = dtNavMesh->getTile(tileIndex);
if (!tile->header)
continue;
//DebugDraw::DrawWireBox(*(BoundingBox*)&tile->header->bmin[0], Color::CadetBlue);
for (int i = 0; i < tile->header->polyCount; i++)
{
const dtPoly* poly = &tile->polys[i];
if (poly->getType() != DT_POLYTYPE_GROUND)
continue;
DrawPoly(this, *tile, *poly);
}
}
}
#endif
void NavMeshRuntime::Dispose()
{
if (_navMesh)
@@ -284,7 +528,7 @@ void NavMeshRuntime::Dispose()
_tiles.Resize(0);
}
void NavMeshRuntime::AddTileInternal(NavigationScene* scene, NavMeshTileData& tileData)
void NavMeshRuntime::AddTileInternal(NavMesh* navMesh, NavMeshTileData& tileData)
{
// Check if that tile has been added to navmesh
NavMeshTile* tile = nullptr;
@@ -313,7 +557,7 @@ void NavMeshRuntime::AddTileInternal(NavigationScene* scene, NavMeshTileData& ti
ASSERT(tile);
// Copy tile properties
tile->Scene = scene;
tile->NavMesh = navMesh;
tile->X = tileData.PosX;
tile->Y = tileData.PosY;
tile->Layer = tileData.Layer;