a.samsonkin
/
DotRecastNetSim
forked from bit/DotRecastNetSim
1
0
Fork 0
Code Pull Requests Activity

change ClosestPointOnPolyBoundary

This commit is contained in:
ikpil 2023-06-18 11:43:01 +09:00
parent 0971bf2c3c
commit 8a5614eca4
2 changed files with 30 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -515,7 +515,7 @@ namespace DotRecast.Detour.Crowd
}
}
public void TrimInvalidPath(long safeRef, float[] safePos, DtNavMeshQuery navquery, IDtQueryFilter filter)
public bool TrimInvalidPath(long safeRef, float[] safePos, DtNavMeshQuery navquery, IDtQueryFilter filter)
{
// Keep valid path as far as possible.
int n = 0;
@ -524,7 +524,12 @@ namespace DotRecast.Detour.Crowd
n++;
}
if (n == 0)
if (m_path.Count == n)
{
// All valid, no need to fix.
return true;
}
else if (n == 0)
{
// The first polyref is bad, use current safe values.
m_pos.Set(safePos);
@ -533,16 +538,13 @@ namespace DotRecast.Detour.Crowd
}
else if (n < m_path.Count)
{
m_path = m_path.GetRange(0, n);
// The path is partially usable.
m_path = m_path.GetRange(0, n);
}
// Clamp target pos to last poly
var result = navquery.ClosestPointOnPolyBoundary(m_path[m_path.Count - 1], m_target);
if (result.Succeeded())
{
m_target = result.result;
}
navquery.ClosestPointOnPolyBoundary(m_path[m_path.Count - 1], m_target, out m_target);
return true;
}
/**

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

@ -461,22 +461,18 @@ namespace DotRecast.Detour
/// @param[in] pos The position to check. [(x, y, z)]
/// @param[out] closest The closest point. [(x, y, z)]
/// @returns The status flags for the query.
public Result<RcVec3f> ClosestPointOnPolyBoundary(long refs, RcVec3f pos)
public DtStatus ClosestPointOnPolyBoundary(long refs, RcVec3f pos, out RcVec3f closest)
{
closest = pos;
var status = m_nav.GetTileAndPolyByRef(refs, out var tile, out var poly);
if (status.Failed())
{
return Results.Of<RcVec3f>(status, "");
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
if (tile == null)
if (tile == null || !RcVec3f.IsFinite(pos))
{
return Results.InvalidParam<RcVec3f>("Invalid tile");
}
if (!RcVec3f.IsFinite(pos))
{
return Results.InvalidParam<RcVec3f>();
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
// Collect vertices.
@ -489,7 +485,6 @@ namespace DotRecast.Detour
Array.Copy(tile.data.verts, poly.verts[i] * 3, verts, i * 3, 3);
}
RcVec3f closest;
if (DetourCommon.DistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
{
closest = pos;
@ -513,7 +508,7 @@ namespace DotRecast.Detour
closest = RcVec3f.Lerp(verts, va, vb, edget[imin]);
}
return Results.Success(closest);
return DtStatus.DT_SUCCSESS;
}
/// @par
@ -1545,20 +1540,18 @@ namespace DotRecast.Detour
}
// TODO: Should this be callers responsibility?
Result<RcVec3f> closestStartPosRes = ClosestPointOnPolyBoundary(path[0], startPos);
var closestStartPosRes = ClosestPointOnPolyBoundary(path[0], startPos, out var closestStartPos);
if (closestStartPosRes.Failed())
{
return Results.InvalidParam<List<StraightPathItem>>("Cannot find start position");
}
var closestStartPos = closestStartPosRes.result;
var closestEndPosRes = ClosestPointOnPolyBoundary(path[path.Count - 1], endPos);
var closestEndPosRes = ClosestPointOnPolyBoundary(path[path.Count - 1], endPos, out var closestEndPos);
if (closestEndPosRes.Failed())
{
return Results.InvalidParam<List<StraightPathItem>>("Cannot find end position");
}
var closestEndPos = closestEndPosRes.result;
// Add start point.
DtStatus stat = AppendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0], straightPath, maxStraightPath);
if (!stat.InProgress())
@ -1592,18 +1585,17 @@ namespace DotRecast.Detour
int fromType; // // fromType is ignored.
// Next portal.
var portalPoints = GetPortalPoints(path[i], path[i + 1], out left, out right, out fromType, out toType);
if (portalPoints.Failed())
var ppStatus = GetPortalPoints(path[i], path[i + 1], out left, out right, out fromType, out toType);
if (ppStatus.Failed())
{
// Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
// Clamp the end point to path[i], and return the path so far.
closestEndPosRes = ClosestPointOnPolyBoundary(path[i], endPos);
if (closestEndPosRes.Failed())
var cpStatus = ClosestPointOnPolyBoundary(path[i], endPos, out closestEndPos);
if (cpStatus.Failed())
{
return Results.InvalidParam<List<StraightPathItem>>();
}
closestEndPos = closestEndPosRes.result;
// Append portals along the current straight path segment.
if ((options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
{