bit
/
DotRecastNetSim
forked from mirror/DotRecast
40
0
Fork 1
Code Pull Requests Releases Activity

test2

This commit is contained in:
ikpil 2024-05-06 15:45:24 +09:00
parent 741200b559
commit fc673b2c25
4 changed files with 65 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/DotRecast.Detour.Crowd/DtPathCorridor.cs
View File

@ -34,6 +34,7 @@ namespace DotRecast.Detour.Crowd
private RcVec3f m_target;
private List<long> m_path;
private int m_npath;
private int m_maxPath;
/**
@ -88,7 +89,8 @@ namespace DotRecast.Detour.Crowd
/// @return True if the initialization succeeded.
public bool Init(int maxPath)
{
m_path = new List<long>();
m_path = new List<long>(maxPath);
m_npath = 0;
m_maxPath = maxPath;
return true;
}
@ -107,6 +109,7 @@ namespace DotRecast.Detour.Crowd
m_target = pos;
m_path.Clear();
m_path.Add(refs);
m_npath = 1;
}
/**
@ -215,7 +218,7 @@ namespace DotRecast.Detour.Crowd
{
if (res.Count > 1 && t > 0.99f)
{
m_path = DtPathUtils.MergeCorridorStartShortcut(m_path, m_path.Count, m_maxPath, res);
m_npath = DtPathUtils.MergeCorridorStartShortcut(ref m_path, m_npath, m_maxPath, res);
}
}
}
@ -247,7 +250,7 @@ namespace DotRecast.Detour.Crowd
if (status.Succeeded() && res.Count > 0)
{
m_path = DtPathUtils.MergeCorridorStartShortcut(m_path, m_path.Count, m_maxPath, res);
m_npath = DtPathUtils.MergeCorridorStartShortcut(ref m_path, m_npath, m_maxPath, res);
return true;
}
@ -274,6 +277,8 @@ namespace DotRecast.Detour.Crowd
// Prune path
m_path = m_path.GetRange(npos, m_path.Count - npos);
m_npath -= npos;
refs[0] = prevRef;
refs[1] = polyRef;
@ -316,7 +321,7 @@ namespace DotRecast.Detour.Crowd
var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, ref visited);
if (status.Succeeded())
{
m_path = DtPathUtils.MergeCorridorStartMoved(m_path, m_path.Count, m_maxPath, visited);
m_npath = DtPathUtils.MergeCorridorStartMoved(ref m_path, m_npath, m_maxPath, visited);
// Adjust the position to stay on top of the navmesh.
m_pos = result;
@ -358,7 +363,8 @@ namespace DotRecast.Detour.Crowd
var status = navquery.MoveAlongSurface(m_path[^1], m_target, npos, filter, out var result, ref visited);
if (status.Succeeded())
{
m_path = DtPathUtils.MergeCorridorEndMoved(m_path, m_path.Count, m_maxPath, visited);
m_npath = DtPathUtils.MergeCorridorEndMoved(ref m_path, m_npath, m_maxPath, visited);
// TODO: should we do that?
// Adjust the position to stay on top of the navmesh.
/*
@ -386,6 +392,7 @@ namespace DotRecast.Detour.Crowd
{
m_target = target;
m_path = new List<long>(path);
m_npath = path.Count;
}
public void FixPathStart(long safeRef, RcVec3f safePos)
@ -393,17 +400,19 @@ namespace DotRecast.Detour.Crowd
m_pos = safePos;
if (m_path.Count < 3 && m_path.Count > 0)
{
long p = m_path[m_path.Count - 1];
long p = m_path[m_npath - 1];
m_path.Clear();
m_path.Add(safeRef);
m_path.Add(0L);
m_path.Add(p);
m_npath = 3;
}
else
{
m_path.Clear();
m_path.Add(safeRef);
m_path.Add(0L);
m_npath = 2;
}
}
@ -427,11 +436,13 @@ namespace DotRecast.Detour.Crowd
m_pos = RcVecUtils.Create(safePos);
m_path.Clear();
m_path.Add(safeRef);
m_npath = 1;
}
else if (n < m_path.Count)
{
// The path is partially usable.
m_path = m_path.GetRange(0, n);
m_npath = n;
}
// Clamp target pos to last poly

53
src/DotRecast.Detour/DtPathUtils.cs
View File

@ -20,6 +20,7 @@ freely, subject to the following restrictions:
using System;
using System.Collections.Generic;
using System.Linq;
using DotRecast.Core.Numerics;
namespace DotRecast.Detour
@ -88,11 +89,11 @@ namespace DotRecast.Detour
// +-S-+-T-+
// |:::| | <-- the step can end up in here, resulting U-turn path.
// +---+---+
public static List<long> FixupShortcuts(List<long> path, int npath, DtNavMeshQuery navQuery)
public static int FixupShortcuts(ref List<long> path, int npath, DtNavMeshQuery navQuery)
{
if (path.Count < 3)
if (npath < 3)
{
return path;
return npath;
}
// Get connected polygons
@ -103,7 +104,7 @@ namespace DotRecast.Detour
var status = navQuery.GetAttachedNavMesh().GetTileAndPolyByRef(path[0], out var tile, out var poly);
if (status.Failed())
{
return path;
return npath;
}
@ -121,7 +122,7 @@ namespace DotRecast.Detour
// in the path, short cut to that polygon directly.
const int maxLookAhead = 6;
int cut = 0;
for (int i = Math.Min(maxLookAhead, path.Count) - 1; i > 1 && cut == 0; i--)
for (int i = Math.Min(maxLookAhead, npath) - 1; i > 1 && cut == 0; i--)
{
for (int j = 0; j < nneis; j++)
{
@ -137,20 +138,22 @@ namespace DotRecast.Detour
{
List<long> shortcut = new List<long>();
shortcut.Add(path[0]);
shortcut.AddRange(path.GetRange(cut, path.Count - cut));
return shortcut;
shortcut.AddRange(path.GetRange(cut, npath - cut));
path = shortcut;
return shortcut.Count;
}
return path;
return npath;
}
public static List<long> MergeCorridorStartMoved(List<long> path, int npath, int maxPath, List<long> visited)
public static int MergeCorridorStartMoved(ref List<long> path, int npath, int maxPath, List<long> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
// Find furthest common polygon.
for (int i = path.Count - 1; i >= 0; --i)
for (int i = npath - 1; i >= 0; --i)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; --j)
@ -172,7 +175,7 @@ namespace DotRecast.Detour
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited == -1)
{
return path;
return npath;
}
// Concatenate paths.
@ -185,17 +188,19 @@ namespace DotRecast.Detour
result.Add(visited[i]);
}
result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath));
return result;
result.AddRange(path.GetRange(furthestPath, npath - furthestPath));
path = result;
return result.Count;
}
public static List<long> MergeCorridorEndMoved(List<long> path, int npath, int maxPath, List<long> visited)
public static int MergeCorridorEndMoved(ref List<long> path, int npath, int maxPath, List<long> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
// Find furthest common polygon.
for (int i = 0; i < path.Count; ++i)
for (int i = 0; i < npath; ++i)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; --j)
@ -217,22 +222,24 @@ namespace DotRecast.Detour
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited == -1)
{
return path;
return npath;
}
// Concatenate paths.
List<long> result = path.GetRange(0, furthestPath);
result.AddRange(visited.GetRange(furthestVisited, visited.Count - furthestVisited));
return result;
path = result;
return result.Count;
}
public static List<long> MergeCorridorStartShortcut(List<long> path, int npath, int maxPath, List<long> visited)
public static int MergeCorridorStartShortcut(ref List<long> path, int npath, int maxPath, List<long> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
// Find furthest common polygon.
for (int i = path.Count - 1; i >= 0; --i)
for (int i = npath - 1; i >= 0; --i)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; --j)
@ -254,15 +261,17 @@ namespace DotRecast.Detour
// If no intersection found just return current path.
if (furthestPath == -1 || furthestVisited <= 0)
{
return path;
return npath;
}
// Concatenate paths.
// Adjust beginning of the buffer to include the visited.
List<long> result = visited.GetRange(0, furthestVisited);
result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath));
return result;
result.AddRange(path.GetRange(furthestPath, npath - furthestPath));
path = result;
return result.Count;
}
}
}

2
src/DotRecast.Recast.Demo/Tools/TestNavmeshSampleTool.cs
View File

@ -661,7 +661,7 @@ public class TestNavmeshSampleTool : ISampleTool
if (_mode == RcTestNavmeshToolMode.PATHFIND_FOLLOW)
{
_tool.FindFollowPath(navMesh, navQuery, m_startRef, m_endRef, m_spos, m_epos, m_filter, _enableRaycast,
ref m_polys, ref m_smoothPath);
ref m_polys, m_polys?.Count ?? 0, ref m_smoothPath);
}
else if (_mode == RcTestNavmeshToolMode.PATHFIND_STRAIGHT)
{

26
src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
View File

@ -22,7 +22,7 @@ namespace DotRecast.Recast.Toolset.Tools
}
public DtStatus FindFollowPath(DtNavMesh navMesh, DtNavMeshQuery navQuery, long startRef, long endRef, RcVec3f startPt, RcVec3f endPt, IDtQueryFilter filter, bool enableRaycast,
ref List<long> pathIterPolys, ref List<RcVec3f> smoothPath)
ref List<long> pathIterPolys, int pathIterPolyCount, ref List<RcVec3f> smoothPath)
{
if (startRef == 0 || endRef == 0)
{
@ -36,27 +36,32 @@ namespace DotRecast.Recast.Toolset.Tools
smoothPath ??= new List<RcVec3f>();
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 _);
float STEP_SIZE = 0.5f;
float SLOP = 0.01f;
const float STEP_SIZE = 0.5f;
const float SLOP = 0.01f;
smoothPath.Clear();
smoothPath.Add(iterPos);
var visited = new List<long>();
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
while (0 < pathIterPolys.Count && smoothPath.Count < MAX_SMOOTH)
while (0 < pathIterPolyCount && smoothPath.Count < MAX_SMOOTH)
{
// Find location to steer towards.
if (!DtPathUtils.GetSteerTarget(navQuery, iterPos, targetPos, SLOP,
@ -64,7 +69,7 @@ namespace DotRecast.Recast.Toolset.Tools
{
break;
}
bool endOfPath = (steerPosFlag & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0
? true
: false;
@ -84,7 +89,7 @@ namespace DotRecast.Recast.Toolset.Tools
{
len = STEP_SIZE / len;
}
RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len);
// Move
@ -92,8 +97,8 @@ namespace DotRecast.Recast.Toolset.Tools
iterPos = result;
pathIterPolys = DtPathUtils.MergeCorridorStartMoved(pathIterPolys, pathIterPolys.Count, MAX_POLYS, visited);
pathIterPolys = DtPathUtils.FixupShortcuts(pathIterPolys, pathIterPolys.Count, navQuery);
pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(ref pathIterPolys, pathIterPolyCount, MAX_POLYS, visited);
pathIterPolyCount = DtPathUtils.FixupShortcuts(ref pathIterPolys, pathIterPolyCount, navQuery);
var status = navQuery.GetPolyHeight(pathIterPolys[0], result, out var h);
if (status.Succeeded())
@ -123,7 +128,7 @@ namespace DotRecast.Recast.Toolset.Tools
long prevRef = 0;
long polyRef = pathIterPolys[0];
int npos = 0;
while (npos < pathIterPolys.Count && polyRef != steerPosRef)
while (npos < pathIterPolyCount && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = pathIterPolys[npos];
@ -131,6 +136,7 @@ namespace DotRecast.Recast.Toolset.Tools
}
pathIterPolys = pathIterPolys.GetRange(npos, pathIterPolys.Count - npos);
pathIterPolyCount -= npos;
// Handle the connection.
var status2 = navMesh.GetOffMeshConnectionPolyEndPoints(prevRef, polyRef, ref startPos, ref endPos);