Skip to content

Commit 4e90fbd

Browse files
DoprezDoprez
authored
Bepu navigation clean up part 2 (#7)
* navigation component overhaul * Removing redundant code * move to GameSystem * Updated DotRecast * fix world transofrm move bug * added build time property * More settings! * fix forward to be Z * Eiderens a genius fixed move * docs and extra options for tryfindpath * omit string value
1 parent d48aec4 commit 4e90fbd

File tree

10 files changed

+144
-155
lines changed

10 files changed

+144
-155
lines changed

‎sources/Directory.Packages.props‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
<PackageVersion Include="DotRecast.Core" Version="2024.1.1" />
1010
<PackageVersion Include="DotRecast.Detour" Version="2024.2.1" />
1111
<PackageVersion Include="DotRecast.Recast" Version="2024.2.1" />
12-
<PackageVersion Include="DotRecast.Recast.Toolset" Version="2024.2.1" />
12+
<PackageVersion Include="DotRecast.Recast.Toolset" Version="2024.3.1" />
1313
<PackageVersion Include="FFmpeg.AutoGen" Version="3.4.0.2" />
1414
<PackageVersion Include="K4os.Compression.LZ4.Legacy" Version="1.3.6" />
1515
<PackageVersion Include="Microsoft.Management.Infrastructure" Version="3.0.0-preview.4" />
@@ -139,4 +139,4 @@
139139
<PackageVersion Include="Microsoft.CodeAnalysis.Analyzers" Version="3.3.4" PrivateAssets="all" />
140140
<PackageVersion Include="Microsoft.CodeAnalysis.CSharp" Version="4.7.0" PrivateAssets="all" />
141141
</ItemGroup>
142-
</Project>
142+
</Project>

‎sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Navigation/Components/BepuNavigationBoundingBoxComponent.cs‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,10 +8,10 @@
88
using Stride.Navigation;
99

1010
namespace Stride.BepuPhysics.Navigation.Components;
11-
[DefaultEntityComponentProcessor(typeof(RecastMeshProcessor), ExecutionMode = ExecutionMode.Runtime)]
11+
[DefaultEntityComponentProcessor(typeof(RecastDynamicMeshProcessor), ExecutionMode = ExecutionMode.Runtime)]
1212
[ComponentCategory("Bepu")]
1313
[DataContract("BepuNavigationBoundingBoxComponent")]
1414
public class BepuNavigationBoundingBoxComponent : NavigationBoundingBoxComponent
1515
{
16-
#warning right now this is unimplemented, but when we end up looking into a way to configure navigation, we should compare bounding-box based to unity's hierarchy/filter
16+
#warning right now this is unimplemented, but when we end up looking into a way to configure navigation, we should compare bounding-box based to unity's hierarchy/filter
1717
}

‎sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Navigation/Components/RecastNavigationComponent.cs‎

Lines changed: 14 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,8 @@
1-
using System.ComponentModel;
2-
using Stride.BepuPhysics.Navigation.Processors;
1+
using Stride.BepuPhysics.Navigation.Processors;
32
using Stride.Core;
43
using Stride.Core.Mathematics;
54
using Stride.Engine;
65
using Stride.Engine.Design;
7-
using Stride.Games;
86

97
namespace Stride.BepuPhysics.Navigation.Components;
108
[DataContract(nameof(RecastNavigationComponent))]
@@ -106,77 +104,35 @@ public virtual void Update(float deltaTime)
106104
// This allows the user to determine if the agent should stop moving if the path is no longer valid.
107105
if (IsMoving)
108106
{
109-
Move(deltaTime);
110-
Rotate();
107+
MoveAndRotate(deltaTime);
111108
}
112-
113109
}
114110

115-
private void Move(float deltaTime)
111+
private void MoveAndRotate(float deltaTime)
116112
{
117113
if (Path.Count == 0)
118114
{
119115
State = NavigationState.PathIsInvalid;
120116
return;
121117
}
122118

123-
var position = Entity.Transform.WorldMatrix.TranslationVector;
119+
var worldPosition = Entity.Transform.WorldMatrix.TranslationVector;
124120

125121
var nextWaypointPosition = Path[0];
126-
var distanceToWaypoint = Vector3.Distance(position, nextWaypointPosition);
127-
128-
// When the distance between the character and the next waypoint is large enough, move closer to the waypoint
129-
if (distanceToWaypoint > 0.1)
130-
{
131-
var direction = nextWaypointPosition - position;
132-
direction.Normalize();
133-
direction *= Speed * deltaTime;
134-
135-
position += direction;
136-
}
137-
else
138-
{
139-
if (Path.Count > 0)
140-
{
141-
// need to test if storing the index in Pathfinder would be faster than this.
142-
Path.RemoveAt(0);
143-
}
144-
}
145-
146-
Entity.Transform.Position = position;
147-
}
122+
var rotation = Quaternion.LookRotation(Vector3.Normalize(Path[0] - worldPosition), Vector3.UnitY);
123+
var targetPosition = Vector3.MoveTo(worldPosition, nextWaypointPosition, Speed * deltaTime);
124+
var scale = Entity.Transform.Scale;
148125

149-
private void Rotate()
150-
{
151-
if (Path.Count == 0)
126+
if (targetPosition == nextWaypointPosition && Path.Count > 0)
152127
{
153-
return;
128+
// need to test if storing the index in Pathfinder would be faster than this.
129+
Path.RemoveAt(0);
154130
}
155-
var position = Entity.Transform.WorldMatrix.TranslationVector;
156131

157-
float angle = (float)Math.Atan2(Path[0].Z - position.Z,
158-
Path[0].X - position.X);
132+
// Handle the scenario where the agent has a parent.
133+
Entity.Transform.Parent?.WorldToLocal(ref targetPosition, ref rotation, ref scale);
159134

160-
Entity.Transform.Rotation = Quaternion.RotationY(-angle);
135+
Entity.Transform.Position = targetPosition;
136+
Entity.Transform.Rotation = rotation;// Quaternion.Lerp(Entity.Transform.Rotation, rotation, .1f);
161137
}
162138
}
163-
164-
public enum NavigationState
165-
{
166-
/// <summary>
167-
/// Tells the <see cref="RecastNavigationProcessor"/> a plan needs to be queued. This is used internally to prevent multiple path calculations per frame.
168-
/// </summary>
169-
QueuePathPlanning,
170-
/// <summary>
171-
/// Tells the <see cref="RecastNavigationProcessor"/> to set a new path at the next available opportunity.
172-
/// </summary>
173-
PlanningPath,
174-
/// <summary>
175-
/// Tells the <see cref="RecastNavigationProcessor"/> the agent has a path.
176-
/// </summary>
177-
PathIsReady,
178-
/// <summary>
179-
/// Tells the <see cref="RecastNavigationProcessor"/> the agent does not have a valid path.
180-
/// </summary>
181-
PathIsInvalid,
182-
}

‎sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Navigation/Definitions/PathfindingSettings.cs‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
// Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net)
2+
// Distributed under the MIT license. See the LICENSE.md file in the project root for more information.
3+
4+
using Stride.Core;
5+
6+
namespace Stride.BepuPhysics.Navigation.Definitions;
7+
[DataContract()]
8+
[Display("Pathfinding Settings")]
9+
public class PathfindingSettings
10+
{
11+
public int MaxAllowedVisitedTiles { get; set; } = 16;
12+
}

‎sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Navigation/Definitions/RecastNavigationConfiguration.cs‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,10 @@ public class RecastNavigationConfiguration : Configuration
99
{
1010
[Display("Build Settings", Expand = ExpandRule.Never)]
1111
public BuildSettings BuildSettings { get; set; } = new();
12+
13+
[Display("Pathfinding Settings", Expand = ExpandRule.Never)]
14+
public PathfindingSettings PathfindingSettings { get; set; } = new();
15+
1216
/// <summary>
1317
/// Total thread count to use for pathfinding. Divided by 2 due to noticable stutter if all threads are used.
1418
/// </summary>

‎sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Navigation/NavigationState.cs‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
using System;
2+
using System.Collections.Generic;
3+
using System.Linq;
4+
using System.Text;
5+
using System.Threading.Tasks;
6+
7+
namespace Stride.BepuPhysics.Navigation;
8+
public enum NavigationState
9+
{
10+
/// <summary>
11+
/// Tells the <see cref="RecastNavigationProcessor"/> a plan needs to be queued. This is used internally to prevent multiple path calculations per frame.
12+
/// </summary>
13+
QueuePathPlanning,
14+
/// <summary>
15+
/// Tells the <see cref="RecastNavigationProcessor"/> to set a new path at the next available opportunity.
16+
/// </summary>
17+
PlanningPath,
18+
/// <summary>
19+
/// Tells the <see cref="RecastNavigationProcessor"/> the agent has a path.
20+
/// </summary>
21+
PathIsReady,
22+
/// <summary>
23+
/// Tells the <see cref="RecastNavigationProcessor"/> the agent does not have a valid path.
24+
/// </summary>
25+
PathIsInvalid,
26+
}

‎sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Navigation/Processors/RecastDynamicMeshProcessor.cs‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
// Copyright (c) .NET Foundation and Contributors (https://dotnetfoundation.org/ & https://stride3d.net)
2+
// Distributed under the MIT license. See the LICENSE.md file in the project root for more information.
3+
4+
using Stride.BepuPhysics.Navigation.Components;
5+
using Stride.Engine;
6+
7+
namespace Stride.BepuPhysics.Navigation.Processors;
8+
public class RecastDynamicMeshProcessor : EntityProcessor<BepuNavigationBoundingBoxComponent>
9+
{
10+
11+
}

sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Navigation/Processors/RecastMeshProcessor.cs renamed to sources/engine/Stride.BepuPhysics/Stride.BepuPhysics.Navigation/Processors/RecastMeshSystem.cs

Lines changed: 62 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -21,9 +21,10 @@
2121

2222
namespace Stride.BepuPhysics.Navigation.Processors;
2323

24-
public class RecastMeshProcessor : GameSystemBase
24+
public class RecastMeshSystem : GameSystemBase
2525
{
2626
public TimeSpan LastShapeCacheTime { get; private set; }
27+
public TimeSpan LastNavMeshBuildTime { get; private set; }
2728

2829
public const int MaxPolys = 256;
2930
public const int MaxSmooth = 2048;
@@ -40,7 +41,7 @@ public class RecastMeshProcessor : GameSystemBase
4041
private CancellationTokenSource _rebuildingTask = new();
4142
private RecastNavigationConfiguration _navSettings;
4243

43-
public RecastMeshProcessor(IServiceRegistry registry) : base(registry)
44+
public RecastMeshSystem(IServiceRegistry registry) : base(registry)
4445
{
4546
UpdateOrder = 20000;
4647
Enabled = true; //enabled by default
@@ -60,6 +61,8 @@ public override void Update(GameTime time)
6061
{
6162
_navMesh = _runningRebuild.Result;
6263
_runningRebuild = null;
64+
LastNavMeshBuildTime = _stopwatch.Elapsed;
65+
_stopwatch.Reset();
6366
}
6467
}
6568

@@ -83,7 +86,6 @@ public Task RebuildNavMesh()
8386
{
8487
var collidable = e.Current.Value;
8588

86-
#warning should we really ignore all bodies ?
8789
if (collidable is BodyComponent)
8890
continue;
8991

@@ -96,7 +98,6 @@ public Task RebuildNavMesh()
9698
asyncInput.Matrices.Add((collidable.Entity.Transform.WorldMatrix, shapeCount));
9799
}
98100

99-
_stopwatch.Stop();
100101
LastShapeCacheTime = _stopwatch.Elapsed;
101102
_stopwatch.Reset();
102103

@@ -125,6 +126,7 @@ public Task RebuildNavMesh()
125126
tileSize = _navSettings.BuildSettings.TileSize,
126127
};
127128
var token = _rebuildingTask.Token;
129+
_stopwatch.Start();
128130
var task = Task.Run(() => _navMesh = CreateNavMesh(settingsCopy, asyncInput, _navSettings.UsableThreadCount, token), token);
129131
_runningRebuild = task;
130132
return task;
@@ -236,10 +238,11 @@ private static DtNavMesh CreateNavMesh(RcNavMeshBuildSettings navSettings, Async
236238
option.tileHeight = navSettings.tileSize * navSettings.cellSize;
237239
option.maxTiles = GetMaxTiles(geom, navSettings.cellSize, navSettings.tileSize);
238240
option.maxPolys = GetMaxPolysPerTile(geom, navSettings.cellSize, navSettings.tileSize);
239-
DtNavMesh navMesh = new(option, navSettings.vertsPerPoly);
241+
DtNavMesh navMesh = new DtNavMesh();
242+
navMesh.Init(option, navSettings.vertsPerPoly);
240243
foreach (DtMeshData dtMeshData1 in dtMeshes)
241244
{
242-
navMesh.AddTile(dtMeshData1, 0, 0L);
245+
navMesh.AddTile(dtMeshData1, 0, 0L, out _);
243246
}
244247

245248
cancelToken.ThrowIfCancellationRequested();
@@ -261,12 +264,45 @@ private static int GetMaxPolysPerTile(IInputGeomProvider geom, float cellSize, i
261264

262265
private static int GetTileBits(IInputGeomProvider geom, float cellSize, int tileSize)
263266
{
264-
RcCommons.CalcGridSize(geom.GetMeshBoundsMin(), geom.GetMeshBoundsMax(), cellSize, out var sizeX, out var sizeZ);
267+
RcRecast.CalcGridSize(geom.GetMeshBoundsMin(), geom.GetMeshBoundsMax(), cellSize, out var sizeX, out var sizeZ);
265268
int num = (sizeX + tileSize - 1) / tileSize;
266269
int num2 = (sizeZ + tileSize - 1) / tileSize;
267270
return Math.Min(DtUtils.Ilog2(DtUtils.NextPow2(num * num2)), 14);
268271
}
269272

273+
/// <summary>
274+
/// Tries to find a path from the start to the end.
275+
/// </summary>
276+
/// <param name="start"></param>
277+
/// <param name="end"></param>
278+
/// <param name="polys"></param>
279+
/// <param name="smoothPath"></param>
280+
/// <param name="pathfindingSettings"></param>
281+
/// <returns></returns>
282+
public bool TryFindPath(Vector3 start, Vector3 end, ref List<long> polys, ref List<Vector3> smoothPath, PathfindingSettings pathfindingSettings)
283+
{
284+
if (_navMesh is null) return false;
285+
286+
var queryFilter = new DtQueryDefaultFilter();
287+
var dtNavMeshQuery = new DtNavMeshQuery(_navMesh);
288+
289+
dtNavMeshQuery.FindNearestPoly(start.ToDotRecastVector(), _polyPickExt, queryFilter, out long startRef, out _, out _);
290+
291+
dtNavMeshQuery.FindNearestPoly(end.ToDotRecastVector(), _polyPickExt, queryFilter, out long endRef, out _, out _);
292+
// find the nearest point on the navmesh to the start and end points
293+
var result = FindFollowPath(dtNavMeshQuery, startRef, endRef, start.ToDotRecastVector(), end.ToDotRecastVector(), queryFilter, true, ref polys, polys.Count, ref smoothPath, pathfindingSettings);
294+
295+
return result.Succeeded();
296+
}
297+
298+
/// <summary>
299+
/// Tries to find a path from the start to the end. This uses the default <see cref="PathfindingSettings"/>.
300+
/// </summary>
301+
/// <param name="start"></param>
302+
/// <param name="end"></param>
303+
/// <param name="polys"></param>
304+
/// <param name="smoothPath"></param>
305+
/// <returns></returns>
270306
public bool TryFindPath(Vector3 start, Vector3 end, ref List<long> polys, ref List<Vector3> smoothPath)
271307
{
272308
if (_navMesh is null) return false;
@@ -278,7 +314,7 @@ public bool TryFindPath(Vector3 start, Vector3 end, ref List<long> polys, ref Li
278314

279315
dtNavMeshQuery.FindNearestPoly(end.ToDotRecastVector(), _polyPickExt, queryFilter, out long endRef, out _, out _);
280316
// find the nearest point on the navmesh to the start and end points
281-
var result = FindFollowPath(dtNavMeshQuery, startRef, endRef, start.ToDotRecastVector(), end.ToDotRecastVector(), queryFilter, true, ref polys, ref smoothPath);
317+
var result = FindFollowPath(dtNavMeshQuery, startRef, endRef, start.ToDotRecastVector(), end.ToDotRecastVector(), queryFilter, true, ref polys, polys.Count, ref smoothPath, _navSettings.PathfindingSettings);
282318

283319
return result.Succeeded();
284320
}
@@ -308,7 +344,7 @@ public bool TryFindPath(Vector3 start, Vector3 end, ref List<long> polys, ref Li
308344
return verts;
309345
}
310346

311-
public static DtStatus FindFollowPath(DtNavMeshQuery navQuery, long startRef, long endRef, RcVec3f startPt, RcVec3f endPt, IDtQueryFilter filter, bool enableRaycast, ref List<long> polys, ref List<Vector3> smoothPath)
347+
public static DtStatus FindFollowPath(DtNavMeshQuery navQuery, long startRef, long endRef, RcVec3f startPt, RcVec3f endPt, IDtQueryFilter filter, bool enableRaycast, ref List<long> polys, int pathIterPolyCount, ref List<Vector3> smoothPath, PathfindingSettings navSettings)
312348
{
313349
if (startRef == 0 || endRef == 0)
314350
{
@@ -320,12 +356,15 @@ public static DtStatus FindFollowPath(DtNavMeshQuery navQuery, long startRef, lo
320356

321357
polys.Clear();
322358
smoothPath.Clear();
359+
pathIterPolyCount = 0;
323360

324361
var opt = new DtFindPathOption(enableRaycast ? DtFindPathOptions.DT_FINDPATH_ANY_ANGLE : 0, float.MaxValue);
325362
navQuery.FindPath(startRef, endRef, startPt, endPt, filter, ref polys, opt);
326363
if (0 >= polys.Count)
327364
return DtStatus.DT_FAILURE;
328365

366+
pathIterPolyCount = polys.Count;
367+
329368
// Iterate over the path to find smooth path on the detail mesh surface.
330369
navQuery.ClosestPointOnPoly(startRef, startPt, out var iterPos, out _);
331370
navQuery.ClosestPointOnPoly(polys[polys.Count - 1], endPt, out var targetPos, out _);
@@ -335,22 +374,27 @@ public static DtStatus FindFollowPath(DtNavMeshQuery navQuery, long startRef, lo
335374

336375
smoothPath.Clear();
337376
smoothPath.Add(iterPos.ToStrideVector());
338-
var visited = new List<long>();
377+
378+
Span<long> visited = stackalloc long[navSettings.MaxAllowedVisitedTiles];
379+
int nvisited = 0;
339380

340381
// Move towards target a small advancement at a time until target reached or
341382
// when ran out of memory to store the path.
342383
while (0 < polys.Count && smoothPath.Count < MaxSmooth)
343384
{
344385
// Find location to steer towards.
345386
if (!DtPathUtils.GetSteerTarget(navQuery, iterPos, targetPos, SLOP,
346-
polys, out var steerPos, out var steerPosFlag, out _))
387+
polys, polys.Count, out var steerPos, out var steerPosFlag, out var steerPosRef))
347388
{
348389
break;
349390
}
350391

351-
bool endOfPath = (steerPosFlag & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0;
352-
bool offMeshConnection = (steerPosFlag & DtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0;
353-
392+
bool endOfPath = (steerPosFlag & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0
393+
? true
394+
: false;
395+
bool offMeshConnection = (steerPosFlag & DtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0
396+
? true
397+
: false;
354398
// Find movement delta.
355399
RcVec3f delta = RcVec3f.Subtract(steerPos, iterPos);
356400
float len = MathF.Sqrt(RcVec3f.Dot(delta, delta));
@@ -364,15 +408,15 @@ public static DtStatus FindFollowPath(DtNavMeshQuery navQuery, long startRef, lo
364408
len = STEP_SIZE / len;
365409
}
366410

367-
RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len);
411+
RcVec3f moveTgt = RcVec.Mad(iterPos, delta, len);
368412

369413
// Move
370-
navQuery.MoveAlongSurface(polys[0], iterPos, moveTgt, filter, out var result, ref visited);
414+
navQuery.MoveAlongSurface(polys[0], iterPos, moveTgt, filter, out var result, visited, out nvisited, visited.Length);
371415

372416
iterPos = result;
373417

374-
polys = DtPathUtils.MergeCorridorStartMoved(polys, 0, 256, visited);
375-
polys = DtPathUtils.FixupShortcuts(polys, navQuery);
418+
pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(ref polys, pathIterPolyCount, MaxPolys, visited, nvisited);
419+
pathIterPolyCount = DtPathUtils.FixupShortcuts(ref polys, pathIterPolyCount, navQuery);
376420

377421
var status = navQuery.GetPolyHeight(polys[0], result, out var h);
378422
if (status.Succeeded())

0 commit comments

Comments
 (0)