Skip to content

Commit

Permalink
SOH #41
Browse files Browse the repository at this point in the history
  • Loading branch information
@ikpil
ikpil committed May 8, 2024
1 parent 59849e1 commit e926c23
Show file tree
Hide file tree
Showing 5 changed files with 45 additions and 27 deletions.
15 changes: 9 additions & 6 deletions src/DotRecast.Detour.Crowd/DtPathCorridor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -317,11 +317,12 @@ public bool MoveOverOffmeshConnection(long offMeshConRef, long[] refs, ref RcVec
public bool MovePosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFilter filter)
{
// Move along navmesh and update new position.
var visited = new List<long>();
var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, ref visited);
const int MAX_VISITED = 16;
Span<long> visited = stackalloc long[MAX_VISITED];
var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, visited, out var nvisited, MAX_VISITED);
if (status.Succeeded())
{
m_npath = DtPathUtils.MergeCorridorStartMoved(ref m_path, m_npath, m_maxPath, visited, visited.Count);
m_npath = DtPathUtils.MergeCorridorStartMoved(ref m_path, m_npath, m_maxPath, visited, nvisited);

// Adjust the position to stay on top of the navmesh.
m_pos = result;
Expand Down Expand Up @@ -359,11 +360,13 @@ public bool MovePosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFilter f
public bool MoveTargetPosition(RcVec3f npos, DtNavMeshQuery navquery, IDtQueryFilter filter)
{
// Move along navmesh and update new position.
var visited = new List<long>();
var status = navquery.MoveAlongSurface(m_path[^1], m_target, npos, filter, out var result, ref visited);
const int MAX_VISITED = 16;
Span<long> visited = stackalloc long[MAX_VISITED];
int nvisited = 0;
var status = navquery.MoveAlongSurface(m_path[^1], m_target, npos, filter, out var result, visited, out nvisited, MAX_VISITED);
if (status.Succeeded())
{
m_npath = DtPathUtils.MergeCorridorEndMoved(ref m_path, m_npath, m_maxPath, visited, visited.Count);
m_npath = DtPathUtils.MergeCorridorEndMoved(ref m_path, m_npath, m_maxPath, visited, nvisited);

// TODO: should we do that?
// Adjust the position to stay on top of the navmesh.
Expand Down
19 changes: 14 additions & 5 deletions src/DotRecast.Detour/DtNavMeshQuery.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1790,12 +1790,11 @@ public virtual DtStatus FindStraightPath(RcVec3f startPos, RcVec3f endPos, List<
/// @returns The status flags for the query.
public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos,
IDtQueryFilter filter,
out RcVec3f resultPos, ref List<long> visited)
out RcVec3f resultPos, Span<long> visited, out int visitedCount, int maxVisitedSize)
{
resultPos = RcVec3f.Zero;

if (null != visited)
visited.Clear();
visitedCount = 0;

// Validate input
if (!m_nav.IsValidPolyRef(startRef) || !startPos.IsFinite()
Expand All @@ -1804,6 +1803,8 @@ public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}

DtStatus status = DtStatus.DT_SUCCESS;

m_tinyNodePool.Clear();

DtNode startNode = m_tinyNodePool.GetNode(startRef);
Expand Down Expand Up @@ -1938,6 +1939,7 @@ public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos
}
}

int n = 0;
if (bestNode != null)
{
// Reverse the path.
Expand All @@ -1955,14 +1957,21 @@ public DtStatus MoveAlongSurface(long startRef, RcVec3f startPos, RcVec3f endPos
node = prev;
do
{
visited.Add(node.id);
visited[n++] = node.id;
if (n >= maxVisitedSize)
{
status |= DtStatus.DT_BUFFER_TOO_SMALL;;
break;
}

node = m_tinyNodePool.GetNodeAtIdx(node.pidx);
} while (node != null);
}

resultPos = bestPos;
visitedCount = n;

return DtStatus.DT_SUCCESS;
return status;
}

protected DtStatus GetPortalPoints(long from, long to, out RcVec3f left, out RcVec3f right, out int fromType, out int toType)
Expand Down
9 changes: 6 additions & 3 deletions src/DotRecast.Detour/DtPathUtils.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ public static int FixupShortcuts(ref List<long> path, int npath, DtNavMeshQuery
return npath;
}

public static int MergeCorridorStartMoved(ref List<long> path, int npath, int maxPath, List<long> visited, int nvisited)
public static int MergeCorridorStartMoved(ref List<long> path, int npath, int maxPath, Span<long> visited, int nvisited)
{
int furthestPath = -1;
int furthestVisited = -1;
Expand Down Expand Up @@ -193,7 +193,7 @@ public static int MergeCorridorStartMoved(ref List<long> path, int npath, int ma
return result.Count;
}

public static int MergeCorridorEndMoved(ref List<long> path, int npath, int maxPath, List<long> visited, int nvisited)
public static int MergeCorridorEndMoved(ref List<long> path, int npath, int maxPath, Span<long> visited, int nvisited)
{
int furthestPath = -1;
int furthestVisited = -1;
Expand Down Expand Up @@ -226,7 +226,10 @@ public static int MergeCorridorEndMoved(ref List<long> path, int npath, int maxP

// Concatenate paths.
List<long> result = path.GetRange(0, furthestPath);
result.AddRange(visited.GetRange(furthestVisited, nvisited - furthestVisited));
foreach (var v in visited.Slice(furthestVisited, nvisited - furthestVisited))
{
result.Add(v);
}

path = result;
return result.Count;
Expand Down
20 changes: 11 additions & 9 deletions src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,16 +37,16 @@ public DtStatus FindFollowPath(DtNavMesh navMesh, DtNavMeshQuery navQuery, long

pathIterPolys.Clear();
pathIterPolyCount = 0;

smoothPath.Clear();

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

pathIterPolyCount = pathIterPolys.Count;

// Iterate over the path to find smooth path on the detail mesh surface.
navQuery.ClosestPointOnPoly(startRef, startPt, out var iterPos, out var _);
navQuery.ClosestPointOnPoly(pathIterPolys[pathIterPolys.Count - 1], endPt, out var targetPos, out var _);
Expand All @@ -56,8 +56,10 @@ public DtStatus FindFollowPath(DtNavMesh navMesh, DtNavMeshQuery navQuery, long

smoothPath.Clear();
smoothPath.Add(iterPos);
var visited = new List<long>();


Span<long> visited = stackalloc long[16];
int nvisited = 0;


// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
Expand All @@ -69,7 +71,7 @@ public DtStatus FindFollowPath(DtNavMesh navMesh, DtNavMeshQuery navQuery, long
{
break;
}

bool endOfPath = (steerPosFlag & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0
? true
: false;
Expand All @@ -89,15 +91,15 @@ public DtStatus FindFollowPath(DtNavMesh navMesh, DtNavMeshQuery navQuery, long
{
len = STEP_SIZE / len;
}

RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len);

// Move
navQuery.MoveAlongSurface(pathIterPolys[0], iterPos, moveTgt, filter, out var result, ref visited);
navQuery.MoveAlongSurface(pathIterPolys[0], iterPos, moveTgt, filter, out var result, visited, out nvisited, 16);

iterPos = result;

pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(ref pathIterPolys, pathIterPolyCount, MAX_POLYS, visited, visited.Count);
pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(ref pathIterPolys, pathIterPolyCount, MAX_POLYS, visited, nvisited);
pathIterPolyCount = DtPathUtils.FixupShortcuts(ref pathIterPolys, pathIterPolyCount, navQuery);

var status = navQuery.GetPolyHeight(pathIterPolys[0], result, out var h);
Expand Down
9 changes: 5 additions & 4 deletions test/DotRecast.Detour.Test/MoveAlongSurfaceTest.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,13 +16,13 @@ misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

using System;
using System.Collections.Generic;
using DotRecast.Core.Numerics;
using NUnit.Framework;

namespace DotRecast.Detour.Test;


public class MoveAlongSurfaceTest : AbstractDetourTest
{
private static readonly long[][] VISITED =
Expand Down Expand Up @@ -69,20 +69,21 @@ public class MoveAlongSurfaceTest : AbstractDetourTest
public void TestMoveAlongSurface()
{
IDtQueryFilter filter = new DtQueryDefaultFilter();
var visited = new List<long>();
const int MAX_VISITED = 32;
Span<long> visited = stackalloc long[MAX_VISITED];
for (int i = 0; i < startRefs.Length; i++)
{
long startRef = startRefs[i];
RcVec3f startPos = startPoss[i];
RcVec3f endPos = endPoss[i];
var status = query.MoveAlongSurface(startRef, startPos, endPos, filter, out var result, ref visited);
var status = query.MoveAlongSurface(startRef, startPos, endPos, filter, out var result, visited, out var nvisited, MAX_VISITED);
Assert.That(status.Succeeded(), Is.True);

Assert.That(result.X, Is.EqualTo(POSITION[i].X).Within(0.01f));
Assert.That(result.Y, Is.EqualTo(POSITION[i].Y).Within(0.01f));
Assert.That(result.Z, Is.EqualTo(POSITION[i].Z).Within(0.01f));

Assert.That(visited.Count, Is.EqualTo(VISITED[i].Length));
Assert.That(nvisited, Is.EqualTo(VISITED[i].Length));
for (int j = 0; j < 3; j++)
{
Assert.That(visited[j], Is.EqualTo(VISITED[i][j]));
Expand Down

0 comments on commit e926c23

Please sign in to comment.