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

change FindCorners

This commit is contained in:
ikpil 2023-06-20 01:36:03 +09:00
parent 6433ce8d95
commit 9c42e58e86
10 changed files with 102 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/DotRecast.Detour.Crowd/DtCrowd.cs
View File

@ -942,7 +942,7 @@ namespace DotRecast.Detour.Crowd
}
// Find corners for steering
ag.corners = ag.corridor.FindCorners(DT_CROWDAGENT_MAX_CORNERS, _navQuery, _filters[ag.option.queryFilterType]);
ag.corridor.FindCorners(ref ag.corners, DT_CROWDAGENT_MAX_CORNERS, _navQuery, _filters[ag.option.queryFilterType]);
// Check to see if the corner after the next corner is directly visible,
// and short cut to there.

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

@ -234,16 +234,14 @@ namespace DotRecast.Detour.Crowd
* @param[in] navquery The query object used to build the corridor.
* @return Corners
*/
public List<StraightPathItem> FindCorners(int maxCorners, DtNavMeshQuery navquery, IDtQueryFilter filter)
public int FindCorners(ref List<StraightPathItem> corners, int maxCorners, DtNavMeshQuery navquery, IDtQueryFilter filter)
{
List<StraightPathItem> path = new List<StraightPathItem>();
Result<List<StraightPathItem>> result = navquery.FindStraightPath(m_pos, m_target, m_path, maxCorners, 0);
var result = navquery.FindStraightPath(m_pos, m_target, m_path, ref corners, maxCorners, 0);
if (result.Succeeded())
{
path = result.result;
// Prune points in the beginning of the path which are too close.
int start = 0;
foreach (StraightPathItem spi in path)
foreach (StraightPathItem spi in corners)
{
if ((spi.GetFlags() & DtNavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0
|| RcVec3f.Dist2DSqr(spi.GetPos(), m_pos) > MIN_TARGET_DIST)
@ -254,11 +252,11 @@ namespace DotRecast.Detour.Crowd
start++;
}
int end = path.Count;
int end = corners.Count;
// Prune points after an off-mesh connection.
for (int i = start; i < path.Count; i++)
for (int i = start; i < corners.Count; i++)
{
StraightPathItem spi = path[i];
StraightPathItem spi = corners[i];
if ((spi.GetFlags() & DtNavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0)
{
end = i + 1;
@ -266,10 +264,10 @@ namespace DotRecast.Detour.Crowd
}
}
path = path.GetRange(start, end - start);
corners = corners.GetRange(start, end - start);
}
return path;
return corners.Count;
}
/**

89
src/DotRecast.Detour/DtNavMeshQuery.cs
View File

@ -1422,7 +1422,7 @@ namespace DotRecast.Detour
return Results.Of(status, path);
}
protected DtStatus AppendVertex(RcVec3f pos, int flags, long refs, List<StraightPathItem> straightPath,
protected DtStatus AppendVertex(RcVec3f pos, int flags, long refs, ref List<StraightPathItem> straightPath,
int maxStraightPath)
{
if (straightPath.Count > 0 && DetourCommon.VEqual(straightPath[straightPath.Count - 1].pos, pos))
@ -1450,7 +1450,7 @@ namespace DotRecast.Detour
}
protected DtStatus AppendPortals(int startIdx, int endIdx, RcVec3f endPos, List<long> path,
List<StraightPathItem> straightPath, int maxStraightPath, int options)
ref List<StraightPathItem> straightPath, int maxStraightPath, int options)
{
var startPos = straightPath[straightPath.Count - 1].pos;
// Append or update last vertex
@ -1491,7 +1491,7 @@ namespace DotRecast.Detour
if (DetourCommon.IntersectSegSeg2D(startPos, endPos, left, right, out var _, out var t))
{
var pt = RcVec3f.Lerp(left, right, t);
stat = AppendVertex(pt, 0, path[i + 1], straightPath, maxStraightPath);
stat = AppendVertex(pt, 0, path[i + 1], ref straightPath, maxStraightPath);
if (!stat.InProgress())
{
return stat;
@ -1503,58 +1503,61 @@ namespace DotRecast.Detour
}
/// @par
/// Finds the straight path from the start to the end position within the polygon corridor.
///
///
/// This method peforms what is often called 'string pulling'.
///
/// The start position is clamped to the first polygon in the path, and the
/// end position is clamped to the last. So the start and end positions should
/// The start position is clamped to the first polygon in the path, and the
/// end position is clamped to the last. So the start and end positions should
/// normally be within or very near the first and last polygons respectively.
///
/// The returned polygon references represent the reference id of the polygon
/// that is entered at the associated path position. The reference id associated
/// with the end point will always be zero. This allows, for example, matching
/// The returned polygon references represent the reference id of the polygon
/// that is entered at the associated path position. The reference id associated
/// with the end point will always be zero. This allows, for example, matching
/// off-mesh link points to their representative polygons.
///
/// If the provided result buffers are too small for the entire result set,
/// they will be filled as far as possible from the start toward the end
/// If the provided result buffers are too small for the entire result set,
/// they will be filled as far as possible from the start toward the end
/// position.
///
/// @param[in] startPos Path start position. [(x, y, z)]
/// @param[in] endPos Path end position. [(x, y, z)]
/// @param[in] path An array of polygon references that represent the path corridor.
/// @param[out] straightPath Points describing the straight path. [(x, y, z) * @p straightPathCount].
/// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0]
/// @param[in] options Query options. (see: #dtStraightPathOptions)
/// Finds the straight path from the start to the end position within the polygon corridor.
/// @param[in] startPos Path start position. [(x, y, z)]
/// @param[in] endPos Path end position. [(x, y, z)]
/// @param[in] path An array of polygon references that represent the path corridor.
/// @param[in] pathSize The number of polygons in the @p path array.
/// @param[out] straightPath Points describing the straight path. [(x, y, z) * @p straightPathCount].
/// @param[in] maxStraightPath The maximum number of points the straight path arrays can hold. [Limit: > 0]
/// @param[in] options Query options. (see: #dtStraightPathOptions)
/// @returns The status flags for the query.
public virtual Result<List<StraightPathItem>> FindStraightPath(RcVec3f startPos, RcVec3f endPos, List<long> path,
public virtual DtStatus FindStraightPath(RcVec3f startPos, RcVec3f endPos, List<long> path,
ref List<StraightPathItem> straightPath,
int maxStraightPath, int options)
{
List<StraightPathItem> straightPath = new List<StraightPathItem>();
straightPath.Clear();
if (!RcVec3f.IsFinite(startPos) || !RcVec3f.IsFinite(endPos)
|| null == path || 0 == path.Count || path[0] == 0 || maxStraightPath <= 0)
{
return Results.InvalidParam<List<StraightPathItem>>();
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
// TODO: Should this be callers responsibility?
var closestStartPosRes = ClosestPointOnPolyBoundary(path[0], startPos, out var closestStartPos);
if (closestStartPosRes.Failed())
{
return Results.InvalidParam<List<StraightPathItem>>("Cannot find start position");
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
var closestEndPosRes = ClosestPointOnPolyBoundary(path[path.Count - 1], endPos, out var closestEndPos);
if (closestEndPosRes.Failed())
{
return Results.InvalidParam<List<StraightPathItem>>("Cannot find end position");
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
// Add start point.
DtStatus stat = AppendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], straightPath, maxStraightPath);
DtStatus stat = AppendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], ref straightPath, maxStraightPath);
if (!stat.InProgress())
{
return Results.Success(straightPath);
return stat;
}
if (path.Count > 1)
@ -1591,19 +1594,20 @@ namespace DotRecast.Detour
var cpStatus = ClosestPointOnPolyBoundary(path[i], endPos, out closestEndPos);
if (cpStatus.Failed())
{
return Results.InvalidParam<List<StraightPathItem>>();
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
// Append portals along the current straight path segment.
if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
{
// Ignore status return value as we're just about to return anyway.
AppendPortals(apexIndex, i, closestEndPos, path, straightPath, maxStraightPath, options);
AppendPortals(apexIndex, i, closestEndPos, path, ref straightPath, maxStraightPath, options);
}
// Ignore status return value as we're just about to return anyway.
AppendVertex(closestEndPos, 0, path[i], straightPath, maxStraightPath);
return Results.Success(straightPath);
AppendVertex(closestEndPos, 0, path[i], ref straightPath, maxStraightPath);
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM | (straightPath.Count >= maxStraightPath ? DtStatus.DT_BUFFER_TOO_SMALL : DtStatus.DT_STATUS_NOTHING);
}
// If starting really close the portal, advance.
@ -1639,11 +1643,10 @@ namespace DotRecast.Detour
// Append portals along the current straight path segment.
if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
{
stat = AppendPortals(apexIndex, leftIndex, portalLeft, path, straightPath, maxStraightPath,
options);
stat = AppendPortals(apexIndex, leftIndex, portalLeft, path, ref straightPath, maxStraightPath, options);
if (!stat.InProgress())
{
return Results.Success(straightPath);
return stat;
}
}
@ -1663,10 +1666,10 @@ namespace DotRecast.Detour
long refs = leftPolyRef;
// Append or update vertex
stat = AppendVertex(portalApex, flags, refs, straightPath, maxStraightPath);
stat = AppendVertex(portalApex, flags, refs, ref straightPath, maxStraightPath);
if (!stat.InProgress())
{
return Results.Success(straightPath);
return stat;
}
portalLeft = portalApex;
@ -1696,11 +1699,10 @@ namespace DotRecast.Detour
// Append portals along the current straight path segment.
if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
{
stat = AppendPortals(apexIndex, rightIndex, portalRight, path, straightPath,
maxStraightPath, options);
stat = AppendPortals(apexIndex, rightIndex, portalRight, path, ref straightPath, maxStraightPath, options);
if (!stat.InProgress())
{
return Results.Success(straightPath);
return stat;
}
}
@ -1720,10 +1722,10 @@ namespace DotRecast.Detour
long refs = rightPolyRef;
// Append or update vertex
stat = AppendVertex(portalApex, flags, refs, straightPath, maxStraightPath);
stat = AppendVertex(portalApex, flags, refs, ref straightPath, maxStraightPath);
if (!stat.InProgress())
{
return Results.Success(straightPath);
return stat;
}
portalLeft = portalApex;
@ -1742,18 +1744,17 @@ namespace DotRecast.Detour
// Append portals along the current straight path segment.
if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
{
stat = AppendPortals(apexIndex, path.Count - 1, closestEndPos, path, straightPath, maxStraightPath,
options);
stat = AppendPortals(apexIndex, path.Count - 1, closestEndPos, path, ref straightPath, maxStraightPath, options);
if (!stat.InProgress())
{
return Results.Success(straightPath);
return stat;
}
}
}
// Ignore status return value as we're just about to return anyway.
AppendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, straightPath, maxStraightPath);
return Results.Success(straightPath);
AppendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0, ref straightPath, maxStraightPath);
return DtStatus.DT_SUCCSESS | (straightPath.Count >= maxStraightPath ? DtStatus.DT_BUFFER_TOO_SMALL : DtStatus.DT_STATUS_NOTHING);
}
/// @par

3
src/DotRecast.Detour/DtStatus.cs
View File

@ -31,6 +31,7 @@ namespace DotRecast.Detour
// Detail information for status.
public static readonly DtStatus DT_STATUS_DETAIL_MASK = new DtStatus(0x0ffffff);
public static readonly DtStatus DT_STATUS_NOTHING = new DtStatus(0); // nothing
public static readonly DtStatus DT_WRONG_MAGIC = new DtStatus(1 << 0); // Input data is not recognized.
public static readonly DtStatus DT_WRONG_VERSION = new DtStatus(1 << 1); // Input data is in wrong version.
public static readonly DtStatus DT_OUT_OF_MEMORY = new DtStatus(1 << 2); // Operation ran out of memory.
@ -82,7 +83,7 @@ namespace DotRecast.Detour
{
return new DtStatus(left.Value | right.Value);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static DtStatus operator &(DtStatus left, DtStatus right)
{

4
src/DotRecast.Detour/PathUtils.cs
View File

@ -34,13 +34,13 @@ namespace DotRecast.Detour
float minTargetDist, List<long> path)
{
// Find steer target.
Result<List<StraightPathItem>> result = navQuery.FindStraightPath(startPos, endPos, path, MAX_STEER_POINTS, 0);
var straightPath = new List<StraightPathItem>();
var result = navQuery.FindStraightPath(startPos, endPos, path, ref straightPath, MAX_STEER_POINTS, 0);
if (result.Failed())
{
return null;
}
List<StraightPathItem> straightPath = result.result;
float[] steerPoints = new float[straightPath.Count * 3];
for (int i = 0; i < straightPath.Count; i++)
{

2
src/DotRecast.Detour/StraightPathItem.cs
View File

@ -22,7 +22,7 @@ using DotRecast.Core;
namespace DotRecast.Detour
{
//TODO: (PP) Add comments
//TODO: (PP) Add comments
public class StraightPathItem
{
public RcVec3f pos;

18
src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs
View File

@ -63,7 +63,7 @@ public class TestNavmeshTool : IRcTool
{
return _impl;
}
public void OnSampleChanged()
{
// ..
@ -277,7 +277,7 @@ public class TestNavmeshTool : IRcTool
// Reached off-mesh connection.
RcVec3f startPos = RcVec3f.Zero;
RcVec3f endPos = RcVec3f.Zero;
// Advance the path up to and over the off-mesh connection.
long prevRef = 0;
long polyRef = polys[0];
@ -345,8 +345,7 @@ public class TestNavmeshTool : IRcTool
}
}
m_straightPath = m_navQuery.FindStraightPath(m_spos, epos, m_polys, MAX_POLYS,
m_straightPathOptions).result;
m_navQuery.FindStraightPath(m_spos, epos, m_polys, ref m_straightPath, MAX_POLYS, m_straightPathOptions);
}
}
else
@ -839,7 +838,7 @@ public class TestNavmeshTool : IRcTool
var result = _impl.GetSample()
.GetNavMeshQuery()
.GetPolyWallSegments(m_polys[i], false, m_filter, ref segmentVerts, ref segmentRefs);
if (result.Succeeded())
{
dd.Begin(LINES, 2.0f);
@ -999,14 +998,7 @@ public class TestNavmeshTool : IRcTool
}
}
{
Result<List<StraightPathItem>> result = m_navQuery.FindStraightPath(m_spos, epos, m_polys,
MAX_POLYS, DtNavMeshQuery.DT_STRAIGHTPATH_ALL_CROSSINGS);
if (result.Succeeded())
{
m_straightPath = result.result;
}
}
m_navQuery.FindStraightPath(m_spos, epos, m_polys, ref m_straightPath, MAX_POLYS, DtNavMeshQuery.DT_STRAIGHTPATH_ALL_CROSSINGS);
}
m_pathFindStatus = DtStatus.DT_FAILURE;

50
test/DotRecast.Detour.Crowd.Test/PathCorridorTest.cs
View File

@ -44,16 +44,24 @@ public class PathCorridorTest
straightPath.Add(new StraightPathItem(RcVec3f.Of(12, 20, 30.00002f), 0, 0));
straightPath.Add(new StraightPathItem(RcVec3f.Of(11f, 21, 32f), 0, 0));
straightPath.Add(new StraightPathItem(RcVec3f.Of(11f, 21, 32f), 0, 0));
Result<List<StraightPathItem>> result = Results.Success(straightPath);
var mockQuery = new Mock<DtNavMeshQuery>(It.IsAny<DtNavMesh>());
mockQuery.Setup(q => q.FindStraightPath(
It.IsAny<RcVec3f>(),
It.IsAny<RcVec3f>(),
It.IsAny<List<long>>(),
It.IsAny<int>(),
It.IsAny<int>())
).Returns(result);
List<StraightPathItem> path = corridor.FindCorners(int.MaxValue, mockQuery.Object, filter);
It.IsAny<RcVec3f>(),
It.IsAny<RcVec3f>(),
It.IsAny<List<long>>(),
ref It.Ref<List<StraightPathItem>>.IsAny,
It.IsAny<int>(),
It.IsAny<int>())
)
.Callback((RcVec3f startPos, RcVec3f endPos, List<long> path,
ref List<StraightPathItem> refStraightPath, int maxStraightPath, int options) =>
{
refStraightPath = straightPath;
})
.Returns(() => DtStatus.DT_SUCCSESS);
var path = new List<StraightPathItem>();
corridor.FindCorners(ref path, int.MaxValue, mockQuery.Object, filter);
Assert.That(path.Count, Is.EqualTo(4));
Assert.That(path, Is.EqualTo(straightPath));
}
@ -67,19 +75,25 @@ public class PathCorridorTest
straightPath.Add(new StraightPathItem(RcVec3f.Of(11f, 21, 32f), 0, 0));
straightPath.Add(new StraightPathItem(RcVec3f.Of(12f, 22, 33f), DtNavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION, 0)); // offmesh
straightPath.Add(new StraightPathItem(RcVec3f.Of(11f, 21, 32f), DtNavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION, 0)); // offmesh
Result<List<StraightPathItem>> result = Results.Success(straightPath);
var mockQuery = new Mock<DtNavMeshQuery>(It.IsAny<DtNavMesh>());
var s = mockQuery.Setup(q => q.FindStraightPath(
It.IsAny<RcVec3f>(),
It.IsAny<RcVec3f>(),
It.IsAny<List<long>>(),
It.IsAny<int>(),
It.IsAny<int>())
).Returns(result);
mockQuery.Setup(q => q.FindStraightPath(
It.IsAny<RcVec3f>(),
It.IsAny<RcVec3f>(),
It.IsAny<List<long>>(),
ref It.Ref<List<StraightPathItem>>.IsAny,
It.IsAny<int>(),
It.IsAny<int>())
).Callback((RcVec3f startPos, RcVec3f endPos, List<long> path,
ref List<StraightPathItem> refStraightPath, int maxStraightPath, int options) =>
{
refStraightPath = straightPath;
})
.Returns(() => DtStatus.DT_SUCCSESS);
List<StraightPathItem> path = corridor.FindCorners(int.MaxValue, mockQuery.Object, filter);
var path = new List<StraightPathItem>();
corridor.FindCorners(ref path, int.MaxValue, mockQuery.Object, filter);
Assert.That(path.Count, Is.EqualTo(2));
Assert.That(path, Is.EqualTo(new List<StraightPathItem> { straightPath[2], straightPath[3] }));
}
}
}

5
test/DotRecast.Detour.Test/FindPathTest.cs
View File

@ -188,9 +188,8 @@ public class FindPathTest : AbstractDetourTest
var startPos = startPoss[i];
var endPos = endPoss[i];
Result<List<long>> path = query.FindPath(startRef, endRef, startPos, endPos, filter);
Result<List<StraightPathItem>> result = query.FindStraightPath(startPos, endPos, path.result,
int.MaxValue, 0);
List<StraightPathItem> straightPath = result.result;
var straightPath = new List<StraightPathItem>();
query.FindStraightPath(startPos, endPos, path.result, ref straightPath, int.MaxValue, 0);
Assert.That(straightPath.Count, Is.EqualTo(STRAIGHT_PATHS[i].Length));
for (int j = 0; j < STRAIGHT_PATHS[i].Length; j++)
{

6
test/DotRecast.Detour.TileCache.Test/TileCacheFindPathTest.cs
View File

@ -55,7 +55,9 @@ public class TileCacheFindPathTest : AbstractTileCacheTest
Result<List<long>> path = query.FindPath(startRef, endRef, startPos, endPos, filter);
int maxStraightPath = 256;
int options = 0;
Result<List<StraightPathItem>> pathStr = query.FindStraightPath(startPos, endPos, path.result, maxStraightPath, options);
Assert.That(pathStr.result.Count, Is.EqualTo(8));
var pathStr = new List<StraightPathItem>();
query.FindStraightPath(startPos, endPos, path.result, ref pathStr, maxStraightPath, options);
Assert.That(pathStr.Count, Is.EqualTo(8));
}
}