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 RcVec3f m_target;
private List<long> m_path; private List<long> m_path;
private int m_npath;
private int m_maxPath; private int m_maxPath;
/** /**
@ -88,7 +89,8 @@ namespace DotRecast.Detour.Crowd
/// @return True if the initialization succeeded. /// @return True if the initialization succeeded.
public bool Init(int maxPath) public bool Init(int maxPath)
{ {
m_path = new List<long>(); m_path = new List<long>(maxPath);
m_npath = 0;
m_maxPath = maxPath; m_maxPath = maxPath;
return true; return true;
} }
@ -107,6 +109,7 @@ namespace DotRecast.Detour.Crowd
m_target = pos; m_target = pos;
m_path.Clear(); m_path.Clear();
m_path.Add(refs); m_path.Add(refs);
m_npath = 1;
} }
/** /**
@ -215,7 +218,7 @@ namespace DotRecast.Detour.Crowd
{ {
if (res.Count > 1 && t > 0.99f) 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) 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; return true;
} }
@ -274,6 +277,8 @@ namespace DotRecast.Detour.Crowd
// Prune path // Prune path
m_path = m_path.GetRange(npos, m_path.Count - npos); m_path = m_path.GetRange(npos, m_path.Count - npos);
m_npath -= npos;
refs[0] = prevRef; refs[0] = prevRef;
refs[1] = polyRef; 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); var status = navquery.MoveAlongSurface(m_path[0], m_pos, npos, filter, out var result, ref visited);
if (status.Succeeded()) 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. // Adjust the position to stay on top of the navmesh.
m_pos = result; 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); var status = navquery.MoveAlongSurface(m_path[^1], m_target, npos, filter, out var result, ref visited);
if (status.Succeeded()) 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? // TODO: should we do that?
// Adjust the position to stay on top of the navmesh. // Adjust the position to stay on top of the navmesh.
/* /*
@ -386,6 +392,7 @@ namespace DotRecast.Detour.Crowd
{ {
m_target = target; m_target = target;
m_path = new List<long>(path); m_path = new List<long>(path);
m_npath = path.Count;
} }
public void FixPathStart(long safeRef, RcVec3f safePos) public void FixPathStart(long safeRef, RcVec3f safePos)
@ -393,17 +400,19 @@ namespace DotRecast.Detour.Crowd
m_pos = safePos; m_pos = safePos;
if (m_path.Count < 3 && m_path.Count > 0) 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.Clear();
m_path.Add(safeRef); m_path.Add(safeRef);
m_path.Add(0L); m_path.Add(0L);
m_path.Add(p); m_path.Add(p);
m_npath = 3;
} }
else else
{ {
m_path.Clear(); m_path.Clear();
m_path.Add(safeRef); m_path.Add(safeRef);
m_path.Add(0L); m_path.Add(0L);
m_npath = 2;
} }
} }
@ -427,11 +436,13 @@ namespace DotRecast.Detour.Crowd
m_pos = RcVecUtils.Create(safePos); m_pos = RcVecUtils.Create(safePos);
m_path.Clear(); m_path.Clear();
m_path.Add(safeRef); m_path.Add(safeRef);
m_npath = 1;
} }
else if (n < m_path.Count) else if (n < m_path.Count)
{ {
// The path is partially usable. // The path is partially usable.
m_path = m_path.GetRange(0, n); m_path = m_path.GetRange(0, n);
m_npath = n;
} }
// Clamp target pos to last poly // 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;
using System.Collections.Generic; using System.Collections.Generic;
using System.Linq;
using DotRecast.Core.Numerics; using DotRecast.Core.Numerics;
namespace DotRecast.Detour namespace DotRecast.Detour
@ -88,11 +89,11 @@ namespace DotRecast.Detour
// +-S-+-T-+ // +-S-+-T-+
// |:::| | <-- the step can end up in here, resulting U-turn path. // |:::| | <-- 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 // Get connected polygons
@ -103,7 +104,7 @@ namespace DotRecast.Detour
var status = navQuery.GetAttachedNavMesh().GetTileAndPolyByRef(path[0], out var tile, out var poly); var status = navQuery.GetAttachedNavMesh().GetTileAndPolyByRef(path[0], out var tile, out var poly);
if (status.Failed()) if (status.Failed())
{ {
return path; return npath;
} }
@ -121,7 +122,7 @@ namespace DotRecast.Detour
// in the path, short cut to that polygon directly. // in the path, short cut to that polygon directly.
const int maxLookAhead = 6; const int maxLookAhead = 6;
int cut = 0; 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++) for (int j = 0; j < nneis; j++)
{ {
@ -137,20 +138,22 @@ namespace DotRecast.Detour
{ {
List<long> shortcut = new List<long>(); List<long> shortcut = new List<long>();
shortcut.Add(path[0]); shortcut.Add(path[0]);
shortcut.AddRange(path.GetRange(cut, path.Count - cut)); shortcut.AddRange(path.GetRange(cut, npath - cut));
return shortcut;
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 furthestPath = -1;
int furthestVisited = -1; int furthestVisited = -1;
// Find furthest common polygon. // Find furthest common polygon.
for (int i = path.Count - 1; i >= 0; --i) for (int i = npath - 1; i >= 0; --i)
{ {
bool found = false; bool found = false;
for (int j = visited.Count - 1; j >= 0; --j) 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 no intersection found just return current path.
if (furthestPath == -1 || furthestVisited == -1) if (furthestPath == -1 || furthestVisited == -1)
{ {
return path; return npath;
} }
// Concatenate paths. // Concatenate paths.
@ -185,17 +188,19 @@ namespace DotRecast.Detour
result.Add(visited[i]); result.Add(visited[i]);
} }
result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath)); result.AddRange(path.GetRange(furthestPath, npath - furthestPath));
return result;
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 furthestPath = -1;
int furthestVisited = -1; int furthestVisited = -1;
// Find furthest common polygon. // Find furthest common polygon.
for (int i = 0; i < path.Count; ++i) for (int i = 0; i < npath; ++i)
{ {
bool found = false; bool found = false;
for (int j = visited.Count - 1; j >= 0; --j) 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 no intersection found just return current path.
if (furthestPath == -1 || furthestVisited == -1) if (furthestPath == -1 || furthestVisited == -1)
{ {
return path; return npath;
} }
// Concatenate paths. // Concatenate paths.
List<long> result = path.GetRange(0, furthestPath); List<long> result = path.GetRange(0, furthestPath);
result.AddRange(visited.GetRange(furthestVisited, visited.Count - furthestVisited)); 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 furthestPath = -1;
int furthestVisited = -1; int furthestVisited = -1;
// Find furthest common polygon. // Find furthest common polygon.
for (int i = path.Count - 1; i >= 0; --i) for (int i = npath - 1; i >= 0; --i)
{ {
bool found = false; bool found = false;
for (int j = visited.Count - 1; j >= 0; --j) 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 no intersection found just return current path.
if (furthestPath == -1 || furthestVisited <= 0) if (furthestPath == -1 || furthestVisited <= 0)
{ {
return path; return npath;
} }
// Concatenate paths. // Concatenate paths.
// Adjust beginning of the buffer to include the visited. // Adjust beginning of the buffer to include the visited.
List<long> result = visited.GetRange(0, furthestVisited); List<long> result = visited.GetRange(0, furthestVisited);
result.AddRange(path.GetRange(furthestPath, path.Count - furthestPath)); result.AddRange(path.GetRange(furthestPath, npath - furthestPath));
return result;
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) if (_mode == RcTestNavmeshToolMode.PATHFIND_FOLLOW)
{ {
_tool.FindFollowPath(navMesh, navQuery, m_startRef, m_endRef, m_spos, m_epos, m_filter, _enableRaycast, _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) 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, 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) if (startRef == 0 || endRef == 0)
{ {
@ -36,27 +36,32 @@ namespace DotRecast.Recast.Toolset.Tools
smoothPath ??= new List<RcVec3f>(); smoothPath ??= new List<RcVec3f>();
pathIterPolys.Clear(); pathIterPolys.Clear();
pathIterPolyCount = 0;
smoothPath.Clear(); smoothPath.Clear();
var opt = new DtFindPathOption(enableRaycast ? DtFindPathOptions.DT_FINDPATH_ANY_ANGLE : 0, float.MaxValue); var opt = new DtFindPathOption(enableRaycast ? DtFindPathOptions.DT_FINDPATH_ANY_ANGLE : 0, float.MaxValue);
navQuery.FindPath(startRef, endRef, startPt, endPt, filter, ref pathIterPolys, opt); navQuery.FindPath(startRef, endRef, startPt, endPt, filter, ref pathIterPolys, opt);
if (0 >= pathIterPolys.Count) if (0 >= pathIterPolys.Count)
return DtStatus.DT_FAILURE; return DtStatus.DT_FAILURE;
pathIterPolyCount = pathIterPolys.Count;
// Iterate over the path to find smooth path on the detail mesh surface. // Iterate over the path to find smooth path on the detail mesh surface.
navQuery.ClosestPointOnPoly(startRef, startPt, out var iterPos, out var _); navQuery.ClosestPointOnPoly(startRef, startPt, out var iterPos, out var _);
navQuery.ClosestPointOnPoly(pathIterPolys[pathIterPolys.Count - 1], endPt, out var targetPos, out var _); navQuery.ClosestPointOnPoly(pathIterPolys[pathIterPolys.Count - 1], endPt, out var targetPos, out var _);
float STEP_SIZE = 0.5f; const float STEP_SIZE = 0.5f;
float SLOP = 0.01f; const float SLOP = 0.01f;
smoothPath.Clear(); smoothPath.Clear();
smoothPath.Add(iterPos); smoothPath.Add(iterPos);
var visited = new List<long>(); var visited = new List<long>();
// Move towards target a small advancement at a time until target reached or // Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path. // 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. // Find location to steer towards.
if (!DtPathUtils.GetSteerTarget(navQuery, iterPos, targetPos, SLOP, if (!DtPathUtils.GetSteerTarget(navQuery, iterPos, targetPos, SLOP,
@ -64,7 +69,7 @@ namespace DotRecast.Recast.Toolset.Tools
{ {
break; break;
} }
bool endOfPath = (steerPosFlag & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0 bool endOfPath = (steerPosFlag & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0
? true ? true
: false; : false;
@ -84,7 +89,7 @@ namespace DotRecast.Recast.Toolset.Tools
{ {
len = STEP_SIZE / len; len = STEP_SIZE / len;
} }
RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len); RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len);
// Move // Move
@ -92,8 +97,8 @@ namespace DotRecast.Recast.Toolset.Tools
iterPos = result; iterPos = result;
pathIterPolys = DtPathUtils.MergeCorridorStartMoved(pathIterPolys, pathIterPolys.Count, MAX_POLYS, visited); pathIterPolyCount = DtPathUtils.MergeCorridorStartMoved(ref pathIterPolys, pathIterPolyCount, MAX_POLYS, visited);
pathIterPolys = DtPathUtils.FixupShortcuts(pathIterPolys, pathIterPolys.Count, navQuery); pathIterPolyCount = DtPathUtils.FixupShortcuts(ref pathIterPolys, pathIterPolyCount, navQuery);
var status = navQuery.GetPolyHeight(pathIterPolys[0], result, out var h); var status = navQuery.GetPolyHeight(pathIterPolys[0], result, out var h);
if (status.Succeeded()) if (status.Succeeded())
@ -123,7 +128,7 @@ namespace DotRecast.Recast.Toolset.Tools
long prevRef = 0; long prevRef = 0;
long polyRef = pathIterPolys[0]; long polyRef = pathIterPolys[0];
int npos = 0; int npos = 0;
while (npos < pathIterPolys.Count && polyRef != steerPosRef) while (npos < pathIterPolyCount && polyRef != steerPosRef)
{ {
prevRef = polyRef; prevRef = polyRef;
polyRef = pathIterPolys[npos]; polyRef = pathIterPolys[npos];
@ -131,6 +136,7 @@ namespace DotRecast.Recast.Toolset.Tools
} }
pathIterPolys = pathIterPolys.GetRange(npos, pathIterPolys.Count - npos); pathIterPolys = pathIterPolys.GetRange(npos, pathIterPolys.Count - npos);
pathIterPolyCount -= npos;
// Handle the connection. // Handle the connection.
var status2 = navMesh.GetOffMeshConnectionPolyEndPoints(prevRef, polyRef, ref startPos, ref endPos); var status2 = navMesh.GetOffMeshConnectionPolyEndPoints(prevRef, polyRef, ref startPos, ref endPos);